#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;
}