52 lines
1.1 KiB
C
52 lines
1.1 KiB
C
#include "coord.h"
|
|
#include "functions.h"
|
|
|
|
static float matrice[4][4]=
|
|
{
|
|
{0,0,0,0},
|
|
{0,0,0,0},
|
|
{0,0,0,0},
|
|
{0,0,0,1}
|
|
};
|
|
|
|
void FE_calc(const FE_point& point)
|
|
{
|
|
static FE_position temp
|
|
{
|
|
point.real.x - FE_user.x,
|
|
point.real.y - FE_user.y,
|
|
point.real.z - FE_user.z
|
|
};
|
|
point.transformed.x = matrice[0][0]*temp.x + matrice[0][1]*temp.y + matrice[0][2]*temp.z;
|
|
point.transformed.y = matrice[1][0]*temp.x + matrice[1][1]*temp.y + matrice[1][2]*temp.z;
|
|
point.transformed.z = matrice[2][0]*temp.x + matrice[2][1]*temp.y + matrice[2][2]*temp.z;
|
|
}
|
|
|
|
void FE_set_matrice()
|
|
{
|
|
const double A=FE_cos(FE_dh), B=FE_sin(FE_dh);
|
|
const double C=FE_cos(FE_dv), D=FE_sin(FE_dv);
|
|
const double E=FE_cos(FE_roulis), F=FE_sin(FE_roulis);
|
|
|
|
const double AD=A*D, BC=B*C;
|
|
|
|
matrice[0][0]=C*E;
|
|
matrice[0][1]=-C*F;
|
|
matrice[0][2]=D;
|
|
//matrice[0][3]=0;
|
|
|
|
matrice[1][0]=BC*E+A*F;
|
|
matrice[1][1]=-BD*F+A*E;
|
|
matrice[1][2]=-B*C;
|
|
//matrice[1][3]=0;
|
|
|
|
matrice[2][0]=-AD*E+B*F;
|
|
matrice[2][1]=AD*F+B*E;
|
|
matrice[2][2]=A*C;
|
|
//matrice[2][2]=0;
|
|
|
|
//matrice[3][0]=0;
|
|
//matrice[3][1]=0;
|
|
//matrice[3][2]=0;
|
|
//matrice[3][3]=1;
|
|
}
|