From 2c6da465562893cb8512a25614fb46b02e6d59eb Mon Sep 17 00:00:00 2001
From: util1 <milan.garnier@orange.fr>
Date: Sat, 20 Jul 2019 17:52:00 +0200
Subject: [PATCH] add rotation matrices

---
 src/FxEngine/FxEngine.c   |  1 +
 src/FxEngine/math/coord.c | 36 ++++++++++++++++++++++++++++++++++--
 src/FxEngine/math/coord.h |  2 +-
 3 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/src/FxEngine/FxEngine.c b/src/FxEngine/FxEngine.c
index 6ada3f9..f112778 100644
--- a/src/FxEngine/FxEngine.c
+++ b/src/FxEngine/FxEngine.c
@@ -32,5 +32,6 @@ int FE_get_fps()
 static void FE_move() // call at each frame
 {
 	// TODO with math functions, not implemented yet !
+	FE_set_matrice(FE_dh,FE_dv,FR_roulis);
 	// TODO insert physical engine 
 }
diff --git a/src/FxEngine/math/coord.c b/src/FxEngine/math/coord.c
index a3b16e0..216272a 100644
--- a/src/FxEngine/math/coord.c
+++ b/src/FxEngine/math/coord.c
@@ -2,13 +2,45 @@
 
 #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()
+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;
 }	
\ No newline at end of file
diff --git a/src/FxEngine/math/coord.h b/src/FxEngine/math/coord.h
index 99fd5bf..ee07032 100644
--- a/src/FxEngine/math/coord.h
+++ b/src/FxEngine/math/coord.h
@@ -15,7 +15,7 @@ struct FE_point
 };
 
 void FE_calc(const FE_coord& coord);
-void FE_set_matrice()
+void FE_set_matrice(float dh, float dv, float roulis);