Removed QThread reference of protocol server-code.

Have still to adapt some of them though...

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

1 files changed, 176 insertions, 162 deletions

diff --git a/FaceTrackNoIR/tracker.cpp b/FaceTrackNoIR/tracker.cpp
index 1e3c17b4..a43acd9a 100644
--- a/FaceTrackNoIR/tracker.cpp
+++ b/FaceTrackNoIR/tracker.cpp
@@ -23,6 +23,8 @@
 *********************************************************************************/

 /*

 	Modifications (last one on top):

+		20101224 - WVR: Removed the QThread inheritance of the Base Class for the protocol-servers.

+						Again, this drastically simplifies the code in the protocols.

 		20101217 - WVR: Created Base Class for the protocol-servers. This drastically simplifies

 						the code needed here.

 		20101024 - WVR: Added shortkey to disable/enable one or more axis during tracking.

@@ -55,6 +57,7 @@ bool Tracker::do_tracking = true;
 bool Tracker::do_center = false;

 bool Tracker::do_inhibit = false;

 bool Tracker::useFilter = false;

+HANDLE Tracker::hTrackMutex = 0;

 

 long Tracker::prevHeadPoseTime = 0;

 THeadPoseDOF Tracker::Pitch;							// One structure for each of 6DOF's

@@ -80,6 +83,8 @@ Tracker::Tracker( int clientID, int facetrackerID ) {
 	m_StopThread = CreateEvent(0, TRUE, FALSE, 0);

 	m_WaitThread = CreateEvent(0, TRUE, FALSE, 0);

 

+	Tracker::hTrackMutex = CreateMutexA(NULL, false, "HeadPose_mutex");

+

 	try {

 	    // Initialize the faceAPI Qt library

 		sm::faceapi::qt::initialize();

@@ -157,6 +162,7 @@ Tracker::~Tracker() {
 	// Close handles

 	::CloseHandle(m_StopThread);

 	::CloseHandle(m_WaitThread);

+	::CloseHandle( Tracker::hTrackMutex );

 

 	_engine->stop();

 	smAPIQuit();

@@ -196,10 +202,10 @@ void Tracker::setup(QWidget *head, FaceTrackNoIR *parent) {
 	if (server_Game) {

 

 		DLL_Ok = server_Game->checkServerInstallationOK( mainApp->winId() );

-		if (DLL_Ok) {

-			server_Game->start();							// Start the thread

-		}

-		else {

+		if (!DLL_Ok) {

+//			server_Game->start();							// Start the thread

+		//}

+		//else {

 			QMessageBox::information(mainApp, "FaceTrackNoIR error", "Protocol is not (correctly) installed!");

 		}

 

@@ -342,186 +348,191 @@ void Tracker::run() {
 		   }

 		}

 

-		//

-		// Get the System-time and substract the time from the previous call.

-		// dT will be used for the EWMA-filter.

-		//

-		GetSystemTime ( &now );

-		newHeadPoseTime = (((now.wHour * 3600) + (now.wMinute * 60) + now.wSecond) * 1000) + now.wMilliseconds;

-		dT = (newHeadPoseTime - Tracker::prevHeadPoseTime) / 1000.0f;

-

-		// Remember time for next call

-		Tracker::prevHeadPoseTime = newHeadPoseTime;

-

-		//if the confidence is good enough the headpose will be updated **/

-		if (Tracker::confid) {

+		if (WaitForSingleObject(Tracker::hTrackMutex, 100) == WAIT_OBJECT_0) {

 

 			//

-			// Most games need an offset to the initial position and NOT the

-			// absolute distance to the camera: so remember the initial distance

-			// to substract that later...

+			// Get the System-time and substract the time from the previous call.

+			// dT will be used for the EWMA-filter.

 			//

-			if(Tracker::set_initial == false) {

-				Tracker::Pitch.initial_headPos = Tracker::Pitch.headPos;

-				Tracker::Yaw.initial_headPos = Tracker::Yaw.headPos;

-				Tracker::Roll.initial_headPos = Tracker::Roll.headPos;

-				Tracker::X.initial_headPos = Tracker::X.headPos;

-				Tracker::Y.initial_headPos = Tracker::Y.headPos;

-				Tracker::Z.initial_headPos = Tracker::Z.headPos;

-				MessageBeep (MB_ICONASTERISK);

-				Tracker::set_initial = true;

-			}

+			GetSystemTime ( &now );

+			newHeadPoseTime = (((now.wHour * 3600) + (now.wMinute * 60) + now.wSecond) * 1000) + now.wMilliseconds;

+			dT = (newHeadPoseTime - Tracker::prevHeadPoseTime) / 1000.0f;

 

-			rawrotX = Tracker::Pitch.headPos- Tracker::Pitch.initial_headPos;	// degrees

-			rawrotY = Tracker::Yaw.headPos- Tracker::Yaw.initial_headPos;

-			rawrotZ = Tracker::Roll.headPos - Tracker::Roll.initial_headPos;

-			rawposX = Tracker::X.headPos - Tracker::X.initial_headPos;										// centimeters

-			rawposY = Tracker::Y.headPos - Tracker::Y.initial_headPos;

-			rawposZ = Tracker::Z.headPos - Tracker::Z.initial_headPos;

+			// Remember time for next call

+			Tracker::prevHeadPoseTime = newHeadPoseTime;

 

-			headRotXLine->setText(QString("%1").arg( rawrotX, 0, 'f', 1));		// show degrees

-			headRotYLine->setText(QString("%1").arg( rawrotY, 0, 'f', 1));

-			headRotZLine->setText(QString("%1").arg( rawrotZ, 0, 'f', 1));

+			//if the confidence is good enough the headpose will be updated **/

+			if (Tracker::confid) {

 

-			headXLine->setText(QString("%1").arg( rawposX, 0, 'f', 1));			// show centimeters

-			headYLine->setText(QString("%1").arg( rawposY, 0, 'f', 1));

-			headZLine->setText(QString("%1").arg( rawposZ, 0, 'f', 1));

-

-		}

+				//

+				// Most games need an offset to the initial position and NOT the

+				// absolute distance to the camera: so remember the initial distance

+				// to substract that later...

+				//

+				if(Tracker::set_initial == false) {

+					Tracker::Pitch.initial_headPos = Tracker::Pitch.headPos;

+					Tracker::Yaw.initial_headPos = Tracker::Yaw.headPos;

+					Tracker::Roll.initial_headPos = Tracker::Roll.headPos;

+					Tracker::X.initial_headPos = Tracker::X.headPos;

+					Tracker::Y.initial_headPos = Tracker::Y.headPos;

+					Tracker::Z.initial_headPos = Tracker::Z.headPos;

+					MessageBeep (MB_ICONASTERISK);

+					Tracker::set_initial = true;

+				}

 

-		//

-		// If Center is pressed, copy the current values to the offsets.

-		//

-		if (Tracker::do_center && Tracker::set_initial) {

-			Pitch.offset_headPos = getSmoothFromList( &Pitch.rawList )- Tracker::Pitch.initial_headPos;

-			Yaw.offset_headPos = getSmoothFromList( &Yaw.rawList ) - Tracker::Yaw.initial_headPos;

-			Roll.offset_headPos = getSmoothFromList( &Roll.rawList ) - Tracker::Roll.initial_headPos;

-			X.offset_headPos = getSmoothFromList( &X.rawList ) - Tracker::X.initial_headPos;

-			Y.offset_headPos = getSmoothFromList( &Y.rawList ) - Tracker::Y.initial_headPos;

-			Z.offset_headPos = getSmoothFromList( &Z.rawList ) - Tracker::Z.initial_headPos;

-

-			Tracker::do_center = false;

-		}

+				rawrotX = Tracker::Pitch.headPos- Tracker::Pitch.initial_headPos;	// degrees

+				rawrotY = Tracker::Yaw.headPos- Tracker::Yaw.initial_headPos;

+				rawrotZ = Tracker::Roll.headPos - Tracker::Roll.initial_headPos;

+				rawposX = Tracker::X.headPos - Tracker::X.initial_headPos;										// centimeters

+				rawposY = Tracker::Y.headPos - Tracker::Y.initial_headPos;

+				rawposZ = Tracker::Z.headPos - Tracker::Z.initial_headPos;

 

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

+				headRotXLine->setText(QString("%1").arg( rawrotX, 0, 'f', 1));		// show degrees

+				headRotYLine->setText(QString("%1").arg( rawrotY, 0, 'f', 1));

+				headRotZLine->setText(QString("%1").arg( rawrotZ, 0, 'f', 1));

 

-//////		Use this for some debug-output to file...

-////		QFile data("output.txt");

-////		if (data.open(QFile::WriteOnly | QFile::Append)) {

-////			QTextStream out(&data);

-////				out << Pitch.NeutralZone << " " << getDegreesFromRads(rotX) << " " << getOutputFromCurve(&Pitch.curve, getDegreesFromRads(rotX), Pitch.NeutralZone, Pitch.MaxInput)  << '\n';

-//////			out << dT << " " << getSmoothFromList( &Pitch.rawList ) << " " << Pitch.offset_headPos << '\n';

-////		}

+				headXLine->setText(QString("%1").arg( rawposX, 0, 'f', 1));			// show centimeters

+				headYLine->setText(QString("%1").arg( rawposY, 0, 'f', 1));

+				headZLine->setText(QString("%1").arg( rawposZ, 0, 'f', 1));

 

-			// Pitch

-			if (Tracker::useFilter) {

-				rotX = lowPassFilter ( getSmoothFromList( &Pitch.rawList ) - Pitch.offset_headPos - Pitch.initial_headPos, 

-											   &Pitch.prevPos, dT, Tracker::Pitch.red );

-			}

-			else {

-				rotX = getSmoothFromList( &Pitch.rawList ) - Pitch.offset_headPos - Pitch.initial_headPos;

 			}

-			rotX = Pitch.invert * getOutputFromCurve(&Pitch.curve, rotX, Pitch.NeutralZone, Pitch.MaxInput);

 

-			// Yaw

-			if (Tracker::useFilter) {

-				rotY = lowPassFilter ( getSmoothFromList( &Yaw.rawList ) - Yaw.offset_headPos - Yaw.initial_headPos, 

-											   &Yaw.prevPos, dT, Tracker::Yaw.red );

-			}

-			else {

-				rotY = getSmoothFromList( &Yaw.rawList ) - Yaw.offset_headPos - Yaw.initial_headPos;

+			//

+			// If Center is pressed, copy the current values to the offsets.

+			//

+			if (Tracker::do_center && Tracker::set_initial) {

+				Pitch.offset_headPos = getSmoothFromList( &Pitch.rawList )- Tracker::Pitch.initial_headPos;

+				Yaw.offset_headPos = getSmoothFromList( &Yaw.rawList ) - Tracker::Yaw.initial_headPos;

+				Roll.offset_headPos = getSmoothFromList( &Roll.rawList ) - Tracker::Roll.initial_headPos;

+				X.offset_headPos = getSmoothFromList( &X.rawList ) - Tracker::X.initial_headPos;

+				Y.offset_headPos = getSmoothFromList( &Y.rawList ) - Tracker::Y.initial_headPos;

+				Z.offset_headPos = getSmoothFromList( &Z.rawList ) - Tracker::Z.initial_headPos;

+

+				Tracker::do_center = false;

 			}

-			rotY = Yaw.invert * getOutputFromCurve(&Yaw.curve, rotY, Yaw.NeutralZone, Yaw.MaxInput);

 

-			// Roll

-			if (Tracker::useFilter) {

-				rotZ = lowPassFilter ( getSmoothFromList( &Roll.rawList ) - Roll.offset_headPos - Roll.initial_headPos, 

-											   &Roll.prevPos, dT, Tracker::Roll.red );

-			}

-			else {

-				rotZ = getSmoothFromList( &Roll.rawList ) - Roll.offset_headPos - Roll.initial_headPos;

-			}

-			rotZ = Roll.invert * getOutputFromCurve(&Roll.curve, rotZ, Roll.NeutralZone, Roll.MaxInput);

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

 

-			// X

-			if (Tracker::useFilter) {

-				posX = lowPassFilter ( getSmoothFromList( &X.rawList ) - X.offset_headPos - X.initial_headPos, 

-											   &X.prevPos, dT, Tracker::X.red );

-			}

-			else {

-				posX = getSmoothFromList( &X.rawList ) - X.offset_headPos - X.initial_headPos;

-			}

-			posX = X.invert * getOutputFromCurve(&X.curve, posX, X.NeutralZone, X.MaxInput);

+	//////		Use this for some debug-output to file...

+	////		QFile data("output.txt");

+	////		if (data.open(QFile::WriteOnly | QFile::Append)) {

+	////			QTextStream out(&data);

+	////				out << Pitch.NeutralZone << " " << getDegreesFromRads(rotX) << " " << getOutputFromCurve(&Pitch.curve, getDegreesFromRads(rotX), Pitch.NeutralZone, Pitch.MaxInput)  << '\n';

+	//////			out << dT << " " << getSmoothFromList( &Pitch.rawList ) << " " << Pitch.offset_headPos << '\n';

+	////		}

 

-			// Y

-			if (Tracker::useFilter) {

-				posY = lowPassFilter ( getSmoothFromList( &Y.rawList ) - Y.offset_headPos - Y.initial_headPos, 

-											   &Y.prevPos, dT, Tracker::Y.red );

-			}

-			else {

-				posY = getSmoothFromList( &Y.rawList ) - Y.offset_headPos - Y.initial_headPos;

-			}

-			posY = Y.invert * getOutputFromCurve(&Y.curve, posY, Y.NeutralZone, Y.MaxInput);

+				// Pitch

+				if (Tracker::useFilter) {

+					rotX = lowPassFilter ( getSmoothFromList( &Pitch.rawList ) - Pitch.offset_headPos - Pitch.initial_headPos, 

+												   &Pitch.prevPos, dT, Tracker::Pitch.red );

+				}

+				else {

+					rotX = getSmoothFromList( &Pitch.rawList ) - Pitch.offset_headPos - Pitch.initial_headPos;

+				}

+				rotX = Pitch.invert * getOutputFromCurve(&Pitch.curve, rotX, Pitch.NeutralZone, Pitch.MaxInput);

 

-			// Z

-			if (Tracker::useFilter) {

-				posZ = lowPassFilter ( getSmoothFromList( &Z.rawList ) - Z.offset_headPos - Z.initial_headPos, 

-											   &Z.prevPos, dT, Tracker::Z.red );

-			}

-			else {

-				posZ = getSmoothFromList( &Z.rawList ) - Z.offset_headPos - Z.initial_headPos;

-			}

-			posZ = Z.invert * getOutputFromCurve(&Z.curve, posZ, Z.NeutralZone, Z.MaxInput);

+				// Yaw

+				if (Tracker::useFilter) {

+					rotY = lowPassFilter ( getSmoothFromList( &Yaw.rawList ) - Yaw.offset_headPos - Yaw.initial_headPos, 

+												   &Yaw.prevPos, dT, Tracker::Yaw.red );

+				}

+				else {

+					rotY = getSmoothFromList( &Yaw.rawList ) - Yaw.offset_headPos - Yaw.initial_headPos;

+				}

+				rotY = Yaw.invert * getOutputFromCurve(&Yaw.curve, rotY, Yaw.NeutralZone, Yaw.MaxInput);

 

-			//

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

-			//

-			if (Tracker::do_inhibit) {

-				if (InhibitKey.doPitch) rotX = 0.0f;

-				if (InhibitKey.doYaw) rotY = 0.0f;

-				if (InhibitKey.doRoll) rotZ = 0.0f;

-				if (InhibitKey.doX) posX = 0.0f;

-				if (InhibitKey.doY) posY = 0.0f;

-				if (InhibitKey.doZ) posZ = 0.0f;

-			}

+				// Roll

+				if (Tracker::useFilter) {

+					rotZ = lowPassFilter ( getSmoothFromList( &Roll.rawList ) - Roll.offset_headPos - Roll.initial_headPos, 

+												   &Roll.prevPos, dT, Tracker::Roll.red );

+				}

+				else {

+					rotZ = getSmoothFromList( &Roll.rawList ) - Roll.offset_headPos - Roll.initial_headPos;

+				}

+				rotZ = Roll.invert * getOutputFromCurve(&Roll.curve, rotZ, Roll.NeutralZone, Roll.MaxInput);

 

-			//

-			// Send the Virtual Pose to selected Protocol-Server

-			//

-			// Free-track

-			if (selectedClient == FREE_TRACK) {

-				server_Game->setHeadRotX( rotX );					// degrees

-				server_Game->setHeadRotY( rotY );

-				server_Game->setHeadRotZ( rotZ );

-

-				server_Game->setHeadPosX( posX );					// centimeters

-				server_Game->setHeadPosY( posY );

-				server_Game->setHeadPosZ( posZ );

-			}

+				// X

+				if (Tracker::useFilter) {

+					posX = lowPassFilter ( getSmoothFromList( &X.rawList ) - X.offset_headPos - X.initial_headPos, 

+												   &X.prevPos, dT, Tracker::X.red );

+				}

+				else {

+					posX = getSmoothFromList( &X.rawList ) - X.offset_headPos - X.initial_headPos;

+				}

+				posX = X.invert * getOutputFromCurve(&X.curve, posX, X.NeutralZone, X.MaxInput);

+

+				// Y

+				if (Tracker::useFilter) {

+					posY = lowPassFilter ( getSmoothFromList( &Y.rawList ) - Y.offset_headPos - Y.initial_headPos, 

+												   &Y.prevPos, dT, Tracker::Y.red );

+				}

+				else {

+					posY = getSmoothFromList( &Y.rawList ) - Y.offset_headPos - Y.initial_headPos;

+				}

+				posY = Y.invert * getOutputFromCurve(&Y.curve, posY, Y.NeutralZone, Y.MaxInput);

+

+				// Z

+				if (Tracker::useFilter) {

+					posZ = lowPassFilter ( getSmoothFromList( &Z.rawList ) - Z.offset_headPos - Z.initial_headPos, 

+												   &Z.prevPos, dT, Tracker::Z.red );

+				}

+				else {

+					posZ = getSmoothFromList( &Z.rawList ) - Z.offset_headPos - Z.initial_headPos;

+				}

+				posZ = Z.invert * getOutputFromCurve(&Z.curve, posZ, Z.NeutralZone, Z.MaxInput);

+

+				//

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

+				//

+				if (Tracker::do_inhibit) {

+					if (InhibitKey.doPitch) rotX = 0.0f;

+					if (InhibitKey.doYaw) rotY = 0.0f;

+					if (InhibitKey.doRoll) rotZ = 0.0f;

+					if (InhibitKey.doX) posX = 0.0f;

+					if (InhibitKey.doY) posY = 0.0f;

+					if (InhibitKey.doZ) posZ = 0.0f;

+				}

 

-			// FlightGear

-			if (server_Game) {

-				server_Game->setVirtRotX ( rotX );				// degrees

-				server_Game->setVirtRotY ( rotY );

-				server_Game->setVirtRotZ ( rotZ );

-				server_Game->setVirtPosX ( posX );				// centimeters

-				server_Game->setVirtPosY ( posY );

-				server_Game->setVirtPosZ ( posZ );

+				//

+				// Send the Virtual Pose to selected Protocol-Server

+				//

+				// Free-track

+				if (selectedClient == FREE_TRACK) {

+					server_Game->setHeadRotX( rotX );					// degrees

+					server_Game->setHeadRotY( rotY );

+					server_Game->setHeadRotZ( rotZ );

+

+					server_Game->setHeadPosX( posX );					// centimeters

+					server_Game->setHeadPosY( posY );

+					server_Game->setHeadPosZ( posZ );

+				}

+

+				// FlightGear

+				if (server_Game) {

+					server_Game->setVirtRotX ( rotX );				// degrees

+					server_Game->setVirtRotY ( rotY );

+					server_Game->setVirtRotZ ( rotZ );

+					server_Game->setVirtPosX ( posX );				// centimeters

+					server_Game->setVirtPosY ( posY );

+					server_Game->setVirtPosZ ( posZ );

+				}

 			}

-		}

-		else {

-			//

-			// Go to initial position

-			//

-			if (server_Game) {

-				server_Game->setVirtRotX ( 0.0f );

-				server_Game->setVirtRotY ( 0.0f );

-				server_Game->setVirtRotZ ( 0.0f );

-				server_Game->setVirtPosX ( 0.0f );

-				server_Game->setVirtPosY ( 0.0f );

-				server_Game->setVirtPosZ ( 0.0f );

+			else {

+				//

+				// Go to initial position

+				//

+				if (server_Game) {

+					server_Game->setVirtRotX ( 0.0f );

+					server_Game->setVirtRotY ( 0.0f );

+					server_Game->setVirtRotZ ( 0.0f );

+					server_Game->setVirtPosX ( 0.0f );

+					server_Game->setVirtPosY ( 0.0f );

+					server_Game->setVirtPosZ ( 0.0f );

+				}

 			}

 		}

+		ReleaseMutex(Tracker::hTrackMutex);

+		server_Game->sendHeadposeToGame();

 

 		//for lower cpu load 

 		msleep(10);

@@ -543,7 +554,8 @@ void Tracker::receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVid
 	// Perform actions, when valid data is received from faceAPI.

 	// Write the Raw headpose-data and add it to the RawList, for processing...

 	//

-	if( head_pose.confidence > 0 ) {

+	if (( head_pose.confidence > 0 ) && (WaitForSingleObject(Tracker::hTrackMutex, 100) == WAIT_OBJECT_0) ) {

+

 		Tracker::confid = true;

 

 		// Pitch

@@ -574,6 +586,8 @@ void Tracker::receiveHeadPose(void *,smEngineHeadPoseData head_pose, smCameraVid
 		Tracker::confid = false;

 	}

 

+	ReleaseMutex(Tracker::hTrackMutex);

+

 	// for lower cpu load

 	msleep(10);

 	yieldCurrentThread();