Changed centering algorithm to use proper matrix algebra.

Cleaned up T6DOF structure.
Updated vc_9 project files.


git-svn-id: svn+ssh://svn.code.sf.net/p/facetracknoir/code@169 19e81ba0-9b1a-49c3-bd6c-561e1906d5fb

26 files changed, 329 insertions, 183 deletions

diff --git a/FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2_vc9.vcproj b/FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2_vc9.vcproj
index d1f0fb8c..d692d8cd 100644
--- a/FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2_vc9.vcproj
+++ b/FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2_vc9.vcproj
@@ -173,6 +173,10 @@
 				RelativePath=".\ftnoir_filter_ewma2_dialog.cpp"

 				>

 			</File>

+			<File

+				RelativePath=".\ftnoir_filter_ewma_dll.cpp"

+				>

+			</File>

 		</Filter>

 		<Filter

 			Name="Header Files"

diff --git a/FTNoIR_Protocol_Base/ftnoir_protocol_base.h b/FTNoIR_Protocol_Base/ftnoir_protocol_base.h
index d8ab755f..76f5ecc7 100644
--- a/FTNoIR_Protocol_Base/ftnoir_protocol_base.h
+++ b/FTNoIR_Protocol_Base/ftnoir_protocol_base.h
@@ -58,7 +58,7 @@ struct IProtocol
 	virtual ~IProtocol() {}

 	virtual void Initialize() = 0;

 	virtual bool checkServerInstallationOK ( HANDLE handle ) = 0;

-	virtual void sendHeadposeToGame( T6DOF *headpose ) = 0;

+	virtual void sendHeadposeToGame( THeadPoseData *headpose ) = 0;

 	virtual void getNameFromGame( char *dest ) = 0;				// Take care dest can handle up to 100 chars...

 

 	virtual void getFullName(QString *strToBeFilled) = 0;

diff --git a/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.cpp b/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.cpp
index a2a9505a..9234c4d4 100644
--- a/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.cpp
+++ b/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.cpp
@@ -97,7 +97,7 @@ void FTNoIR_Protocol_FG::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_FG::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_FG::sendHeadposeToGame( THeadPoseData *headpose ) {

 int no_bytes;

 QHostAddress sender;

 quint16 senderPort;

@@ -109,12 +109,12 @@ char data[100];
 	//

 	// Copy the Raw measurements directly to the client.

 	//

-	FlightData.x = headpose->position.x;

-	FlightData.y = headpose->position.y;

-	FlightData.z = headpose->position.z;

-	FlightData.p = headpose->position.pitch;

-	FlightData.h = headpose->position.yaw;

-	FlightData.r = headpose->position.roll;

+	FlightData.x = headpose->x;

+	FlightData.y = headpose->y;

+	FlightData.z = headpose->z;

+	FlightData.p = headpose->pitch;

+	FlightData.h = headpose->yaw;

+	FlightData.r = headpose->roll;

 	FlightData.status = fg_cmd;

 

 	//

diff --git a/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.h b/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.h
index 216095bf..ff3be6bd 100644
--- a/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.h
+++ b/FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.h
@@ -49,7 +49,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );						// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("FlightGear"); };

diff --git a/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.cpp b/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.cpp
index b4d6bebe..c88e7ec0 100644
--- a/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.cpp
+++ b/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.cpp
@@ -103,7 +103,7 @@ void FTNoIR_Protocol_FSUIPC::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_FSUIPC::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_FSUIPC::sendHeadposeToGame( THeadPoseData *headpose ) {

 DWORD result;

 TFSState pitch;

 TFSState yaw;

@@ -120,13 +120,13 @@ float virtRotZ;
 

 //	qDebug() << "FSUIPCServer::run() says: started!";

 

-	virtRotX = -1.0f * headpose->position.pitch;				// degrees

-	virtRotY = headpose->position.yaw;

-	virtRotZ = headpose->position.roll;

+	virtRotX = -1.0f * headpose->pitch;				// degrees

+	virtRotY = headpose->yaw;

+	virtRotZ = headpose->roll;

 

 	virtPosX = 0.0f;											// cm, X and Y are not working for FS2002/2004!

 	virtPosY = 0.0f;

-	virtPosZ = -1.0f * headpose->position.z;

+	virtPosZ = -1.0f * headpose->z;

 

 	//

 	// Init. the FSUIPC offsets (derived from Free-track...)

diff --git a/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.h b/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.h
index 196a107a..15ef9209 100644
--- a/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.h
+++ b/FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.h
@@ -65,7 +65,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );					// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("FS2002/FS2004"); };

diff --git a/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.cpp b/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.cpp
index 43326d6a..f4018c2e 100644
--- a/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.cpp
+++ b/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.cpp
@@ -78,7 +78,7 @@ void FTNoIR_Protocol_FT::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_FT::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_FT::sendHeadposeToGame( THeadPoseData *headpose ) {

 float virtPosX;

 float virtPosY;

 float virtPosZ;

@@ -101,19 +101,19 @@ PDWORD_PTR MsgResult = 0;
 	//

 	// Scale the Raw measurements to the client measurements.

 	//

-	headRotX = getRadsFromDegrees(headpose->position.pitch);

-	headRotY = getRadsFromDegrees(headpose->position.yaw);

-	headRotZ = getRadsFromDegrees(headpose->position.roll);

-	headPosX = headpose->position.x * 10;

-	headPosY = headpose->position.y * 10;

-	headPosZ = headpose->position.z * 10;

-

-	virtRotX = getRadsFromDegrees(headpose->position.pitch);

-	virtRotY = getRadsFromDegrees(headpose->position.yaw);

-	virtRotZ = getRadsFromDegrees(headpose->position.roll);

-	virtPosX = headpose->position.x * 10;

-	virtPosY = headpose->position.y * 10;

-	virtPosZ = headpose->position.z * 10;

+	headRotX = getRadsFromDegrees(headpose->pitch);

+	headRotY = getRadsFromDegrees(headpose->yaw);

+	headRotZ = getRadsFromDegrees(headpose->roll);

+	headPosX = headpose->x * 10;

+	headPosY = headpose->y * 10;

+	headPosZ = headpose->z * 10;

+

+	virtRotX = getRadsFromDegrees(headpose->pitch);

+	virtRotY = getRadsFromDegrees(headpose->yaw);

+	virtRotZ = getRadsFromDegrees(headpose->roll);

+	virtPosX = headpose->x * 10;

+	virtPosY = headpose->y * 10;

+	virtPosZ = headpose->z * 10;

 

 	//

 	// Check if the pointer is OK and wait for the Mutex.

diff --git a/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.h b/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.h
index 9d0a5033..736b8600 100644
--- a/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.h
+++ b/FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.h
@@ -52,7 +52,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );					// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("FreeTrack"); };

diff --git a/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.cpp b/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.cpp
index 399fc012..0c8b2fbf 100644
--- a/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.cpp
+++ b/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.cpp
@@ -124,7 +124,7 @@ void FTNoIR_Protocol_FTIR::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_FTIR::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_FTIR::sendHeadposeToGame( THeadPoseData *headpose ) {

 float virtPosX;

 float virtPosY;

 float virtPosZ;

@@ -138,13 +138,13 @@ TRACKIRDATA localdata;
 	//

 	// Copy the Raw measurements directly to the client.

 	//

-	virtRotX = scale2AnalogLimits (headpose->position.pitch, -180.0f, 180.0f);

-	virtRotY = scale2AnalogLimits (headpose->position.yaw, -180.0f, 180.0f);

-	virtRotZ = scale2AnalogLimits (headpose->position.roll, -180.0f, 180.0f);

+	virtRotX = scale2AnalogLimits (headpose->pitch, -180.0f, 180.0f);

+	virtRotY = scale2AnalogLimits (headpose->yaw, -180.0f, 180.0f);

+	virtRotZ = scale2AnalogLimits (headpose->roll, -180.0f, 180.0f);

 

-	virtPosX = scale2AnalogLimits (headpose->position.x * 10.0f, -500.0f, 500.0f);

-	virtPosY = scale2AnalogLimits (headpose->position.y * 10.0f, -500.0f, 500.0f);

-	virtPosZ = scale2AnalogLimits (headpose->position.z * 10.0f, -500.0f, 500.0f);

+	virtPosX = scale2AnalogLimits (headpose->x * 10.0f, -500.0f, 500.0f);

+	virtPosY = scale2AnalogLimits (headpose->y * 10.0f, -500.0f, 500.0f);

+	virtPosZ = scale2AnalogLimits (headpose->z * 10.0f, -500.0f, 500.0f);

 

 	//

 	// Check if the pointer is OK and wait for the Mutex.

diff --git a/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.h b/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.h
index 29b16e4b..7478f405 100644
--- a/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.h
+++ b/FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.h
@@ -56,7 +56,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );					// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("fake TrackIR"); };

diff --git a/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.cpp b/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.cpp
index eb51fb76..55888484 100755
--- a/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.cpp
+++ b/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.cpp
@@ -86,7 +86,7 @@ void FTNoIR_Protocol_FTN::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_FTN::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_FTN::sendHeadposeToGame( THeadPoseData *headpose ) {

 int no_bytes;

 QHostAddress sender;

 quint16 senderPort;

@@ -95,7 +95,7 @@ quint16 senderPort;
 	// Copy the Raw measurements directly to the client.

 	//

 	frame_counter += 1;

-	TestData = headpose->position;

+	TestData = *headpose;

 	TestData.frame_number = frame_counter;

 

 	//

diff --git a/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.h b/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.h
index 8f0aa478..b460563c 100644
--- a/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.h
+++ b/FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.h
@@ -49,7 +49,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );						// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("FaceTrackNoIR"); };

diff --git a/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.cpp b/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.cpp
index 98371eef..e431348d 100644
--- a/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.cpp
+++ b/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.cpp
@@ -105,7 +105,7 @@ void FTNoIR_Protocol_MOUSE::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_MOUSE::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_MOUSE::sendHeadposeToGame( THeadPoseData *headpose ) {

 float fMouse_X;							// The actual value

 float fMouse_Y;

 float fMouse_Wheel;

@@ -118,27 +118,27 @@ float fMouse_Wheel;
 	//

 	switch (Mouse_X) {

 		case FTN_PITCH:

-			fMouse_X = headpose->position.pitch;

+			fMouse_X = headpose->pitch;

 			break;

 

 		case FTN_YAW:

-			fMouse_X = headpose->position.yaw;

+			fMouse_X = headpose->yaw;

 			break;

 

 		case FTN_ROLL:

-			fMouse_X = headpose->position.roll;

+			fMouse_X = headpose->roll;

 			break;

 

 		case FTN_X:

-			fMouse_X = headpose->position.x * 3.0f;

+			fMouse_X = headpose->x * 3.0f;

 			break;

 

 		case FTN_Y:

-			fMouse_X = headpose->position.y * 3.0f;

+			fMouse_X = headpose->y * 3.0f;

 			break;

 

 		case FTN_Z:

-			fMouse_X = headpose->position.z * 3.0f;

+			fMouse_X = headpose->z * 3.0f;

 			break;

 

 		default:

@@ -152,27 +152,27 @@ float fMouse_Wheel;
 	//

 	switch (Mouse_Y) {

 		case FTN_PITCH:

-			fMouse_Y = headpose->position.pitch;

+			fMouse_Y = headpose->pitch;

 			break;

 

 		case FTN_YAW:

-			fMouse_Y = headpose->position.yaw;

+			fMouse_Y = headpose->yaw;

 			break;

 

 		case FTN_ROLL:

-			fMouse_Y = headpose->position.roll;

+			fMouse_Y = headpose->roll;

 			break;

 

 		case FTN_X:

-			fMouse_Y = headpose->position.x * 3.0f;

+			fMouse_Y = headpose->x * 3.0f;

 			break;

 

 		case FTN_Y:

-			fMouse_Y = headpose->position.y * 3.0f;

+			fMouse_Y = headpose->y * 3.0f;

 			break;

 

 		case FTN_Z:

-			fMouse_Y = headpose->position.z * 3.0f;

+			fMouse_Y = headpose->z * 3.0f;

 			break;

 

 		default:

@@ -186,27 +186,27 @@ float fMouse_Wheel;
 	//

 	switch (Mouse_Wheel) {

 		case FTN_PITCH:

-			fMouse_Wheel = headpose->position.pitch;

+			fMouse_Wheel = headpose->pitch;

 			break;

 

 		case FTN_YAW:

-			fMouse_Wheel = headpose->position.yaw;

+			fMouse_Wheel = headpose->yaw;

 			break;

 

 		case FTN_ROLL:

-			fMouse_Wheel = headpose->position.roll;

+			fMouse_Wheel = headpose->roll;

 			break;

 

 		case FTN_X:

-			fMouse_Wheel = headpose->position.x * 3.0f;

+			fMouse_Wheel = headpose->x * 3.0f;

 			break;

 

 		case FTN_Y:

-			fMouse_Wheel = headpose->position.y * 3.0f;

+			fMouse_Wheel = headpose->y * 3.0f;

 			break;

 

 		case FTN_Z:

-			fMouse_Wheel = headpose->position.z * 3.0f;

+			fMouse_Wheel = headpose->z * 3.0f;

 			break;

 

 		default:

diff --git a/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.h b/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.h
index 723f11b1..c2c16abb 100644
--- a/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.h
+++ b/FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.h
@@ -70,7 +70,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );					// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("Mouse Look"); };

diff --git a/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.cpp b/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.cpp
index bfecfeb3..f533fdf3 100644
--- a/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.cpp
+++ b/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.cpp
@@ -128,7 +128,7 @@ void FTNoIR_Protocol_PPJOY::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_PPJOY::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_PPJOY::sendHeadposeToGame( THeadPoseData *headpose ) {

 float virtPosX;

 float virtPosY;

 float virtPosZ;

@@ -140,13 +140,13 @@ float virtRotZ;
 	//

 	// Copy the Raw measurements.

 	//

-	virtRotX = headpose->position.pitch;

-	virtRotY = headpose->position.yaw;

-	virtRotZ = headpose->position.roll;

+	virtRotX = headpose->pitch;

+	virtRotY = headpose->yaw;

+	virtRotZ = headpose->roll;

 

-	virtPosX = headpose->position.x;

-	virtPosY = headpose->position.y;

-	virtPosZ = headpose->position.z;

+	virtPosX = headpose->x;

+	virtPosY = headpose->y;

+	virtPosZ = headpose->z;

 

 	/* Initialise the IOCTL data structure */

 	JoyState.Signature= JOYSTICK_STATE_V1;

diff --git a/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.h b/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.h
index 97f064db..266fd877 100644
--- a/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.h
+++ b/FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.h
@@ -69,7 +69,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );						// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("PPJoy Virtual Joystick"); };

diff --git a/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.cpp b/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.cpp
index 1c3290c6..6c0cf42f 100644
--- a/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.cpp
+++ b/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.cpp
@@ -96,16 +96,16 @@ void FTNoIR_Protocol_SC::loadSettings() {
 //

 // Update Headpose in Game.

 //

-void FTNoIR_Protocol_SC::sendHeadposeToGame( T6DOF *headpose ) {

+void FTNoIR_Protocol_SC::sendHeadposeToGame( THeadPoseData *headpose ) {

 

 

-	virtSCRotX = -1.0f * headpose->position.pitch;					// degrees

-	virtSCRotY = -1.0f * headpose->position.yaw;

-	virtSCRotZ = headpose->position.roll;

+	virtSCRotX = -1.0f * headpose->pitch;					// degrees

+	virtSCRotY = -1.0f * headpose->yaw;

+	virtSCRotZ = headpose->roll;

 

-	virtSCPosX = headpose->position.x/100.f;						// cm to meters

-	virtSCPosY = headpose->position.y/100.f;

-	virtSCPosZ = -1.0f * headpose->position.z/100.f;

+	virtSCPosX = headpose->x/100.f;						// cm to meters

+	virtSCPosY = headpose->y/100.f;

+	virtSCPosZ = -1.0f * headpose->z/100.f;

 

 	//

 	// It's only useful to send data, if the connection was made.

diff --git a/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.h b/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.h
index 33e08838..cf8c3e5c 100644
--- a/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.h
+++ b/FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.h
@@ -87,7 +87,7 @@ public:
     void Initialize();

 

 	bool checkServerInstallationOK( HANDLE handle );

-	void sendHeadposeToGame( T6DOF *headpose );

+	void sendHeadposeToGame( THeadPoseData *headpose );

 	void getNameFromGame( char *dest );					// Take care dest can handle up to 100 chars...

 

 	void getFullName(QString *strToBeFilled) { *strToBeFilled = QString("FSX SimConnect"); };

diff --git a/FTNoIR_Tracker_Base/ftnoir_tracker_types.h b/FTNoIR_Tracker_Base/ftnoir_tracker_types.h
index 93007b54..ace7a6e3 100644
--- a/FTNoIR_Tracker_Base/ftnoir_tracker_types.h
+++ b/FTNoIR_Tracker_Base/ftnoir_tracker_types.h
@@ -24,6 +24,7 @@
 *********************************************************************************/

 /*

 	Modifications (last one on top):

+		20120924 - C14: Moved T6DOF to separate file (not pulic interface)

 		20110415 - WVR: Added overloaded operator - and -=

 */

 #ifndef FTNOIR_TRACKER_TYPES_H

@@ -34,53 +35,17 @@
 //

 #pragma pack(push, 2)

 struct THeadPoseData {

-	double x, y, z, yaw, pitch, roll;

-	long frame_number;

-};

-#pragma pack(pop)

 

-//

-// Structure to hold all 6 DOF's

-//

-class T6DOF {

-public:

-	T6DOF( double x, double y, double z, 

-		   double yaw, double pitch, double roll ) {

-		position.x = x;

-		position.y = y;

-		position.z = z;

-		position.yaw = yaw;

-		position.pitch = pitch;

-		position.roll = roll;

-	}

+	THeadPoseData()

+		: x(0.0), y(0.0), z(0.0), yaw(0.0), pitch(0.0), roll(0.0), frame_number(0) {}

 

-	void initHeadPoseData(){

-		position.x = 0.0f;

-		position.y = 0.0f;

-		position.z = 0.0f;

-		position.yaw = 0.0f;

-		position.pitch = 0.0f;

-		position.roll = 0.0f;

-		position.frame_number = 0;

-	}

-	T6DOF operator-( T6DOF &other ) {

-		return T6DOF(position.x - other.position.x, position.y - other.position.y, position.z - other.position.z,

-			         position.yaw - other.position.yaw, position.pitch - other.position.pitch, position.roll - other.position.roll);

-	}

-	T6DOF operator-=( T6DOF &other ) {

-		return T6DOF(position.x - other.position.x, position.y - other.position.y, position.z - other.position.z,

-			         position.yaw - other.position.yaw, position.pitch - other.position.pitch, position.roll - other.position.roll);

-	}

-	T6DOF operator+( T6DOF &other ) {

-		return T6DOF(position.x + other.position.x, position.y + other.position.y, position.z + other.position.z,

-			         position.yaw + other.position.yaw, position.pitch + other.position.pitch, position.roll + other.position.roll);

-	}

-	T6DOF operator+=( T6DOF &other ) {

-		return T6DOF(position.x + other.position.x, position.y + other.position.y, position.z + other.position.z,

-			         position.yaw + other.position.yaw, position.pitch + other.position.pitch, position.roll + other.position.roll);

-	}

+	THeadPoseData(double x, double y, double z, 

+		double yaw, double pitch, double roll ) 

+		: x(x), y(y), z(z), yaw(yaw), pitch(pitch), roll(roll), frame_number(0) {}

 

-	THeadPoseData position;

+	double x, y, z, yaw, pitch, roll;

+	long frame_number;

 };

+#pragma pack(pop)

 

 #endif // FTNOIR_TRACKER_TYPES_H

diff --git a/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj b/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj
index 9617836f..56b5e1d5 100644
--- a/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj
+++ b/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj
@@ -207,9 +207,17 @@
 				>

 			</File>

 			<File

+				RelativePath=".\rotation.cpp"

+				>

+			</File>

+			<File

 				RelativePath=".\tracker.cpp"

 				>

 			</File>

+			<File

+				RelativePath=".\tracker_types.cpp"

+				>

+			</File>

 		</Filter>

 		<Filter

 			Name="Headers"

@@ -299,6 +307,10 @@
 				>

 			</File>

 			<File

+				RelativePath=".\rotation.h"

+				>

+			</File>

+			<File

 				RelativePath=".\tracker.h"

 				>

 				<FileConfiguration

@@ -324,6 +336,10 @@
 					/>

 				</FileConfiguration>

 			</File>

+			<File

+				RelativePath=".\tracker_types.h"

+				>

+			</File>

 		</Filter>

 		<Filter

 			Name="FormsQt"

diff --git a/FaceTrackNoIR/rotation.cpp b/FaceTrackNoIR/rotation.cpp
new file mode 100644
index 00000000..1c89d775
--- /dev/null
+++ b/FaceTrackNoIR/rotation.cpp
@@ -0,0 +1,48 @@
+/* Copyright (c) 2012 Patrick Ruoff

+ *

+ * Permission to use, copy, modify, and/or distribute this software for any

+ * purpose with or without fee is hereby granted, provided that the above

+ * copyright notice and this permission notice appear in all copies.

+ */

+

+#include "rotation.h"

+

+#include <cmath>

+

+// ----------------------------------------------------------------------------

+Rotation Rotation::inv()

+{

+	return Rotation(a,-b,-c,-d);

+}

+

+// conversions

+// see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

+void Rotation::fromEuler(double yaw, double pitch, double roll)

+{

+	double sin_phi = sin(roll/2.0);

+	double cos_phi = cos(roll/2.0);

+	double sin_the = sin(pitch/2.0);

+	double cos_the = cos(pitch/2.0);

+	double sin_psi = sin(yaw/2.0);

+	double cos_psi = cos(yaw/2.0);

+

+	a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;

+	b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;

+	c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;

+	d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;

+}

+

+void Rotation::toEuler(double& yaw, double& pitch, double& roll)

+{

+	roll = atan2(2.0*(a*b + c*d), 1.0 - 2.0*(b*b + c*c));

+	pitch = asin(2.0*(a*c - b*d));

+	yaw =  atan2(2.0*(a*d + b*c), 1.0 - 2.0*(c*c + d*d));

+}

+

+Rotation operator*(const Rotation& A, const Rotation& B)

+{

+	return Rotation(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d,	// quaternion multiplication

+				    A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,

+				    A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,

+				    A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);

+}
\ No newline at end of file
diff --git a/FaceTrackNoIR/rotation.h b/FaceTrackNoIR/rotation.h
new file mode 100644
index 00000000..967d6661
--- /dev/null
+++ b/FaceTrackNoIR/rotation.h
@@ -0,0 +1,31 @@
+/* Copyright (c) 2012 Patrick Ruoff

+ *

+ * Permission to use, copy, modify, and/or distribute this software for any

+ * purpose with or without fee is hereby granted, provided that the above

+ * copyright notice and this permission notice appear in all copies.

+ */

+

+#ifndef ROTATION_H

+#define ROTATION_H

+

+// ----------------------------------------------------------------------------

+class Rotation {

+	friend Rotation operator*(const Rotation& A, const Rotation& B);

+public:

+	Rotation() : a(1.0),b(0.0),c(0.0),d(0.0) {}

+	Rotation(double yaw, double pitch, double roll) { fromEuler(yaw, pitch, roll); }

+	Rotation(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {}

+

+	Rotation inv();	// inverse

+

+	// conversions

+	void fromEuler(double yaw, double pitch, double roll);

+	void toEuler(double& yaw, double& pitch, double& roll);

+	

+protected:

+	double a,b,c,d; // quaternion coefficients

+};

+

+Rotation operator*(const Rotation& A, const Rotation& B); // composition of rotations

+

+#endif //ROTATION_H

diff --git a/FaceTrackNoIR/tracker.cpp b/FaceTrackNoIR/tracker.cpp
index 5b387555..c78644c1 100644
--- a/FaceTrackNoIR/tracker.cpp
+++ b/FaceTrackNoIR/tracker.cpp
@@ -334,11 +334,6 @@ T6DOF gameoutput_camera(0,0,0,0,0,0);
 	Tracker::do_tracking = true;				// Start initially

 	Tracker::do_center = true;					// Center initially

 

-	current_camera.initHeadPoseData();

-	target_camera.initHeadPoseData();

-	new_camera.initHeadPoseData();

-	output_camera.initHeadPoseData();

-

 	//

 	// Test some Filter-stuff

 	//

@@ -582,12 +577,12 @@ T6DOF gameoutput_camera(0,0,0,0,0,0);
 				//

 				if (Tracker::confid) {

 

-					offset_camera.position.x     = getSmoothFromList( &X.rawList );

-					offset_camera.position.y     = getSmoothFromList( &Y.rawList );

-					offset_camera.position.z     = getSmoothFromList( &Z.rawList );

-					offset_camera.position.pitch = getSmoothFromList( &Pitch.rawList );

-					offset_camera.position.yaw   = getSmoothFromList( &Yaw.rawList );

-					offset_camera.position.roll  = getSmoothFromList( &Roll.rawList );

+					offset_camera.x     = getSmoothFromList( &X.rawList );

+					offset_camera.y     = getSmoothFromList( &Y.rawList );

+					offset_camera.z     = getSmoothFromList( &Z.rawList );

+					offset_camera.pitch = getSmoothFromList( &Pitch.rawList );

+					offset_camera.yaw   = getSmoothFromList( &Yaw.rawList );

+					offset_camera.roll  = getSmoothFromList( &Roll.rawList );

 				}

 

 				Tracker::do_center = false;

@@ -599,50 +594,47 @@ T6DOF gameoutput_camera(0,0,0,0,0,0);
 			//

 			if (Tracker::confid && Tracker::do_game_zero) {

 				if (!pTracker->notifyZeroed())

-					gamezero_camera.position = gameoutput_camera.position;

-//				gamezero_camera.position = gameoutput_camera.position;

+					gamezero_camera = gameoutput_camera;

+//				gamezero_camera = gameoutput_camera;

 

 				Tracker::do_game_zero = false;

 			}

 

 			if (Tracker::do_tracking && Tracker::confid) {

 

-				// Pitch

-				target_camera.position.x     = getSmoothFromList( &X.rawList );

-				target_camera.position.y     = getSmoothFromList( &Y.rawList );

-				target_camera.position.z     = getSmoothFromList( &Z.rawList );

-				target_camera.position.pitch = getSmoothFromList( &Pitch.rawList );

-				target_camera.position.yaw   = getSmoothFromList( &Yaw.rawList );

-				target_camera.position.roll  = getSmoothFromList( &Roll.rawList );

+				// get values

+				target_camera.x     = getSmoothFromList( &X.rawList );

+				target_camera.y     = getSmoothFromList( &Y.rawList );

+				target_camera.z     = getSmoothFromList( &Z.rawList );

+				target_camera.pitch = getSmoothFromList( &Pitch.rawList );

+				target_camera.yaw   = getSmoothFromList( &Yaw.rawList );

+				target_camera.roll  = getSmoothFromList( &Roll.rawList );

+

+				// do the centering

 				target_camera = target_camera - offset_camera;

 

 				if (Tracker::useFilter && pFilter) {

-					pFilter->FilterHeadPoseData(&current_camera.position, &target_camera.position, &new_camera.position, Tracker::Pitch.newSample);

+					pFilter->FilterHeadPoseData(&current_camera, &target_camera, &new_camera, Tracker::Pitch.newSample);

 				}

 				else {

-					new_camera.position.x     = getSmoothFromList( &X.rawList )     - offset_camera.position.x;

-					new_camera.position.y     = getSmoothFromList( &Y.rawList )     - offset_camera.position.y;

-					new_camera.position.z     = getSmoothFromList( &Z.rawList )     - offset_camera.position.z;

-					new_camera.position.pitch = getSmoothFromList( &Pitch.rawList ) - offset_camera.position.pitch;

-					new_camera.position.yaw   = getSmoothFromList( &Yaw.rawList )   - offset_camera.position.yaw;

-					new_camera.position.roll  = getSmoothFromList( &Roll.rawList )  - offset_camera.position.roll;

+					new_camera = target_camera;

 				}

-				output_camera.position.x = X.invert * X.curvePtr->getValue(new_camera.position.x);

-				output_camera.position.y = Y.invert * Y.curvePtr->getValue(new_camera.position.y);

-				output_camera.position.z = Z.invert * Z.curvePtr->getValue(new_camera.position.z);

-				bool altp = new_camera.position.pitch < 0;

+				output_camera.x = X.invert * X.curvePtr->getValue(new_camera.x);

+				output_camera.y = Y.invert * Y.curvePtr->getValue(new_camera.y);

+				output_camera.z = Z.invert * Z.curvePtr->getValue(new_camera.z);

+				bool altp = new_camera.pitch < 0;

 				if (altp) {

-					output_camera.position.pitch = Pitch.invert * Pitch.curvePtrAlt->getValue(new_camera.position.pitch);

+					output_camera.pitch = Pitch.invert * Pitch.curvePtrAlt->getValue(new_camera.pitch);

 					Pitch.curvePtr->setTrackingActive( false );

 					Pitch.curvePtrAlt->setTrackingActive( true );

 				}

 				else {

-					output_camera.position.pitch = Pitch.invert * Pitch.curvePtr->getValue(new_camera.position.pitch);

+					output_camera.pitch = Pitch.invert * Pitch.curvePtr->getValue(new_camera.pitch);

 					Pitch.curvePtr->setTrackingActive( true );

 					Pitch.curvePtrAlt->setTrackingActive( false );

 				}

-				output_camera.position.yaw = Yaw.invert * Yaw.curvePtr->getValue(new_camera.position.yaw);

-				output_camera.position.roll = Roll.invert * Roll.curvePtr->getValue(new_camera.position.roll);

+				output_camera.yaw = Yaw.invert * Yaw.curvePtr->getValue(new_camera.yaw);

+				output_camera.roll = Roll.invert * Roll.curvePtr->getValue(new_camera.roll);

 

 

 				X.curvePtr->setTrackingActive( true );

@@ -654,22 +646,22 @@ T6DOF gameoutput_camera(0,0,0,0,0,0);
 				//

 				// Reverse Axis.

 				//

-				actualYaw = output_camera.position.yaw;					// Save the actual Yaw, otherwise we can't check for +90

-				actualZ = output_camera.position.z;						// Also the Z

+				actualYaw = output_camera.yaw;					// Save the actual Yaw, otherwise we can't check for +90

+				actualZ = output_camera.z;						// Also the Z

 				if (Tracker::do_axis_reverse) {

-					output_camera.position.z = Z_PosWhenReverseAxis;	// Set the desired Z-position

+					output_camera.z = Z_PosWhenReverseAxis;	// Set the desired Z-position

 				}

 

 				//

 				// Reset value for the selected axis, if inhibition is active

 				//

 				if (Tracker::do_inhibit) {

-					if (InhibitKey.doPitch) output_camera.position.pitch = 0.0f;

-					if (InhibitKey.doYaw) output_camera.position.yaw = 0.0f;

-					if (InhibitKey.doRoll) output_camera.position.roll = 0.0f;

-					if (InhibitKey.doX) output_camera.position.x = 0.0f;

-					if (InhibitKey.doY) output_camera.position.y = 0.0f;

-					if (InhibitKey.doZ) output_camera.position.z = 0.0f;

+					if (InhibitKey.doPitch) output_camera.pitch = 0.0f;

+					if (InhibitKey.doYaw) output_camera.yaw = 0.0f;

+					if (InhibitKey.doRoll) output_camera.roll = 0.0f;

+					if (InhibitKey.doX) output_camera.x = 0.0f;

+					if (InhibitKey.doY) output_camera.y = 0.0f;

+					if (InhibitKey.doZ) output_camera.z = 0.0f;

 				}

 

 				// All Protocol server(s)

@@ -687,12 +679,12 @@ T6DOF gameoutput_camera(0,0,0,0,0,0);
 				debug_Client->setHeadPosY( Tracker::Y.headPos );

 				debug_Client->setHeadPosZ( Tracker::Z.headPos );

 

-				debug_Client->setVirtRotX ( new_camera.position.pitch );				// degrees

-				debug_Client->setVirtRotY ( new_camera.position.yaw );

-				debug_Client->setVirtRotZ ( new_camera.position.roll );

-				debug_Client->setVirtPosX ( new_camera.position.x );				// centimeters

-				debug_Client->setVirtPosY ( new_camera.position.y );

-				debug_Client->setVirtPosZ ( new_camera.position.z );

+				debug_Client->setVirtRotX ( new_camera.pitch );				// degrees

+				debug_Client->setVirtRotY ( new_camera.yaw );

+				debug_Client->setVirtRotZ ( new_camera.roll );

+				debug_Client->setVirtPosX ( new_camera.x );				// centimeters

+				debug_Client->setVirtPosY ( new_camera.y );

+				debug_Client->setVirtPosZ ( new_camera.z );

 #       endif

 

 

@@ -702,12 +694,12 @@ T6DOF gameoutput_camera(0,0,0,0,0,0);
 				// Go to initial position

 				//

 				if (pProtocol && setZero) {

-					output_camera.position.pitch = 0.0f;

-					output_camera.position.yaw = 0.0f;

-					output_camera.position.roll = 0.0f;

-					output_camera.position.x = 0.0f;

-					output_camera.position.y = 0.0f;

-					output_camera.position.z = 0.0f;

+					output_camera.pitch = 0.0f;

+					output_camera.yaw = 0.0f;

+					output_camera.roll = 0.0f;

+					output_camera.x = 0.0f;

+					output_camera.y = 0.0f;

+					output_camera.z = 0.0f;

 					gameoutput_camera = output_camera + gamezero_camera;

 					pProtocol->sendHeadposeToGame( &gameoutput_camera );				// degrees & centimeters

 				}

@@ -840,13 +832,13 @@ void Tracker::getHeadPose( THeadPoseData *data ) {
 // Get the output-headpose, so it can be displayed.

 //

 void Tracker::getOutputHeadPose( THeadPoseData *data ) {

-	data->x = output_camera.position.x;										// centimeters

-	data->y = output_camera.position.y;

-	data->z = output_camera.position.z;

+	data->x = output_camera.x;										// centimeters

+	data->y = output_camera.y;

+	data->z = output_camera.z;

 

-	data->pitch = output_camera.position.pitch;	// degrees

-	data->yaw   = output_camera.position.yaw;

-	data->roll  = output_camera.position.roll;

+	data->pitch = output_camera.pitch;	// degrees

+	data->yaw   = output_camera.yaw;

+	data->roll  = output_camera.roll;

 }

 

 //

diff --git a/FaceTrackNoIR/tracker.h b/FaceTrackNoIR/tracker.h
index 57e8136b..4b52e438 100644
--- a/FaceTrackNoIR/tracker.h
+++ b/FaceTrackNoIR/tracker.h
@@ -45,6 +45,7 @@
 #include "..\ftnoir_tracker_base\FTNoIR_Tracker_base.h"

 #include "..\ftnoir_protocol_base\FTNoIR_Protocol_base.h"

 #include "..\ftnoir_filter_base\FTNoIR_Filter_base.h"

+#include "tracker_types.h"

 

 typedef ITrackerPtr (WINAPI *importGetTracker)(void);

 typedef IProtocolPtr (WINAPI *importGetProtocol)(void);

diff --git a/FaceTrackNoIR/tracker_types.cpp b/FaceTrackNoIR/tracker_types.cpp
new file mode 100644
index 00000000..e3c15807
--- /dev/null
+++ b/FaceTrackNoIR/tracker_types.cpp
@@ -0,0 +1,44 @@
+#include "tracker_types.h"

+#include "rotation.h"

+

+const double PI = 3.14159265358979323846264;

+const double D2R = PI/180.0;

+const double R2D = 180.0/PI;

+

+T6DOF operator-(const T6DOF& A, const T6DOF& B)

+{

+	Rotation R_A(A.yaw*D2R, A.pitch*D2R, A.roll*D2R);

+	Rotation R_B(B.yaw*D2R, B.pitch*D2R, B.roll*D2R);

+	Rotation R_C = R_A * R_B.inv();

+

+	T6DOF C;

+	R_C.toEuler(C.yaw, C.pitch, C.roll);

+	C.yaw *= R2D;

+	C.pitch *= R2D;

+	C.roll *= R2D;

+

+	C.x = A.x - B.x;

+	C.y = A.y - B.y;

+	C.z = A.z - B.z;

+	//C.frame_number?

+	return C;

+}

+

+T6DOF operator+(const T6DOF& A, const T6DOF& B)

+{

+	Rotation R_A(A.yaw*D2R, A.pitch*D2R, A.roll*D2R);

+	Rotation R_B(B.yaw*D2R, B.pitch*D2R, B.roll*D2R);

+	Rotation R_C = R_A * R_B;

+

+	T6DOF C;

+	R_C.toEuler(C.yaw, C.pitch, C.roll);

+	C.yaw *= R2D;

+	C.pitch *= R2D;

+	C.roll *= R2D;

+

+	C.x = A.x + B.x;

+	C.y = A.y + B.y;

+	C.z = A.z + B.z;

+	//C.frame_number?

+	return C;

+}
\ No newline at end of file
diff --git a/FaceTrackNoIR/tracker_types.h b/FaceTrackNoIR/tracker_types.h
new file mode 100644
index 00000000..5a13af85
--- /dev/null
+++ b/FaceTrackNoIR/tracker_types.h
@@ -0,0 +1,45 @@
+/********************************************************************************

+* FaceTrackNoIR		This program is a private project of the some enthusiastic	*

+*					gamers from Holland, who don't like to pay much for			*

+*					head-tracking.												*

+*																				*

+* Copyright (C) 2010 - 2012	Wim Vriend (Developing)								*

+*							Ron Hendriks (Researching and Testing)				*

+*																				*

+* Homepage																		*																				*

+* This program is free software; you can redistribute it and/or modify it		*

+* under the terms of the GNU General Public License as published by the			*

+* Free Software Foundation; either version 3 of the License, or (at your		*

+* option) any later version.													*

+*																				*

+* This program is distributed in the hope that it will be useful, but			*

+* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY	*

+* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for	*

+* more details.																	*

+*																				*

+* You should have received a copy of the GNU General Public License along		*

+* with this program; if not, see <http://www.gnu.org/licenses/>.				*

+*********************************************************************************/

+/*

+	Modifications (last one on top):

+		20120924 - C14: Moved tracker types only used by Tracker to this file (should not be part of public interface)

+		                Modified operators to represent correct frame transitions

+*/

+#ifndef __TRACKER_TYPES_H__

+#define __TRACKER_TYPES_H__

+

+#include "..\ftnoir_tracker_base\ftnoir_tracker_types.h"

+

+class T6DOF : public THeadPoseData

+{

+public:

+	T6DOF() : THeadPoseData() {}

+

+	T6DOF(double x, double y, double z, double yaw, double pitch, double roll) 

+		:  THeadPoseData(x,y,z, yaw,pitch,roll) {}

+};

+

+T6DOF operator-(const T6DOF& A, const T6DOF& B); // get new pose with respect to reference pose B

+T6DOF operator+(const T6DOF& A, const T6DOF& B); // get new pose with respect to reference pose B^-1

+

+#endif //__TRACKER_TYPES_H__
\ No newline at end of file