diff options
Diffstat (limited to 'facetracknoir/xxx_tracker.cpp')
| -rw-r--r-- | facetracknoir/xxx_tracker.cpp | 1081 |
1 files changed, 1081 insertions, 0 deletions
diff --git a/facetracknoir/xxx_tracker.cpp b/facetracknoir/xxx_tracker.cpp new file mode 100644 index 00000000..5396c95b --- /dev/null +++ b/facetracknoir/xxx_tracker.cpp @@ -0,0 +1,1081 @@ +/********************************************************************************
+* 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) 2012 Wim Vriend (Developing) *
+* Ron Hendriks (Researching and Testing) *
+* *
+* Homepage: http://facetracknoir.sourceforge.net/home/default.htm *
+* *
+* 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):
+ 20130201 - WVR: Remove the Protocol, when stopping the Thread.
+ 20121215 - WVR: Fixed crash after message: protocol not installed correctly... by terminating the thread.
+ 20120921 - WVR: Fixed centering when no filter is selected.
+ 20120917 - WVR: Added Mouse-buttons to ShortKeys.
+ 20120827 - WVR: Signal tracking = false to Curve-widget(s) when quitting run(). Also when Alternative Pitch curve is used.
+ 20120805 - WVR: The FunctionConfig-widget is used to configure the Curves. It was tweaked some more, because the Accela filter now also
+ uses the Curve(s). ToDo: make the ranges configurable by the user. Development on the Toradex IMU makes us realize, that
+ a fixed input-range may not be so handy after all..
+ 20120427 - WVR: The Protocol-code was already in separate DLLs, but the ListBox was still filled ´statically´. Now, a Dir() of the
+ EXE-folder is done, to locate Protocol-DLLs. The Icons were also moved to the DLLs
+ 20120317 - WVR: The Filter and Tracker-code was moved to separate DLLs. The calling-method
+ was changed accordingly.
+ The face-tracker member-functions NotifyZeroed and refreshVideo were added, as
+ requested by Stanislaw.
+ 20110411 - WVR: Finished moving all Protocols to separate C++ projects. Every protocol now
+ has it's own Class, that's inside it's own DLL. This reduces the size of the program,
+ makes it more structured and enables a more sophisticated installer.
+ 20110328 - WVR: Changed the camera-structs into class-instances. This makes initialisation
+ easier and hopefully solves the remaining 'start-up problem'.
+ 20110313 - WVR: Removed 'set_initial'. Less is more.
+ 20110109 - WVR: Added setZero option to define behaviour after STOP tracking via shortkey.
+ 20110104 - WVR: Removed a few nasty bugs (it was impossible to stop tracker without crash).
+ 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.
+ 20101021 - WVR: Added FSUIPC server for FS2004.
+ 20101011 - WVR: Added SimConnect server.
+ 20101007 - WVR: Created 6DOF-curves and drastically changed the tracker for that.
+ Also eliminated a 'glitch' in the process.
+ 20100607 - WVR: Re-installed Rotation Neutral Zone and improved reaction
+ after 'start/stop'. MessageBeep when confidence is back...
+ 20100604 - WVR: Created structure for DOF-data and changed timing of
+ ReceiveHeadPose end run().
+ 20100602 - WVR: Implemented EWMA-filtering, according to the example of
+ Melchior Franz. Works like a charm...
+ 20100601 - WVR: Added DirectInput keyboard-handling. '=' used for center,
+ 'BACK' for start (+center)/stop.
+ 20100517 - WVR: Added upstream command(s) from FlightGear
+ 20100523 - WVR: Checkboxes to invert 6DOF's was implemented. Multiply by
+ 1 or (-1).
+*/
+#include "tracker.h"
+#include "FaceTrackNoIR.h"
+
+// Flags
+bool Tracker::confid = false;
+bool Tracker::do_tracking = true;
+bool Tracker::do_center = false;
+bool Tracker::do_inhibit = false;
+bool Tracker::do_game_zero = false;
+bool Tracker::do_axis_reverse = false;
+
+bool Tracker::setZero = true;
+bool Tracker::setEngineStop = true;
+HANDLE Tracker::hTrackMutex = 0;
+
+bool Tracker::useAxisReverse = false; // Use Axis Reverse
+float Tracker::YawAngle4ReverseAxis = 40.0f; // Axis Reverse settings
+float Tracker::Z_Pos4ReverseAxis = -20.0f;
+float Tracker::Z_PosWhenReverseAxis = 50.0f;
+
+
+T6DOF Tracker::current_camera(0,0,0,0,0,0); // Used for filtering
+T6DOF Tracker::target_camera(0,0,0,0,0,0);
+T6DOF Tracker::new_camera(0,0,0,0,0,0);
+T6DOF Tracker::output_camera(0,0,0,0,0,0); // Position sent to game protocol
+
+THeadPoseDOF Tracker::Pitch("PitchUp", "PitchDown", 50, 180, 50, 90); // One structure for each of 6DOF's
+THeadPoseDOF Tracker::Yaw("Yaw", "", 50, 180);
+THeadPoseDOF Tracker::Roll("Roll", "", 50, 180);
+THeadPoseDOF Tracker::X("X","", 50, 180);
+THeadPoseDOF Tracker::Y("Y","", 50, 180);
+THeadPoseDOF Tracker::Z("Z","", 50, 180);
+
+TShortKey Tracker::CenterKey; // ShortKey to Center headposition
+TShortKey Tracker::StartStopKey; // ShortKey to Start/stop tracking
+TShortKey Tracker::InhibitKey; // ShortKey to inhibit axis while tracking
+TShortKey Tracker::GameZeroKey; // ShortKey to Set Game Zero
+bool Tracker::DisableBeep = false; // Disable beep when center
+//TShortKey Tracker::AxisReverseKey; // ShortKey to start/stop axis reverse while tracking
+
+int Tracker::CenterMouseKey; // ShortKey to Center headposition
+int Tracker::StartStopMouseKey; // ShortKey to Start/stop tracking
+int Tracker::InhibitMouseKey; // ShortKey to inhibit axis while tracking
+int Tracker::GameZeroMouseKey; // ShortKey to Set Game Zero
+
+//ITrackerPtr Tracker::pTracker; // Pointer to Tracker instance (in DLL)
+IProtocolPtr Tracker::pProtocol; // Pointer to Protocol instance (in DLL)
+IFilterPtr Tracker::pFilter; // Pointer to Filter instance (in DLL)
+
+
+/** constructor **/
+Tracker::Tracker( FaceTrackNoIR *parent ) {
+QString libName;
+importGetTracker getIT;
+QLibrary *trackerLib;
+importGetFilter getFilter;
+QLibrary *filterLib;
+importGetProtocol getProtocol;
+QLibrary *protocolLib;
+QFrame *video_frame;
+
+ // Retieve the pointer to the parent
+ mainApp = parent;
+
+ // Create events
+ m_StopThread = CreateEvent(0, TRUE, FALSE, 0);
+ m_WaitThread = CreateEvent(0, TRUE, FALSE, 0);
+
+ Tracker::hTrackMutex = CreateMutexA(NULL, false, "HeadPose_mutex");
+
+ //
+ // Initialize the headpose-data
+ //
+ Tracker::Yaw.initHeadPoseData();
+ Tracker::Pitch.initHeadPoseData();
+ Tracker::Roll.initHeadPoseData();
+ Tracker::X.initHeadPoseData();
+ Tracker::Y.initHeadPoseData();
+ Tracker::Z.initHeadPoseData();
+
+ //
+ // Locate the video-frame, for the DLL
+ //
+ video_frame = 0;
+ video_frame = mainApp->getVideoWidget();
+ qDebug() << "Tracker::Tracker VideoFrame = " << video_frame;
+
+ //
+ // Load the Tracker-engine DLL, get the tracker-class from it and do stuff...
+ //
+ pTracker = NULL;
+ libName = mainApp->getCurrentTrackerName();
+ if (!libName.isEmpty()) {
+ trackerLib = new QLibrary(libName);
+ getIT = (importGetTracker) trackerLib->resolve("GetTracker");
+ qDebug() << "Tracker::Tracker libName = " << libName;
+
+ if (getIT) {
+ ITracker *ptrXyz(getIT()); // Get the Class
+ if (ptrXyz)
+ {
+ pTracker = ptrXyz;
+ pTracker->Initialize( video_frame );
+ qDebug() << "Tracker::setup Function Resolved!";
+ }
+ }
+ else {
+ QMessageBox::warning(0,"FaceTrackNoIR Error", libName + " DLL not loaded",QMessageBox::Ok,QMessageBox::NoButton);
+ }
+ }
+ //
+ // Load the Tracker-engine DLL, get the tracker-class from it and do stuff...
+ //
+ pSecondTracker = NULL;
+ libName = mainApp->getSecondTrackerName();
+ if ((!libName.isEmpty()) && (libName != "None")) {
+ trackerLib = new QLibrary(libName);
+ getIT = (importGetTracker) trackerLib->resolve("GetTracker");
+
+ if (getIT) {
+ ITracker *ptrXyz(getIT()); // Get the Class
+ if (ptrXyz)
+ {
+ pSecondTracker = ptrXyz;
+ pSecondTracker->Initialize( NULL );
+ qDebug() << "Tracker::setup Function Resolved!";
+ }
+ }
+ else {
+ QMessageBox::warning(0,"FaceTrackNoIR Error", libName + " DLL not loaded",QMessageBox::Ok,QMessageBox::NoButton);
+ }
+ }
+
+ //
+ // Load the DLL with the protocol-logic and retrieve a pointer to the Protocol-class.
+ //
+ libName = mainApp->getCurrentProtocolName();
+ if (!libName.isEmpty()) {
+ protocolLib = new QLibrary(libName);
+ getProtocol = (importGetProtocol) protocolLib->resolve("GetProtocol");
+ if (getProtocol) {
+ IProtocolPtr ptrXyz(getProtocol());
+ if (ptrXyz)
+ {
+ pProtocol = ptrXyz;
+ pProtocol->Initialize();
+ qDebug() << "Protocol::setup Function Resolved!";
+ }
+ }
+ else {
+ QMessageBox::warning(0,"FaceTrackNoIR Error", "Protocol-DLL not loaded",QMessageBox::Ok,QMessageBox::NoButton);
+ return;
+ }
+ }
+
+ //
+ // Load the DLL with the filter-logic and retrieve a pointer to the Filter-class.
+ //
+ pFilter = NULL;
+ libName = mainApp->getCurrentFilterName();
+
+ if ((!libName.isEmpty()) && (libName != "None")) {
+ filterLib = new QLibrary(libName);
+
+ getFilter = (importGetFilter) filterLib->resolve("GetFilter");
+ if (getFilter) {
+ IFilterPtr ptrXyz(getFilter());
+ if (ptrXyz)
+ {
+ pFilter = ptrXyz;
+ qDebug() << "Filter::setup Function Resolved!";
+ }
+ }
+ else {
+ QMessageBox::warning(0,"FaceTrackNoIR Error", "Filter-DLL not loaded",QMessageBox::Ok,QMessageBox::NoButton);
+ return;
+ }
+ }
+
+ // Load the settings from the INI-file
+ loadSettings();
+}
+
+/** destructor empty **/
+Tracker::~Tracker() {
+
+ // Stop the Tracker(s)
+ if (pTracker) {
+ pTracker->StopTracker( true );
+ }
+ if (pSecondTracker) {
+ pSecondTracker->StopTracker( true );
+ }
+
+ // Trigger thread to stop
+ ::SetEvent(m_StopThread);
+
+ // Wait until thread finished
+ if (isRunning()) {
+ ::WaitForSingleObject(m_WaitThread, INFINITE);
+ }
+
+ //
+ // Remove the Tracker
+ // 20120615, WVR: As suggested by Stanislaw
+ if (pTracker) {
+ delete pTracker;
+ pTracker = NULL;
+ }
+ if (pSecondTracker) {
+ delete pSecondTracker;
+ pSecondTracker = NULL;
+ }
+
+ //
+ // Remove the Protocol
+ //
+ if (pProtocol) {
+ delete pProtocol;
+ pProtocol = NULL;
+ }
+
+ // Close handles
+ ::CloseHandle(m_StopThread);
+ ::CloseHandle(m_WaitThread);
+
+ if (Tracker::hTrackMutex != 0) {
+ ::CloseHandle( Tracker::hTrackMutex );
+ }
+
+# ifdef USE_DEBUG_CLIENT
+ debug_Client->deleteLater(); // Delete Excel protocol-server
+# endif
+
+ qDebug() << "Tracker::~Tracker Finished...";
+
+}
+
+/** setting up the tracker engine **/
+void Tracker::setup() {
+ bool DLL_Ok;
+
+ // retrieve pointers to the User Interface and the main Application
+ if (pTracker) {
+ pTracker->StartTracker( mainApp->winId() );
+ }
+ if (pSecondTracker) {
+ pSecondTracker->StartTracker( mainApp->winId() );
+ }
+
+ //
+ // Check if the Protocol-server files were installed OK.
+ // Some servers also create a memory-mapping, for Inter Process Communication.
+ // The handle of the MainWindow is sent to 'The Game', so it can send a message back.
+ //
+ if (pProtocol) {
+
+ DLL_Ok = pProtocol->checkServerInstallationOK( mainApp->winId() );
+ if (!DLL_Ok) {
+ // Trigger thread to stop
+ ::SetEvent(m_StopThread);
+ QMessageBox::information(mainApp, "FaceTrackNoIR error", "Protocol is not (correctly) installed!");
+ }
+ }
+
+# ifdef USE_DEBUG_CLIENT
+ DLL_Ok = debug_Client->checkServerInstallationOK( mainApp->winId() ); // Check installation
+ if (!DLL_Ok) {
+ QMessageBox::information(mainApp, "FaceTrackNoIR error", "Excel Protocol is not (correctly) installed!");
+ }
+# endif
+
+}
+
+/** QThread run method @override **/
+void Tracker::run() {
+/** Direct Input variables **/
+//
+// The DirectX stuff was found here: http://www.directxtutorial.com/tutorial9/e-directinput/dx9e2.aspx
+//
+LPDIRECTINPUT8 din; // the pointer to our DirectInput interface
+LPDIRECTINPUTDEVICE8 dinkeyboard; // the pointer to the keyboard device
+LPDIRECTINPUTDEVICE8 dinmouse; // the pointer to the mouse device
+BYTE keystate[256]; // the storage for the key-information
+DIMOUSESTATE mousestate; // the storage for the mouse-information
+HRESULT retAcquire;
+bool lastCenterKey = false; // Remember state, to detect rising edge
+bool lastStartStopKey = false;
+bool lastInhibitKey = false;
+bool lastGameZeroKey = false;
+
+bool lastCenterMouseKey = false; // Remember state, to detect rising edge
+bool lastStartStopMouseKey = false;
+bool lastInhibitMouseKey = false;
+bool lastGameZeroMouseKey = false;
+
+bool waitAxisReverse = false;
+bool waitThroughZero = false;
+double actualYaw = 0.0f;
+double actualZ = 0.0f;
+T6DOF offset_camera(0,0,0,0,0,0);
+T6DOF gamezero_camera(0,0,0,0,0,0);
+T6DOF gameoutput_camera(0,0,0,0,0,0);
+
+bool bInitialCenter1 = true;
+bool bInitialCenter2 = true;
+bool bTracker1Confid = false;
+bool bTracker2Confid = false;
+
+ Tracker::do_tracking = true; // Start initially
+ Tracker::do_center = false; // Center initially
+
+ //
+ // Test some Filter-stuff
+ //
+ if (pFilter) {
+ QString filterName;
+ //pFilter->getFullName(&filterName);
+ //qDebug() << "Tracker::run() FilterName = " << filterName;
+ }
+
+ //
+ // Setup the DirectInput for keyboard strokes
+ //
+ // create the DirectInput interface
+ if (DirectInput8Create(GetModuleHandle(NULL), DIRECTINPUT_VERSION, IID_IDirectInput8,
+ (void**)&din, NULL) != DI_OK) { // COM stuff, so we'll set it to NULL
+ qDebug() << "Tracker::setup DirectInput8 Creation failed!" << GetLastError();
+ }
+
+ // create the keyboard device
+ if (din->CreateDevice(GUID_SysKeyboard, &dinkeyboard, NULL) != DI_OK) {
+ qDebug() << "Tracker::setup CreateDevice function failed!" << GetLastError();
+ }
+ // create the mouse device
+ din->CreateDevice(GUID_SysMouse, &dinmouse, NULL);
+
+ // set the data format to keyboard format
+ if (dinkeyboard->SetDataFormat(&c_dfDIKeyboard) != DI_OK) {
+ qDebug() << "Tracker::setup SetDataFormat function failed!" << GetLastError();
+ }
+ // set the data format to mouse format
+ dinmouse->SetDataFormat(&c_dfDIMouse);
+
+ // set the control you will have over the keyboard
+ if (dinkeyboard->SetCooperativeLevel(mainApp->winId(), DISCL_NONEXCLUSIVE | DISCL_BACKGROUND) != DI_OK) {
+ qDebug() << "Tracker::setup SetCooperativeLevel function failed!" << GetLastError();
+ }
+ // set the control you will have over the mouse
+ dinmouse->SetCooperativeLevel(mainApp->winId(), DISCL_NONEXCLUSIVE | DISCL_BACKGROUND);
+
+ forever
+ {
+
+ // Check event for stop thread
+ if(::WaitForSingleObject(m_StopThread, 0) == WAIT_OBJECT_0)
+ {
+ dinkeyboard->Unacquire(); // Unacquire keyboard
+ dinkeyboard->Release();
+ dinmouse->Unacquire(); // Unacquire mouse
+ dinmouse->Release();
+ din->Release(); // Release DirectInput
+
+ // Set event
+ ::SetEvent(m_WaitThread);
+ qDebug() << "Tracker::run terminated run()";
+ X.curvePtr->setTrackingActive( false );
+ Y.curvePtr->setTrackingActive( false );
+ Z.curvePtr->setTrackingActive( false );
+ Yaw.curvePtr->setTrackingActive( false );
+ Pitch.curvePtr->setTrackingActive( false );
+ Pitch.curvePtrAlt->setTrackingActive( false );
+ Roll.curvePtr->setTrackingActive( false );
+
+ return;
+ }
+
+ //
+ // Check the mouse
+ //
+ // get access if we don't have it already
+ retAcquire = dinmouse->Acquire();
+ if ( (retAcquire != DI_OK) && (retAcquire != S_FALSE) ) {
+ qDebug() << "Tracker::run Acquire function failed!" << GetLastError();
+ }
+ else {
+ if (dinmouse->GetDeviceState(sizeof(DIMOUSESTATE), (LPVOID)&mousestate) != DI_OK) {
+ qDebug() << "Tracker::run GetDeviceState function failed!" << GetLastError();
+ }
+ else {
+ //
+ // Check the state of the StartStop MouseKey
+ //
+ if ( isMouseKeyPressed( &StartStopMouseKey, &mousestate ) && (!lastStartStopMouseKey) ) {
+ Tracker::do_tracking = !Tracker::do_tracking;
+
+ //
+ // To start tracking again and to be at '0', execute Center command too
+ //
+ if (Tracker::do_tracking) {
+ Tracker::confid = false;
+ if (pTracker) {
+ pTracker->StartTracker( mainApp->winId() );
+ }
+ if (pSecondTracker) {
+ pSecondTracker->StartTracker( mainApp->winId() );
+ }
+ }
+ else {
+ if (setEngineStop) { // Only stop engine when option is checked
+ if (pTracker) {
+ pTracker->StopTracker( false );
+ }
+ if (pSecondTracker) {
+ pSecondTracker->StopTracker( false );
+ }
+ }
+ }
+ qDebug() << "Tracker::run() says StartStop pressed, do_tracking =" << Tracker::do_tracking;
+ }
+ lastStartStopMouseKey = isMouseKeyPressed( &StartStopMouseKey, &mousestate ); // Remember
+
+ //
+ // Check the state of the Center MouseKey
+ //
+ if ( isMouseKeyPressed( &CenterMouseKey, &mousestate ) && (!lastCenterMouseKey) ) {
+ Tracker::do_center = true;
+ qDebug() << "Tracker::run() says Center MouseKey pressed";
+ }
+ lastCenterMouseKey = isMouseKeyPressed( &CenterMouseKey, &mousestate ); // Remember
+
+ //
+ // Check the state of the GameZero MouseKey
+ //
+ if ( isMouseKeyPressed( &GameZeroMouseKey, &mousestate ) && (!lastGameZeroMouseKey) ) {
+ Tracker::do_game_zero = true;
+ qDebug() << "Tracker::run() says GameZero MouseKey pressed";
+ }
+ lastGameZeroMouseKey = isMouseKeyPressed( &GameZeroMouseKey, &mousestate ); // Remember
+
+ //
+ // Check the state of the Inhibit MouseKey
+ //
+ if ( isMouseKeyPressed( &InhibitMouseKey, &mousestate ) && (!lastInhibitMouseKey) ) {
+ Tracker::do_inhibit = !Tracker::do_inhibit;
+ qDebug() << "Tracker::run() says Inhibit MouseKey pressed";
+ //
+ // Execute Center command too, when inhibition ends.
+ //
+ if (!Tracker::do_inhibit) {
+ Tracker::do_center = true;
+ }
+ }
+ lastInhibitMouseKey = isMouseKeyPressed( &InhibitMouseKey, &mousestate ); // Remember
+ }
+ }
+
+ //
+ // Check the keyboard
+ //
+ // get access if we don't have it already
+ retAcquire = dinkeyboard->Acquire();
+ if ( (retAcquire != DI_OK) && (retAcquire != S_FALSE) ) {
+ qDebug() << "Tracker::run Acquire function failed!" << GetLastError();
+ }
+ else {
+ // get the input data
+ if (dinkeyboard->GetDeviceState(256, (LPVOID)keystate) != DI_OK) {
+ qDebug() << "Tracker::run GetDeviceState function failed!" << GetLastError();
+ }
+ else {
+ //
+ // Check the state of the Start/Stop key
+ //
+ if ( isShortKeyPressed( &StartStopKey, &keystate[0] ) && (!lastStartStopKey) ) {
+ Tracker::do_tracking = !Tracker::do_tracking;
+
+ //
+ // To start tracking again and to be at '0', execute Center command too
+ //
+ if (Tracker::do_tracking) {
+ Tracker::confid = false;
+ if (pTracker) {
+ pTracker->StartTracker( mainApp->winId() );
+ }
+ if (pSecondTracker) {
+ pSecondTracker->StartTracker( mainApp->winId() );
+ }
+ }
+ else {
+ if (setEngineStop) { // Only stop engine when option is checked
+ if (pTracker) {
+ pTracker->StopTracker( false );
+ }
+ if (pSecondTracker) {
+ pSecondTracker->StopTracker( false );
+ }
+ }
+ }
+ qDebug() << "Tracker::run() says StartStop pressed, do_tracking =" << Tracker::do_tracking;
+ }
+ lastStartStopKey = isShortKeyPressed( &StartStopKey, &keystate[0] ); // Remember
+
+ //
+ // Check the state of the Center key
+ //
+ if ( isShortKeyPressed( &CenterKey, &keystate[0] ) && (!lastCenterKey) ) {
+ Tracker::do_center = true;
+ qDebug() << "Tracker::run() says Center pressed";
+ }
+ lastCenterKey = isShortKeyPressed( &CenterKey, &keystate[0] ); // Remember
+
+ //
+ // Check the state of the GameZero key
+ //
+ if ( isShortKeyPressed( &GameZeroKey, &keystate[0] ) && (!lastGameZeroKey) ) {
+ Tracker::do_game_zero = true;
+ qDebug() << "Tracker::run() says GameZero pressed";
+ }
+ lastGameZeroKey = isShortKeyPressed( &GameZeroKey, &keystate[0] ); // Remember
+
+ //
+ // Check the state of the Inhibit key
+ //
+ if ( isShortKeyPressed( &InhibitKey, &keystate[0] ) && (!lastInhibitKey) ) {
+ Tracker::do_inhibit = !Tracker::do_inhibit;
+ qDebug() << "Tracker::run() says Inhibit pressed";
+ //
+ // Execute Center command too, when inhibition ends.
+ //
+ if (!Tracker::do_inhibit) {
+ Tracker::do_center = true;
+ }
+ }
+ lastInhibitKey = isShortKeyPressed( &InhibitKey, &keystate[0] ); // Remember
+ }
+ }
+
+ //
+ // Reset the 'wait' flag. Moving above 90 with the key pressed, will (de-)activate Axis Reverse.
+ //
+// qDebug() << "Tracker::run() says actualZ = " << actualZ << ", terwijl Z_Pos4 = " << Z_Pos4ReverseAxis;
+ if (useAxisReverse) {
+ Tracker::do_axis_reverse = ((fabs(actualYaw) > YawAngle4ReverseAxis) && (actualZ < Z_Pos4ReverseAxis));
+ }
+ else {
+ Tracker::do_axis_reverse = false;
+ }
+
+
+ if (WaitForSingleObject(Tracker::hTrackMutex, 100) == WAIT_OBJECT_0) {
+
+ THeadPoseData newpose;
+ newpose.pitch = 0.0f;
+ newpose.roll = 0.0f;
+ newpose.yaw = 0.0f;
+ newpose.x = 0.0f;
+ newpose.y = 0.0f;
+ newpose.z = 0.0f;
+
+ //
+ // The second tracker serves as 'secondary'. So if an axis is written by the second tracker it CAN be overwritten by the Primary tracker.
+ // This is enforced by the sequence below.
+ //
+ if (pSecondTracker) {
+ bTracker2Confid = pSecondTracker->GiveHeadPoseData(&newpose);
+ }
+ else {
+ bTracker2Confid = false;
+ bInitialCenter2 = false;
+ }
+ if (pTracker) {
+ bTracker1Confid = pTracker->GiveHeadPoseData(&newpose);
+// qDebug() << "Tracker::run() says Roll = " << newpose.roll;
+ }
+ else {
+ bTracker1Confid = false;
+ bInitialCenter1 = false;
+ }
+
+ Tracker::confid = (bTracker1Confid || bTracker2Confid);
+ if ( Tracker::confid ) {
+ addHeadPose(newpose);
+ }
+
+ //
+ // If Center is pressed, copy the current values to the offsets.
+ //
+ if ((Tracker::do_center) || ((bInitialCenter1 && bTracker1Confid ) || (bInitialCenter2 && bTracker2Confid))) {
+
+ if (!DisableBeep) {
+ MessageBeep (MB_ICONASTERISK); // Acknowledge the key-press with a beep.
+ }
+ if (pTracker && bTracker1Confid) {
+ pTracker->notifyCenter(); // Send 'center' to the tracker
+ bInitialCenter1 = false;
+ }
+ if (pSecondTracker && bTracker2Confid) {
+ pSecondTracker->notifyCenter(); // Send 'center' to the second tracker
+ bInitialCenter2 = false;
+ }
+
+ //
+ // Only copy valid values
+ //
+ if (Tracker::confid) {
+
+ 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;
+ }
+
+ //
+ // If Set Game Zero is pressed, copy the current values to the offsets.
+ // Change requested by Stanislaw
+ //
+ if (Tracker::confid && Tracker::do_game_zero) {
+ if (pTracker) {
+ if (!pTracker->notifyZeroed())
+ gamezero_camera = gameoutput_camera;
+ }
+// gamezero_camera = gameoutput_camera;
+
+ Tracker::do_game_zero = false;
+ }
+
+ if (Tracker::do_tracking && Tracker::confid) {
+
+ // 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 );
+// qDebug() << "Tracker::run() says Roll from Smoothing = " << target_camera.roll;
+
+ // do the centering
+ target_camera = target_camera - offset_camera;
+
+ //
+ // Use advanced filtering, when a filter was selected.
+ //
+ if (pFilter) {
+ pFilter->FilterHeadPoseData(¤t_camera, &target_camera, &new_camera, Tracker::Pitch.newSample);
+// qDebug() << "Tracker::run() says Roll in Filter = " << current_camera.roll << ", Roll to output = " << new_camera.roll;
+ }
+ else {
+ new_camera = target_camera;
+// qDebug() << "Tracker::run() says Roll to output = " << new_camera.roll;
+ }
+ 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);
+
+ //
+ // Determine, which curve (Up or Down) must be used for Pitch
+ //
+ bool altp = (new_camera.pitch < 0);
+ if (altp) {
+ output_camera.pitch = Pitch.invert * Pitch.curvePtrAlt->getValue(new_camera.pitch);
+ Pitch.curvePtr->setTrackingActive( false );
+ Pitch.curvePtrAlt->setTrackingActive( true );
+ }
+ else {
+ output_camera.pitch = Pitch.invert * Pitch.curvePtr->getValue(new_camera.pitch);
+ Pitch.curvePtr->setTrackingActive( true );
+ Pitch.curvePtrAlt->setTrackingActive( false );
+ }
+ 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 );
+ Y.curvePtr->setTrackingActive( true );
+ Z.curvePtr->setTrackingActive( true );
+ Yaw.curvePtr->setTrackingActive( true );
+ Roll.curvePtr->setTrackingActive( true );
+
+ //
+ // Reverse Axis.
+ //
+ 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.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.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;
+ }
+
+ //
+ // Send the headpose to the game
+ //
+ if (pProtocol) {
+ gameoutput_camera = output_camera + gamezero_camera;
+ pProtocol->sendHeadposeToGame( &gameoutput_camera, &newpose ); // degrees & centimeters
+ }
+ }
+ else {
+ //
+ // Go to initial position
+ //
+ if (pProtocol && setZero) {
+ 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, &newpose ); // degrees & centimeters
+ }
+ X.curvePtr->setTrackingActive( false );
+ Y.curvePtr->setTrackingActive( false );
+ Z.curvePtr->setTrackingActive( false );
+ Yaw.curvePtr->setTrackingActive( false );
+ Pitch.curvePtr->setTrackingActive( false );
+ Pitch.curvePtrAlt->setTrackingActive( false );
+ Roll.curvePtr->setTrackingActive( false );
+ }
+ }
+
+ Tracker::Pitch.newSample = false;
+ ReleaseMutex(Tracker::hTrackMutex);
+
+ //for lower cpu load
+ usleep(10000);
+ yieldCurrentThread();
+ }
+}
+
+/** Add the headpose-data to the Lists **/
+void Tracker::addHeadPose( THeadPoseData head_pose )
+{
+ // Pitch
+ Tracker::Pitch.headPos = head_pose.pitch; // degrees
+ addRaw2List ( &Pitch.rawList, Pitch.maxItems, Tracker::Pitch.headPos );
+// Tracker::Pitch.confidence = head_pose.confidence; // Just this one ...
+ Tracker::Pitch.newSample = true;
+
+ // Yaw
+ Tracker::Yaw.headPos = head_pose.yaw; // degrees
+ addRaw2List ( &Yaw.rawList, Yaw.maxItems, Tracker::Yaw.headPos );
+
+ // Roll
+ Tracker::Roll.headPos = head_pose.roll; // degrees
+ addRaw2List ( &Roll.rawList, Roll.maxItems, Tracker::Roll.headPos );
+
+ // X-position
+ Tracker::X.headPos = head_pose.x; // centimeters
+ addRaw2List ( &X.rawList, X.maxItems, Tracker::X.headPos );
+
+ // Y-position
+ Tracker::Y.headPos = head_pose.y; // centimeters
+ addRaw2List ( &Y.rawList, Y.maxItems, Tracker::Y.headPos );
+
+ // Z-position (distance to camera, absolute!)
+ Tracker::Z.headPos = head_pose.z; // centimeters
+ addRaw2List ( &Z.rawList, Z.maxItems, Tracker::Z.headPos );
+}
+
+//
+// Get the ProgramName from the Game and return it.
+//
+QString Tracker::getGameProgramName() {
+QString str;
+char dest[100];
+
+ str = QString("No protocol active?");
+ if (pProtocol) {
+ pProtocol->getNameFromGame( dest );
+ str = QString( dest );
+ }
+ return str;
+}
+
+//
+// Handle the command, send upstream by the game.
+// Valid values are:
+// 1 = reset Headpose
+//
+bool Tracker::handleGameCommand ( int command ) {
+
+ qDebug() << "handleGameCommand says: Command =" << command;
+
+ switch ( command ) {
+ case 1: // reset headtracker
+ Tracker::do_center = true;
+ break;
+ default:
+ break;
+ }
+ return false;
+}
+
+//
+// Add the new Raw value to the QList.
+// Remove the last item(s), depending on the set maximum list-items.
+//
+void Tracker::addRaw2List ( QList<float> *rawList, float maxIndex, float raw ) {
+ //
+ // Remove old values from the end of the QList.
+ // If the setting for MaxItems was lowered, the QList is shortened here...
+ //
+ while (rawList->size() >= maxIndex) {
+ rawList->removeLast();
+ }
+
+ //
+ // Insert the newest at the beginning.
+ //
+ rawList->prepend ( raw );
+}
+
+//
+// Get the raw headpose, so it can be displayed.
+//
+void Tracker::getHeadPose( THeadPoseData *data ) {
+ data->x = Tracker::X.headPos; // centimeters
+ data->y = Tracker::Y.headPos;
+ data->z = Tracker::Z.headPos;
+
+ data->pitch = Tracker::Pitch.headPos; // degrees
+ data->yaw = Tracker::Yaw.headPos;
+ data->roll = Tracker::Roll.headPos;
+}
+
+//
+// Get the output-headpose, so it can be displayed.
+//
+void Tracker::getOutputHeadPose( THeadPoseData *data ) {
+ data->x = output_camera.x; // centimeters
+ data->y = output_camera.y;
+ data->z = output_camera.z;
+
+ data->pitch = output_camera.pitch; // degrees
+ data->yaw = output_camera.yaw;
+ data->roll = output_camera.roll;
+}
+
+//
+// Get the Smoothed value from the QList.
+//
+float Tracker::getSmoothFromList ( QList<float> *rawList ) {
+float sum = 0;
+
+ if (rawList->isEmpty()) return 0.0f;
+
+ //
+ // Add the Raw values and divide.
+ //
+ for ( int i = 0; i < rawList->size(); i++) {
+ sum += rawList->at(i);
+ }
+ return sum / rawList->size();
+}
+
+//
+// Load the current Settings from the currently 'active' INI-file.
+//
+void Tracker::loadSettings() {
+//int NeutralZone;
+//int sensYaw, sensPitch, sensRoll;
+//int sensX, sensY, sensZ;
+
+ qDebug() << "Tracker::loadSettings says: Starting ";
+ QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER)
+
+ QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString();
+ QSettings iniFile( currentFile, QSettings::IniFormat ); // Application settings (in INI-file)
+
+ qDebug() << "loadSettings says: iniFile = " << currentFile;
+
+ //
+ // Read the Tracking settings, to fill the curves.
+ //
+ //iniFile.beginGroup ( "Tracking" );
+ //NeutralZone = iniFile.value ( "NeutralZone", 5 ).toInt();
+ //sensYaw = iniFile.value ( "sensYaw", 100 ).toInt();
+ //sensPitch = iniFile.value ( "sensPitch", 100 ).toInt();
+ //sensRoll = iniFile.value ( "sensRoll", 100 ).toInt();
+ //sensX = iniFile.value ( "sensX", 100 ).toInt();
+ //sensY = iniFile.value ( "sensY", 100 ).toInt();
+ //sensZ = iniFile.value ( "sensZ", 100 ).toInt();
+ //iniFile.endGroup ();
+
+ //
+ // Read the keyboard shortcuts.
+ //
+ iniFile.beginGroup ( "KB_Shortcuts" );
+
+ // Center key
+ CenterMouseKey = iniFile.value ( "MouseKey_Center", 0 ).toInt();
+ CenterKey.keycode = iniFile.value ( "Keycode_Center", DIK_HOME ).toInt();
+ CenterKey.shift = iniFile.value ( "Shift_Center", 0 ).toBool();
+ CenterKey.ctrl = iniFile.value ( "Ctrl_Center", 0 ).toBool();
+ CenterKey.alt = iniFile.value ( "Alt_Center", 0 ).toBool();
+ DisableBeep = iniFile.value ( "Disable_Beep", 0 ).toBool();
+
+ // StartStop key
+ StartStopMouseKey = iniFile.value ( "MouseKey_StartStop", 0 ).toInt();
+ StartStopKey.keycode = iniFile.value ( "Keycode_StartStop", DIK_END ).toInt();
+ StartStopKey.shift = iniFile.value ( "Shift_StartStop", 0 ).toBool();
+ StartStopKey.ctrl = iniFile.value ( "Ctrl_StartStop", 0 ).toBool();
+ StartStopKey.alt = iniFile.value ( "Alt_StartStop", 0 ).toBool();
+ setZero = iniFile.value ( "SetZero", 1 ).toBool();
+ setEngineStop = iniFile.value ( "SetEngineStop", 1 ).toBool();
+
+ // Inhibit key
+ InhibitMouseKey = iniFile.value ( "MouseKey_Inhibit", 0 ).toInt();
+ InhibitKey.keycode = iniFile.value ( "Keycode_Inhibit", 0 ).toInt();
+ InhibitKey.shift = iniFile.value ( "Shift_Inhibit", 0 ).toBool();
+ InhibitKey.ctrl = iniFile.value ( "Ctrl_Inhibit", 0 ).toBool();
+ InhibitKey.alt = iniFile.value ( "Alt_Inhibit", 0 ).toBool();
+ InhibitKey.doPitch = iniFile.value ( "Inhibit_Pitch", 0 ).toBool();
+ InhibitKey.doYaw = iniFile.value ( "Inhibit_Yaw", 0 ).toBool();
+ InhibitKey.doRoll = iniFile.value ( "Inhibit_Roll", 0 ).toBool();
+ InhibitKey.doX = iniFile.value ( "Inhibit_X", 0 ).toBool();
+ InhibitKey.doY = iniFile.value ( "Inhibit_Y", 0 ).toBool();
+ InhibitKey.doZ = iniFile.value ( "Inhibit_Z", 0 ).toBool();
+
+ // Game Zero key
+ GameZeroMouseKey = iniFile.value ( "MouseKey_GameZero", 0 ).toInt();
+ GameZeroKey.keycode = iniFile.value ( "Keycode_GameZero", 0 ).toInt();
+ GameZeroKey.shift = iniFile.value ( "Shift_GameZero", 0 ).toBool();
+ GameZeroKey.ctrl = iniFile.value ( "Ctrl_GameZero", 0 ).toBool();
+ GameZeroKey.alt = iniFile.value ( "Alt_GameZero", 0 ).toBool();
+
+ // Axis Reverse key
+ //AxisReverseKey.keycode = DIK_R;
+ //AxisReverseKey.shift = false;
+ //AxisReverseKey.ctrl = false;
+ //AxisReverseKey.alt = false;
+
+ // Reverse Axis
+ useAxisReverse = iniFile.value ( "Enable_ReverseAxis", 0 ).toBool();
+ YawAngle4ReverseAxis = iniFile.value ( "RA_Yaw", 40 ).toInt();
+ Z_Pos4ReverseAxis = iniFile.value ( "RA_ZPos", 50 ).toInt();
+ Z_PosWhenReverseAxis = iniFile.value ( "RA_ToZPos", 80 ).toInt();
+
+ iniFile.endGroup ();
+}
+
+//
+// Determine if the ShortKey (incl. CTRL, SHIFT and/or ALT) is pressed.
+//
+bool Tracker::isShortKeyPressed( TShortKey *key, BYTE *keystate ){
+bool shift;
+bool ctrl;
+bool alt;
+
+ //
+ // First, check if the right key is pressed. If so, check the modifiers
+ //
+ if (keystate[key->keycode] & 0x80) {
+ shift = ( (keystate[DIK_LSHIFT] & 0x80) || (keystate[DIK_RSHIFT] & 0x80) );
+ ctrl = ( (keystate[DIK_LCONTROL] & 0x80) || (keystate[DIK_RCONTROL] & 0x80) );
+ alt = ( (keystate[DIK_LALT] & 0x80) || (keystate[DIK_RALT] & 0x80) );
+
+ //
+ // If one of the modifiers is needed and not pressed, return false.
+ //
+ if (key->shift && !shift) return false;
+ if (key->ctrl && !ctrl) return false;
+ if (key->alt && !alt) return false;
+
+ //
+ // All is well!
+ //
+ return true;
+ }
+ else {
+ return false;
+ }
+}
+
+//
+// Determine if the MouseKey is pressed.
+//
+bool Tracker::isMouseKeyPressed( int *key, DIMOUSESTATE *mousestate ){
+
+ //
+ // If key == NONE, or invalid: ready!
+ //
+ if ((*key <= 0) || (*key > 5)) {
+ return false;
+ }
+
+ //
+ // Now, check if the right key is pressed.
+ //
+ if (mousestate->rgbButtons[*key-1] & 0x80) {
+ return true;
+ }
+ else {
+ return false;
+ }
+}
|