From 0948938ea70784b7b0607a709a199d1acefda949 Mon Sep 17 00:00:00 2001 From: Patrick Ruoff Date: Tue, 25 Sep 2012 17:02:38 +0000 Subject: 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 --- FTNoIR_Filter_EWMA2/FTNoIR_Filter_EWMA2_vc9.vcproj | 4 + FTNoIR_Protocol_Base/ftnoir_protocol_base.h | 2 +- FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.cpp | 14 +-- FTNoIR_Protocol_FG/FTNoIR_Protocol_FG.h | 2 +- FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.cpp | 10 +- FTNoIR_Protocol_FSUIPC/FTNoIR_Protocol_FSUIPC.h | 2 +- FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.cpp | 28 ++--- FTNoIR_Protocol_FT/FTNoIR_Protocol_FT.h | 2 +- FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.cpp | 14 +-- FTNoIR_Protocol_FTIR/FTNoIR_Protocol_FTIR.h | 2 +- FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.cpp | 4 +- FTNoIR_Protocol_FTN/FTNoIR_Protocol_FTN.h | 2 +- FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.cpp | 38 +++---- FTNoIR_Protocol_MOUSE/FTNoIR_Protocol_MOUSE.h | 2 +- FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.cpp | 14 +-- FTNoIR_Protocol_PPJOY/FTNoIR_Protocol_PPJOY.h | 2 +- FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.cpp | 14 +-- FTNoIR_Protocol_SC/FTNoIR_Protocol_SC.h | 2 +- FTNoIR_Tracker_Base/ftnoir_tracker_types.h | 53 ++-------- FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj | 16 +++ FaceTrackNoIR/rotation.cpp | 48 +++++++++ FaceTrackNoIR/rotation.h | 31 ++++++ FaceTrackNoIR/tracker.cpp | 116 ++++++++++----------- FaceTrackNoIR/tracker.h | 1 + FaceTrackNoIR/tracker_types.cpp | 44 ++++++++ FaceTrackNoIR/tracker_types.h | 45 ++++++++ 26 files changed, 329 insertions(+), 183 deletions(-) create mode 100644 FaceTrackNoIR/rotation.cpp create mode 100644 FaceTrackNoIR/rotation.h create mode 100644 FaceTrackNoIR/tracker_types.cpp create mode 100644 FaceTrackNoIR/tracker_types.h 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" > + + 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 @@ -206,10 +206,18 @@ RelativePath=".\main.cpp" > + + + + + + @@ -324,6 +336,10 @@ /> + + + +// ---------------------------------------------------------------------------- +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(¤t_camera.position, &target_camera.position, &new_camera.position, Tracker::Pitch.newSample); + pFilter->FilterHeadPoseData(¤t_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 . * +*********************************************************************************/ +/* + 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 -- cgit v1.2.3