2019-07-21 20:14:54 +02:00
|
|
|
#include "coord.h"
|
|
|
|
#include "FxEngine.h"
|
|
|
|
|
2019-07-22 16:28:59 +02:00
|
|
|
|
|
|
|
/* Cosinus and sinus implementation */
|
|
|
|
|
|
|
|
/// Math values
|
|
|
|
const double pi=3.141592653589793238462643383279;
|
|
|
|
const double pi2=pi*2;
|
|
|
|
const double pi_sur2=pi/2;
|
|
|
|
|
|
|
|
/// Internal Use
|
|
|
|
// cos function with 0<a<pi/2
|
2019-07-24 15:33:26 +02:00
|
|
|
static const int precision=15;
|
2019-07-22 16:28:59 +02:00
|
|
|
static double reducted_cos(const double a)
|
|
|
|
{
|
|
|
|
double u=1.0; const double a2=a*a;
|
|
|
|
// precision%2=1 && precision>1
|
|
|
|
for(int p=precision;p>=1;p-=2)
|
|
|
|
u=1-a2/(p*p+p)*u;
|
|
|
|
return u;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return a with -pi<=a<pi
|
2019-07-23 17:13:35 +02:00
|
|
|
double FE_modulo_2pi(double a)
|
2019-07-22 16:28:59 +02:00
|
|
|
{
|
|
|
|
while (a<=-pi) a+=pi2;
|
|
|
|
while (a>pi) a-=pi2;
|
|
|
|
return a;
|
|
|
|
}
|
|
|
|
|
2019-07-24 15:33:26 +02:00
|
|
|
static double cos_recursive(double angle)
|
|
|
|
{
|
|
|
|
if (angle<0)
|
|
|
|
return cos_recursive(-angle);
|
|
|
|
if (angle>=pi_sur2)
|
|
|
|
return -reducted_cos(angle-pi);
|
|
|
|
return reducted_cos(angle); // OK
|
|
|
|
}
|
2019-07-23 18:30:27 +02:00
|
|
|
|
2019-07-22 16:28:59 +02:00
|
|
|
/// External functions
|
|
|
|
// Official FE_cos function
|
|
|
|
double FE_cos(double angle)
|
|
|
|
{
|
2019-07-23 17:13:35 +02:00
|
|
|
angle=FE_modulo_2pi(angle);
|
2019-07-24 15:33:26 +02:00
|
|
|
return cos_recursive(angle);
|
2019-07-22 16:28:59 +02:00
|
|
|
}
|
2019-07-24 15:33:26 +02:00
|
|
|
|
2019-07-22 16:28:59 +02:00
|
|
|
// Official FE_sin function
|
|
|
|
double FE_sin(double angle)
|
|
|
|
{return FE_cos(angle-pi_sur2);}
|
|
|
|
|
|
|
|
|
2019-07-25 13:23:41 +02:00
|
|
|
#define sgn(x) (x>=0?x:-x)
|
2019-07-22 16:28:59 +02:00
|
|
|
|
2019-08-02 10:18:29 +02:00
|
|
|
static double matrice[3][3]=
|
2019-07-21 20:14:54 +02:00
|
|
|
{
|
2019-08-02 10:18:29 +02:00
|
|
|
{0,0,0},
|
|
|
|
{0,0,0},
|
|
|
|
{0,0,0}
|
2019-07-21 20:14:54 +02:00
|
|
|
};
|
|
|
|
|
2019-07-23 18:30:27 +02:00
|
|
|
void FE_calc(FE_point* point)
|
2019-07-21 20:14:54 +02:00
|
|
|
{
|
2019-07-22 16:28:59 +02:00
|
|
|
static FE_position temp;
|
2019-07-23 18:30:27 +02:00
|
|
|
temp.x = point->real.x - FE_user.x;
|
|
|
|
temp.y = point->real.y - FE_user.y;
|
|
|
|
temp.z = point->real.z - FE_user.z;
|
|
|
|
|
2019-08-02 10:18:29 +02:00
|
|
|
point->rotated.x = (double)(matrice[0][0]*(double)temp.x + matrice[0][1]*(double)temp.y + matrice[0][2]*(double)temp.z);
|
|
|
|
point->rotated.z = (double)(matrice[1][0]*(double)temp.x + matrice[1][1]*(double)temp.y + matrice[1][2]*(double)temp.z);
|
|
|
|
point->rotated.y = (double)(matrice[2][0]*(double)temp.x + matrice[2][1]*(double)temp.y + matrice[2][2]*(double)temp.z);
|
2019-07-23 18:30:27 +02:00
|
|
|
|
|
|
|
//point->translated.x*=10;
|
|
|
|
//point->translated.y*=10;
|
2019-08-02 10:18:29 +02:00
|
|
|
point->rotated.x*=64;
|
|
|
|
point->rotated.y*=64;
|
|
|
|
point->translated.z=point->rotated.z;
|
2019-07-25 13:23:41 +02:00
|
|
|
if (point->translated.z>0)
|
|
|
|
{
|
2019-08-02 10:18:29 +02:00
|
|
|
point->translated.x=point->rotated.x/point->translated.z;
|
|
|
|
point->translated.y=point->rotated.y/point->translated.z;
|
2019-07-25 13:23:41 +02:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2019-08-02 10:18:29 +02:00
|
|
|
point->translated.x=point->rotated.x*10000*sgn(point->translated.z);
|
|
|
|
point->translated.y=point->rotated.y*10000*sgn(point->translated.z);
|
2019-07-25 13:23:41 +02:00
|
|
|
}
|
2019-08-02 10:18:29 +02:00
|
|
|
(point->translated.x*1000)/point->translated.z;
|
|
|
|
(point->translated.y*1000)/point->translated.z;
|
2019-07-22 16:28:59 +02:00
|
|
|
|
2019-07-24 15:33:26 +02:00
|
|
|
point->translated.x+=63;
|
|
|
|
point->translated.y+=31;
|
2019-07-21 20:14:54 +02:00
|
|
|
}
|
|
|
|
|
2019-08-02 10:18:29 +02:00
|
|
|
|
|
|
|
// FIABLE
|
2019-07-21 20:14:54 +02:00
|
|
|
void FE_set_matrice(void)
|
|
|
|
{
|
2019-07-24 15:33:26 +02:00
|
|
|
const double A=FE_cos(FE_dv), B=FE_sin(FE_dv);
|
|
|
|
const double C=FE_cos(FE_roulis), D=FE_sin(FE_roulis);
|
|
|
|
const double E=FE_cos(FE_dh), F=FE_sin(FE_dh);
|
2019-07-22 16:28:59 +02:00
|
|
|
|
|
|
|
const double AD=A*D, BD=B*D;
|
|
|
|
|
|
|
|
matrice[0][0]=C*E;
|
|
|
|
matrice[0][1]=-C*F;
|
|
|
|
matrice[0][2]=D;
|
|
|
|
|
|
|
|
matrice[1][0]=BD*E+A*F;
|
|
|
|
matrice[1][1]=-BD*F+A*E;
|
|
|
|
matrice[1][2]=-B*C;
|
|
|
|
|
|
|
|
matrice[2][0]=-AD*E+B*F;
|
|
|
|
matrice[2][1]=AD*F+B*E;
|
|
|
|
matrice[2][2]=A*C;
|
|
|
|
|
|
|
|
}
|