#include "coord.h"
#include "functions.h"
#include "FxEngine.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(FE_point point)
{
	static FE_position temp;
	temp.x = point.real.x - FE_user.x;
	temp.y = point.real.y - FE_user.y;
	temp.z = point.real.z - FE_user.z;
	point.translated.x = matrice[0][0]*temp.x + matrice[0][1]*temp.y + matrice[0][2]*temp.z;
	point.translated.z = matrice[1][0]*temp.x + matrice[1][1]*temp.y + matrice[1][2]*temp.z;
	point.translated.y = matrice[2][0]*temp.x + matrice[2][1]*temp.y + matrice[2][2]*temp.z;

	point.translated.x*=100;
	point.translated.y*=100;

	point.translated.x/=point.translated.z;
	point.translated.y/=point.translated.z;
}

void FE_set_matrice(void)
{
	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, BD=B*D;

	matrice[0][0]=C*E;
	matrice[0][1]=-C*F;
	matrice[0][2]=D;
	//matrice[0][3]=0;

	matrice[1][0]=BD*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;
}