46 lines
727 B
C
46 lines
727 B
C
#include "coord.h"
|
|
|
|
#include "angle.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()
|
|
{
|
|
transformed=original-real;
|
|
// rotate
|
|
}
|
|
|
|
void FE_set_matrice(float dh, float dv, float roulis)
|
|
{
|
|
const float A=cos(dh), B=sin(dh);
|
|
const float C=cos(dv), D=sin(dv);
|
|
const float E=cos(roulis), F=sin(roulis);
|
|
|
|
const float AD=A*D, BC=B*C;
|
|
|
|
matrice[0][0]=C*E;
|
|
matrice[1][0]=-C*F;
|
|
matrice[2][0]=D;
|
|
//matrice[3][0]=0;
|
|
|
|
matrice[0][1]=BC*E+A*F;
|
|
matrice[1][1]=-BD*F+A*E;
|
|
matrice[2][1]=-B*C;
|
|
//matrice[3][1]=0;
|
|
|
|
matrice[0][2]=-AD*E+B*F;
|
|
matrice[1][2]=AD*F+B*E;
|
|
matrice[2][2]=A*C;
|
|
//matrice[3][2]=0;
|
|
|
|
//matrice[0][3]=0;
|
|
//matrice[1][3]=0;
|
|
//matrice[2][3]=0;
|
|
//matrice[3][3]=1;
|
|
}
|