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

7 files changed, 239 insertions, 62 deletions

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