From 351e844f6a3c5484acfcf4fb0154bcab6f1780a0 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 13 Apr 2013 05:45:07 +0200 Subject: Don't use axes as class/struct members. Use an array instead, and iterate over them where applicable. --- facetracknoir/facetracknoir.cpp | 231 ++++++++++------------ facetracknoir/facetracknoir.h | 7 +- facetracknoir/tracker.cpp | 184 ++++++----------- facetracknoir/tracker.h | 47 +++-- facetracknoir/tracker_types.cpp | 36 ++-- facetracknoir/tracker_types.h | 8 +- ftnoir_filter_accela/ftnoir_filter_accela.cpp | 60 ++---- ftnoir_filter_accela/ftnoir_filter_accela.h | 4 +- ftnoir_filter_base/ftnoir_filter_base.h | 2 +- ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp | 91 +++------ ftnoir_filter_ewma2/ftnoir_filter_ewma2.h | 22 +-- ftnoir_protocol_base/ftnoir_protocol_base.h | 2 +- ftnoir_protocol_fg/ftnoir_protocol_fg.cpp | 19 +- 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_ftn/ftnoir_protocol_ftn.cpp | 14 +- ftnoir_protocol_ftn/ftnoir_protocol_ftn.h | 2 +- ftnoir_protocol_mouse/ftnoir_protocol_mouse.cpp | 106 ++-------- ftnoir_protocol_mouse/ftnoir_protocol_mouse.h | 14 +- ftnoir_protocol_sc/ftnoir_protocol_sc.cpp | 14 +- ftnoir_protocol_sc/ftnoir_protocol_sc.h | 2 +- ftnoir_tracker_base/ftnoir_tracker_base.h | 2 +- ftnoir_tracker_base/ftnoir_tracker_types.h | 18 +- ftnoir_tracker_ht/ftnoir_tracker_ht.cpp | 26 +-- ftnoir_tracker_ht/ftnoir_tracker_ht.h | 2 +- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 14 +- ftnoir_tracker_pt/ftnoir_tracker_pt.h | 2 +- ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp | 14 +- ftnoir_tracker_sm/ftnoir_tracker_sm.h | 2 +- ftnoir_tracker_udp/ftnoir_tracker_udp.cpp | 22 +-- ftnoir_tracker_udp/ftnoir_tracker_udp.h | 4 +- 34 files changed, 381 insertions(+), 634 deletions(-) diff --git a/facetracknoir/facetracknoir.cpp b/facetracknoir/facetracknoir.cpp index fd971c96..d5d24db1 100644 --- a/facetracknoir/facetracknoir.cpp +++ b/facetracknoir/facetracknoir.cpp @@ -743,26 +743,26 @@ void FaceTrackNoIR::startTracker( ) { 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) - GlobalPose->X.curvePtr->loadSettings(iniFile); - GlobalPose->Y.curvePtr->loadSettings(iniFile); - GlobalPose->Z.curvePtr->loadSettings(iniFile); - GlobalPose->Yaw.curvePtr->loadSettings(iniFile); - GlobalPose->Pitch.curvePtr->loadSettings(iniFile); - GlobalPose->Roll.curvePtr->loadSettings(iniFile); - - GlobalPose->X.curvePtrAlt->loadSettings(iniFile); - GlobalPose->Y.curvePtrAlt->loadSettings(iniFile); - GlobalPose->Z.curvePtrAlt->loadSettings(iniFile); - GlobalPose->Yaw.curvePtrAlt->loadSettings(iniFile); - GlobalPose->Pitch.curvePtrAlt->loadSettings(iniFile); - GlobalPose->Roll.curvePtrAlt->loadSettings(iniFile); - - GlobalPose->Yaw.altp = iniFile.value("rx_alt", false).toBool(); - GlobalPose->Pitch.altp = iniFile.value("ry_alt", false).toBool(); - GlobalPose->Roll.altp = iniFile.value("rz_alt", false).toBool(); - GlobalPose->X.altp = iniFile.value("tx_alt", false).toBool(); - GlobalPose->Y.altp = iniFile.value("ty_alt", false).toBool(); - GlobalPose->Z.altp = iniFile.value("tz_alt", false).toBool(); + + for (int i = 0; i < 6; i++) + { + GlobalPose->axes[i].curvePtr->loadSettings(iniFile); + GlobalPose->axes[i].curvePtrAlt->loadSettings(iniFile); + } + + static const char* names[] = { + "rx_alt", + "ry_alt", + "rz_alt", + "tx_alt", + "ty_alt", + "tz_alt" + }; + + for (int i = 0; i < 6; i++) + { + GlobalPose->axes[i].altp = iniFile.value(names[i], false).toBool(); + } tracker = new Tracker ( this ); @@ -770,12 +770,12 @@ void FaceTrackNoIR::startTracker( ) { // Setup the Tracker and send the settings. // This is necessary, because the events are only triggered 'on change' // - tracker->setInvertYaw (ui.chkInvertYaw->isChecked() ); - tracker->setInvertPitch (ui.chkInvertPitch->isChecked() ); - tracker->setInvertRoll (ui.chkInvertRoll->isChecked() ); - tracker->setInvertX (ui.chkInvertX->isChecked() ); - tracker->setInvertY (ui.chkInvertY->isChecked() ); - tracker->setInvertZ (ui.chkInvertZ->isChecked() ); + tracker->setInvertAxis(RX, ui.chkInvertYaw->isChecked() ); + tracker->setInvertAxis(TY, ui.chkInvertPitch->isChecked() ); + tracker->setInvertAxis(RZ, ui.chkInvertRoll->isChecked() ); + tracker->setInvertAxis(TX, ui.chkInvertX->isChecked() ); + tracker->setInvertAxis(TY, ui.chkInvertY->isChecked() ); + tracker->setInvertAxis(TZ, ui.chkInvertZ->isChecked() ); tracker->start(); @@ -930,50 +930,15 @@ void FaceTrackNoIR::stopTracker( ) { } /** set the invert from the checkbox **/ -void FaceTrackNoIR::setInvertYaw( int invert ) { - if (tracker) - tracker->setInvertYaw ( (invert != 0)?true:false ); - settingsDirty = true; -} - -/** set the invert from the checkbox **/ -void FaceTrackNoIR::setInvertPitch( int invert ) { - if (tracker) - tracker->setInvertPitch ( (invert != 0)?true:false ); - settingsDirty = true; -} - -/** set the invert from the checkbox **/ -void FaceTrackNoIR::setInvertRoll( int invert ) { +void FaceTrackNoIR::setInvertAxis(Axis axis, int invert ) { if (tracker) - tracker->setInvertRoll ( (invert != 0)?true:false ); - settingsDirty = true; -} - -/** set the invert from the checkbox **/ -void FaceTrackNoIR::setInvertX( int invert ) { - if (tracker) - tracker->setInvertX ( (invert != 0)?true:false ); - settingsDirty = true; -} - -/** set the invert from the checkbox **/ -void FaceTrackNoIR::setInvertY( int invert ) { - if (tracker) - tracker->setInvertY ( (invert != 0)?true:false ); - settingsDirty = true; -} - -/** set the invert from the checkbox **/ -void FaceTrackNoIR::setInvertZ( int invert ) { - if (tracker) - tracker->setInvertZ ( (invert != 0)?true:false ); + tracker->setInvertAxis (axis, (invert != 0)?true:false ); settingsDirty = true; } /** Show the headpose in the widget (triggered by timer) **/ void FaceTrackNoIR::showHeadPose() { -THeadPoseData newdata; + double newdata[6]; ui.lblX->setVisible(true); ui.lblY->setVisible(true); @@ -993,14 +958,14 @@ THeadPoseData newdata; // Get the pose and also display it. // Updating the pose from within the Tracker-class caused crashes... // - tracker->getHeadPose(&newdata); - ui.lcdNumX->display(QString("%1").arg(newdata.x, 0, 'f', 1)); - ui.lcdNumY->display(QString("%1").arg(newdata.y, 0, 'f', 1)); - ui.lcdNumZ->display(QString("%1").arg(newdata.z, 0, 'f', 1)); + tracker->getHeadPose(newdata); + ui.lcdNumX->display(QString("%1").arg(newdata[TX], 0, 'f', 1)); + ui.lcdNumY->display(QString("%1").arg(newdata[TY], 0, 'f', 1)); + ui.lcdNumZ->display(QString("%1").arg(newdata[TZ], 0, 'f', 1)); - ui.lcdNumRotX->display(QString("%1").arg(newdata.yaw, 0, 'f', 1)); - ui.lcdNumRotY->display(QString("%1").arg(newdata.pitch, 0, 'f', 1)); - ui.lcdNumRotZ->display(QString("%1").arg(newdata.roll, 0, 'f', 1)); + ui.lcdNumRotX->display(QString("%1").arg(newdata[RX], 0, 'f', 1)); + ui.lcdNumRotY->display(QString("%1").arg(newdata[RY], 0, 'f', 1)); + ui.lcdNumRotZ->display(QString("%1").arg(newdata[RZ], 0, 'f', 1)); ui.txtTracking->setVisible(tracker->getTrackingActive()); ui.txtAxisReverse->setVisible(tracker->getAxisReverse()); @@ -1009,16 +974,16 @@ THeadPoseData newdata; // Get the output-pose and also display it. // if (_pose_display) { - tracker->getOutputHeadPose(&newdata); - _pose_display->rotateBy(newdata.pitch, newdata.yaw, newdata.roll); + tracker->getOutputHeadPose(newdata); + _pose_display->rotateBy(newdata[RY], newdata[RX], newdata[RZ]); - ui.lcdNumOutputPosX->display(QString("%1").arg(newdata.x, 0, 'f', 1)); - ui.lcdNumOutputPosY->display(QString("%1").arg(newdata.y, 0, 'f', 1)); - ui.lcdNumOutputPosZ->display(QString("%1").arg(newdata.z, 0, 'f', 1)); + ui.lcdNumOutputPosX->display(QString("%1").arg(newdata[TX], 0, 'f', 1)); + ui.lcdNumOutputPosY->display(QString("%1").arg(newdata[TY], 0, 'f', 1)); + ui.lcdNumOutputPosZ->display(QString("%1").arg(newdata[TZ], 0, 'f', 1)); - ui.lcdNumOutputRotX->display(QString("%1").arg(newdata.yaw, 0, 'f', 1)); - ui.lcdNumOutputRotY->display(QString("%1").arg(newdata.pitch, 0, 'f', 1)); - ui.lcdNumOutputRotZ->display(QString("%1").arg(newdata.roll, 0, 'f', 1)); + ui.lcdNumOutputRotX->display(QString("%1").arg(newdata[RX], 0, 'f', 1)); + ui.lcdNumOutputRotY->display(QString("%1").arg(newdata[RY], 0, 'f', 1)); + ui.lcdNumOutputRotZ->display(QString("%1").arg(newdata[RZ], 0, 'f', 1)); } } @@ -1866,45 +1831,46 @@ QWidget( parent , f) QSettings settings("Abbequerque Inc.", "FaceTrackNoIR"); // Registry settings (in HK_USER) QString currentFile = settings.value ( "SettingsFile", QCoreApplication::applicationDirPath() + "/Settings/default.ini" ).toString(); - ui.txconfig->setConfig(GlobalPose->X.curvePtr, currentFile); - ui.tyconfig->setConfig(GlobalPose->Y.curvePtr, currentFile); - ui.tzconfig->setConfig(GlobalPose->Z.curvePtr, currentFile); - ui.rxconfig->setConfig(GlobalPose->Yaw.curvePtr, currentFile); - ui.ryconfig->setConfig(GlobalPose->Pitch.curvePtr, currentFile); - ui.rzconfig->setConfig(GlobalPose->Roll.curvePtr, currentFile); - ui.txconfig_alt->setConfig(GlobalPose->X.curvePtrAlt, currentFile); - ui.tyconfig_alt->setConfig(GlobalPose->Y.curvePtrAlt, currentFile); - ui.tzconfig_alt->setConfig(GlobalPose->Z.curvePtrAlt, currentFile); - ui.rxconfig_alt->setConfig(GlobalPose->Yaw.curvePtrAlt, currentFile); - ui.ryconfig_alt->setConfig(GlobalPose->Pitch.curvePtrAlt, currentFile); - ui.rzconfig_alt->setConfig(GlobalPose->Roll.curvePtrAlt, currentFile); + QFunctionConfigurator* configs[6] = { + ui.txconfig, + ui.tyconfig, + ui.tzconfig, + ui.rxconfig, + ui.ryconfig, + ui.rzconfig + }; + + QFunctionConfigurator* alt_configs[6] = { + ui.txconfig_alt, + ui.tyconfig_alt, + ui.tzconfig_alt, + ui.rxconfig_alt, + ui.ryconfig_alt, + ui.rzconfig_alt + }; + + QCheckBox* checkboxes[6] = { + ui.rx_altp, + ui.ry_altp, + ui.rz_altp, + ui.tx_altp, + ui.ty_altp, + ui.tz_altp + }; + + for (int i = 0; i < 6; i++) + { + configs[i]->setConfig(GlobalPose->axes[i].curvePtr, currentFile); + alt_configs[i]->setConfig(GlobalPose->axes[i].curvePtrAlt, currentFile); + connect(configs[i], SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); + connect(alt_configs[i], SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); + connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int))); + } // Connect Qt signals to member-functions connect(ui.btnOK, SIGNAL(clicked()), this, SLOT(doOK())); connect(ui.btnCancel, SIGNAL(clicked()), this, SLOT(doCancel())); - connect(ui.txconfig, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.txconfig, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - - - connect(ui.tyconfig, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.tzconfig, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.rxconfig, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.ryconfig, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.rzconfig, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.txconfig_alt, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.tyconfig_alt, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.tzconfig_alt, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.rxconfig_alt, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.ryconfig_alt, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.rzconfig_alt, SIGNAL(CurveChanged(bool)), this, SLOT(curveChanged(bool))); - connect(ui.rx_altp, SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int))); - connect(ui.ry_altp, SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int))); - connect(ui.rz_altp, SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int))); - connect(ui.tx_altp, SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int))); - connect(ui.ty_altp, SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int))); - connect(ui.tz_altp, SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int))); - // Load the settings from the current .INI-file loadSettings(); } @@ -1974,22 +1940,31 @@ void CurveConfigurationDialog::loadSettings() { qDebug() << "loadSettings says: iniFile = " << currentFile; - GlobalPose->Yaw.altp = iniFile.value("rx_alt", false).toBool(); - GlobalPose->Pitch.altp = iniFile.value("ry_alt", false).toBool(); - GlobalPose->Roll.altp = iniFile.value("rz_alt", false).toBool(); - GlobalPose->X.altp = iniFile.value("tx_alt", false).toBool(); - GlobalPose->Y.altp = iniFile.value("ty_alt", false).toBool(); - GlobalPose->Z.altp = iniFile.value("tz_alt", false).toBool(); - - ui.rx_altp->setChecked(GlobalPose->Yaw.altp); - ui.ry_altp->setChecked(GlobalPose->Pitch.altp); - ui.rz_altp->setChecked(GlobalPose->Roll.altp); - ui.tx_altp->setChecked(GlobalPose->X.altp); - ui.ty_altp->setChecked(GlobalPose->Y.altp); - ui.tz_altp->setChecked(GlobalPose->Z.altp); - - settingsDirty = false; + static const char* names[] = { + "rx_alt", + "ry_alt", + "rz_alt", + "tx_alt", + "ty_alt", + "tz_alt" + }; + + for (int i = 0; i < 6; i++) + GlobalPose->axes[i].altp = iniFile.value(names[i], false).toBool(); + + static QCheckBox* widgets[] = { + ui.rx_altp, + ui.ry_altp, + ui.rz_altp, + ui.tx_altp, + ui.ty_altp, + ui.tz_altp + }; + + for (int i = 0; i < 6; i++) + widgets[i]->setChecked(GlobalPose->axes[i].altp); + settingsDirty = false; } // diff --git a/facetracknoir/facetracknoir.h b/facetracknoir/facetracknoir.h index e59c2190..40044ca5 100644 --- a/facetracknoir/facetracknoir.h +++ b/facetracknoir/facetracknoir.h @@ -211,12 +211,7 @@ private: void showKeyboardShortcuts(); void showCurveConfiguration(); - void setInvertYaw( int invert ); - void setInvertPitch( int invert ); - void setInvertRoll( int invert ); - void setInvertX( int invert ); - void setInvertY( int invert ); - void setInvertZ( int invert ); + void setInvertAxis( Axis axis, int invert ); void showHeadPose(); diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp index ed39765d..7f1201cc 100644 --- a/facetracknoir/tracker.cpp +++ b/facetracknoir/tracker.cpp @@ -80,26 +80,18 @@ Tracker::Tracker( FaceTrackNoIR *parent ) : should_quit(false), do_tracking(true), do_center(false), - do_inhibit(false), do_game_zero(false), - do_axis_reverse(false), - inhibit_rx(false), - inhibit_ry(false), - inhibit_rz(false), - inhibit_tx(false), - inhibit_ty(false), - inhibit_tz(false) + do_axis_reverse(false) { // Retieve the pointer to the parent mainApp = parent; // Load the settings from the INI-file loadSettings(); - GlobalPose->Yaw.headPos = 0; - GlobalPose->Pitch.headPos = 0; - GlobalPose->Roll.headPos = 0; - GlobalPose->X.headPos = 0; - GlobalPose->Y.headPos = 0; - GlobalPose->Z.headPos = 0; + for (int i = 0; i < 6; i++) + { + GlobalPose->axes[i].headPos = 0; + inhibit[i] = false; + } } Tracker::~Tracker() @@ -142,53 +134,34 @@ void Tracker::run() { bool bTracker1Confid = false; bool bTracker2Confid = false; - THeadPoseData last; - THeadPoseData newpose; - THeadPoseData last_post_filter; + double newpose[6]; + double last_post_filter[6]; forever { if (should_quit) break; - newpose.pitch = 0.0f; - newpose.roll = 0.0f; - newpose.yaw = 0.0f; - newpose.x = 0.0f; - newpose.y = 0.0f; - newpose.z = 0.0f; + for (int i = 0; i < 6; i++) + newpose[i] = 0; // // 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 (Libraries->pSecondTracker) { - bTracker2Confid = Libraries->pSecondTracker->GiveHeadPoseData(&newpose); + bTracker2Confid = Libraries->pSecondTracker->GiveHeadPoseData(newpose); } if (Libraries->pTracker) { - bTracker1Confid = Libraries->pTracker->GiveHeadPoseData(&newpose); + bTracker1Confid = Libraries->pTracker->GiveHeadPoseData(newpose); } confid = (bTracker1Confid || bTracker2Confid); - - bool newp = last.yaw != newpose.yaw || - last.pitch != newpose.pitch || - last.roll != newpose.roll || - last.x != newpose.x || - last.y != newpose.y || - last.z != newpose.z; - - if (newp) - last = newpose; - + if ( confid ) { - GlobalPose->Yaw.headPos = newpose.yaw; - GlobalPose->Pitch.headPos = newpose.pitch; - GlobalPose->Roll.headPos = newpose.roll; - GlobalPose->X.headPos = newpose.x; - GlobalPose->Y.headPos = newpose.y; - GlobalPose->Z.headPos = newpose.z; + for (int i = 0; i < 6; i++) + GlobalPose->axes[i].headPos = newpose[i]; } // @@ -199,12 +172,8 @@ void Tracker::run() { // Only copy valid values // if (confid) { - offset_camera.x = GlobalPose->X.headPos; - offset_camera.y = GlobalPose->Y.headPos; - offset_camera.z = GlobalPose->Z.headPos; - offset_camera.pitch = GlobalPose->Pitch.headPos; - offset_camera.yaw = GlobalPose->Yaw.headPos; - offset_camera.roll = GlobalPose->Roll.headPos; + for (int i = 0; i < 6; i++) + offset_camera.axes[i] = GlobalPose->axes[i].headPos; } Tracker::do_center = false; @@ -212,8 +181,6 @@ void Tracker::run() { // for kalman if (Libraries->pFilter) Libraries->pFilter->Initialize(); - - last = newpose; } if (do_game_zero) { @@ -223,12 +190,8 @@ void Tracker::run() { if (do_tracking && confid) { // get values - target_camera.x = GlobalPose->X.headPos; - target_camera.y = GlobalPose->Y.headPos; - target_camera.z = GlobalPose->Z.headPos; - target_camera.pitch = GlobalPose->Pitch.headPos; - target_camera.yaw = GlobalPose->Yaw.headPos; - target_camera.roll = GlobalPose->Roll.headPos; + for (int i = 0; i < 6; i++) + target_camera.axes[i] = GlobalPose->axes[i].headPos; // do the centering target_camera = target_camera - offset_camera; @@ -237,22 +200,19 @@ void Tracker::run() { // Use advanced filtering, when a filter was selected. // if (Libraries->pFilter) { - last_post_filter = gameoutput_camera; - Libraries->pFilter->FilterHeadPoseData(¤t_camera, &target_camera, &new_camera, &last_post_filter, newp); + for (int i = 0; i < 6; i++) + last_post_filter[i] = gameoutput_camera.axes[i]; + Libraries->pFilter->FilterHeadPoseData(current_camera.axes, target_camera.axes, new_camera.axes, last_post_filter); } else { new_camera = target_camera; } - get_curve(do_inhibit && inhibit_rx, inhibit_zero, new_camera.yaw, output_camera.yaw, GlobalPose->Yaw); - get_curve(do_inhibit && inhibit_ry, inhibit_zero, new_camera.pitch, output_camera.pitch, GlobalPose->Pitch); - get_curve(do_inhibit && inhibit_rz, inhibit_zero, new_camera.roll, output_camera.roll, GlobalPose->Roll); - get_curve(do_inhibit && inhibit_tx, inhibit_zero, new_camera.x, output_camera.x, GlobalPose->X); - get_curve(do_inhibit && inhibit_ty, inhibit_zero, new_camera.y, output_camera.y, GlobalPose->Y); - get_curve(do_inhibit && inhibit_tz, inhibit_zero, new_camera.z, output_camera.z, GlobalPose->Z); + for (int i = 0; i < 6; i++) + get_curve(do_inhibit && inhibit[i], inhibit_zero, new_camera.axes[i], output_camera.axes[i], GlobalPose->axes[i]); if (useAxisReverse) { - do_axis_reverse = ((fabs(output_camera.yaw) > YawAngle4ReverseAxis) && (output_camera.z < Z_Pos4ReverseAxis)); + do_axis_reverse = ((fabs(output_camera.axes[RX]) > YawAngle4ReverseAxis) && (output_camera.axes[TZ] < Z_Pos4ReverseAxis)); } else { do_axis_reverse = false; } @@ -261,7 +221,7 @@ void Tracker::run() { // Reverse Axis. // if (do_axis_reverse) { - output_camera.z = Z_PosWhenReverseAxis; // Set the desired Z-position + output_camera.axes[TZ] = Z_PosWhenReverseAxis; // Set the desired Z-position } // @@ -269,7 +229,7 @@ void Tracker::run() { // if (Libraries->pProtocol) { gameoutput_camera = output_camera + gamezero_camera; - Libraries->pProtocol->sendHeadposeToGame( &gameoutput_camera, &newpose ); // degrees & centimeters + Libraries->pProtocol->sendHeadposeToGame( gameoutput_camera.axes, newpose ); // degrees & centimeters } } else { @@ -277,27 +237,16 @@ void Tracker::run() { // Go to initial position // if (Libraries->pProtocol && inhibit_zero) { - 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; + for (int i = 0; i < 6; i++) + output_camera.axes[i] = 0; gameoutput_camera = output_camera + gamezero_camera; - Libraries->pProtocol->sendHeadposeToGame( &gameoutput_camera, &newpose ); // degrees & centimeters + Libraries->pProtocol->sendHeadposeToGame( gameoutput_camera.axes, newpose ); // degrees & centimeters + } + for (int i = 0; i < 6; i++) + { + GlobalPose->axes[i].curvePtr->setTrackingActive(false); + GlobalPose->axes[i].curvePtrAlt->setTrackingActive(false); } - GlobalPose->X.curvePtr->setTrackingActive( false ); - GlobalPose->Y.curvePtr->setTrackingActive( false ); - GlobalPose->Z.curvePtr->setTrackingActive( false ); - GlobalPose->Yaw.curvePtr->setTrackingActive( false ); - GlobalPose->Pitch.curvePtr->setTrackingActive( false ); - GlobalPose->Roll.curvePtr->setTrackingActive( false ); - GlobalPose->X.curvePtrAlt->setTrackingActive( false ); - GlobalPose->Y.curvePtrAlt->setTrackingActive( false ); - GlobalPose->Z.curvePtrAlt->setTrackingActive( false ); - GlobalPose->Yaw.curvePtrAlt->setTrackingActive( false ); - GlobalPose->Pitch.curvePtrAlt->setTrackingActive( false ); - GlobalPose->Roll.curvePtrAlt->setTrackingActive( false ); if (Libraries->pFilter) Libraries->pFilter->Initialize(); } @@ -306,13 +255,11 @@ void Tracker::run() { usleep(1000); } - GlobalPose->X.curvePtr->setTrackingActive( false ); - GlobalPose->Y.curvePtr->setTrackingActive( false ); - GlobalPose->Z.curvePtr->setTrackingActive( false ); - GlobalPose->Yaw.curvePtr->setTrackingActive( false ); - GlobalPose->Pitch.curvePtr->setTrackingActive( false ); - GlobalPose->Pitch.curvePtrAlt->setTrackingActive( false ); - GlobalPose->Roll.curvePtr->setTrackingActive( false ); + for (int i = 0; i < 6; i++) + { + GlobalPose->axes[i].curvePtr->setTrackingActive(false); + GlobalPose->axes[i].curvePtrAlt->setTrackingActive(false); + } } // @@ -352,27 +299,19 @@ bool Tracker::handleGameCommand ( int command ) { // // Get the raw headpose, so it can be displayed. // -void Tracker::getHeadPose( THeadPoseData *data ) { - data->x = GlobalPose->X.headPos; // centimeters - data->y = GlobalPose->Y.headPos; - data->z = GlobalPose->Z.headPos; - - data->pitch = GlobalPose->Pitch.headPos; // degrees - data->yaw = GlobalPose->Yaw.headPos; - data->roll = GlobalPose->Roll.headPos; +void Tracker::getHeadPose( double *data ) { + for (int i = 0; i < 6; i++) + { + data[i] = GlobalPose->axes[i].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; +void Tracker::getOutputHeadPose( double *data ) { + for (int i = 0; i < 6; i++) + data[i] = output_camera.axes[i]; } // @@ -395,19 +334,22 @@ void Tracker::loadSettings() { YawAngle4ReverseAxis = iniFile.value ( "RA_Yaw", 40 ).toInt(); Z_Pos4ReverseAxis = iniFile.value ( "RA_ZPos", 50 ).toInt(); Z_PosWhenReverseAxis = iniFile.value ( "RA_ToZPos", 80 ).toInt(); - inhibit_rx = iniFile.value("Inhibit_Yaw", false).toBool(); - inhibit_ry = iniFile.value("Inhibit_Pitch", false).toBool(); - inhibit_rz = iniFile.value("Inhibit_Roll", false).toBool(); - inhibit_tx = iniFile.value("Inhibit_X", false).toBool(); - inhibit_ty = iniFile.value("Inhibit_Y", false).toBool(); - inhibit_tz = iniFile.value("Inhibit_Z", false).toBool(); + + static const char* names[] = { + "Inhibit_Yaw", + "Inhibit_Pitch", + "Inhibit_Roll", + "Inhibit_X", + "Inhibit_Y", + "Inhibit_Z" + }; + + for (int i = 0; i < 6; i++) + { + inhibit[i] = iniFile.value(names[i], false).toBool(); + } inhibit_zero = iniFile.value("SetZero", false).toBool(); iniFile.endGroup (); } -void Tracker::setInvertPitch(bool invert) { GlobalPose->Pitch.invert = invert?-1.0f:1.0f; } -void Tracker::setInvertYaw(bool invert) { GlobalPose->Yaw.invert = invert?-1.0f:+1.0f; } -void Tracker::setInvertRoll(bool invert) { GlobalPose->Roll.invert = invert?-1.0f:+1.0f; } -void Tracker::setInvertX(bool invert) { GlobalPose->X.invert = invert?-1.0f:+1.0f; } -void Tracker::setInvertY(bool invert) { GlobalPose->Y.invert = invert?-1.0f:+1.0f; } -void Tracker::setInvertZ(bool invert) { GlobalPose->Z.invert = invert?-1.0f:+1.0f; } +void Tracker::setInvertAxis(Axis axis, bool invert) { GlobalPose->axes[axis].invert = invert?-1.0f:1.0f; } diff --git a/facetracknoir/tracker.h b/facetracknoir/tracker.h index e413c8d7..5dc48f60 100644 --- a/facetracknoir/tracker.h +++ b/facetracknoir/tracker.h @@ -37,6 +37,7 @@ #include #include #include "global-settings.h" +#include //#define DIRECTINPUT_VERSION 0x0800 //#include @@ -101,13 +102,17 @@ extern HeadPoseData* GlobalPose; // class THeadPoseDOF { public: - THeadPoseDOF(QString primary, QString secondary, int maxInput1 = 50, int maxOutput1 = 180, int maxInput2 = 50, int maxOutput2 = 90) { + THeadPoseDOF(QString primary = "", QString secondary= "", int maxInput1 = 50, int maxOutput1 = 180, int maxInput2 = 50, int maxOutput2 = 90) { 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) - curvePtr = new FunctionConfig(primary, maxInput1, maxOutput1); // Create the Function-config for input-output translation - curvePtr->loadSettings(iniFile); // Load the settings from the INI-file + if (primary != "") + { + curvePtr = new FunctionConfig(primary, maxInput1, maxOutput1); // Create the Function-config for input-output translation + curvePtr->loadSettings(iniFile); // Load the settings from the INI-file + } + if (secondary != "") { curvePtrAlt = new FunctionConfig(secondary, maxInput2, maxOutput2); @@ -133,7 +138,9 @@ private: float Z_Pos4ReverseAxis; float Z_PosWhenReverseAxis; - volatile bool inhibit_rx, inhibit_ry, inhibit_rz, inhibit_tx, inhibit_ty, inhibit_tz, inhibit_zero; + + volatile bool inhibit[6]; + volatile bool inhibit_zero; FaceTrackNoIR *mainApp; @@ -157,15 +164,10 @@ public: bool getConfid() { return confid; } - void setInvertPitch(bool invert); - void setInvertYaw(bool invert); - void setInvertRoll(bool invert); - void setInvertX(bool invert); - void setInvertY(bool invert); - void setInvertZ(bool invert); + void setInvertAxis(Axis axis, bool invert); - void getHeadPose(THeadPoseData *data); // Return the current headpose data - void getOutputHeadPose(THeadPoseData *data); // Return the current (processed) headpose data + void getHeadPose(double *data); // Return the current headpose data + void getOutputHeadPose(double *data); // Return the current (processed) headpose data float getDegreesFromRads ( float rads ) { return (rads * 57.295781f); } float getRadsFromDegrees ( float degrees ) { return (degrees * 0.017453f); } @@ -185,20 +187,15 @@ public: struct HeadPoseData { public: - THeadPoseDOF Pitch; - THeadPoseDOF Yaw; - THeadPoseDOF Roll; - THeadPoseDOF X; - THeadPoseDOF Y; - THeadPoseDOF Z; - HeadPoseData() : - Pitch("ry", "ry_alt", 60, 180, 60, 90), - Yaw("rx", "rx_alt", 60, 180, 60, 180), - Roll("rz", "rz_alt", 60, 180, 60, 180), - X("tx","tx_alt", 60, 200, 60, 200), - Y("ty","ty_alt", 60, 200, 60, 200), - Z("tz","tz_alt", 60, 200, 60, 200) + THeadPoseDOF axes[6]; + HeadPoseData() { + axes[TX] = THeadPoseDOF("tx","tx_alt", 60, 200, 60, 200); + axes[TY] = THeadPoseDOF("ty","ty_alt", 60, 200, 60, 200); + axes[TZ] = THeadPoseDOF("tz","tz_alt", 60, 200, 60, 200); + axes[RX] = THeadPoseDOF("rx", "rx_alt", 60, 180, 60, 180); + axes[RY] = THeadPoseDOF("ry", "ry_alt", 60, 180, 60, 90); + axes[RZ] = THeadPoseDOF("rz", "rz_alt", 60, 180, 60, 180); } }; diff --git a/facetracknoir/tracker_types.cpp b/facetracknoir/tracker_types.cpp index 5a6126ff..82109952 100644 --- a/facetracknoir/tracker_types.cpp +++ b/facetracknoir/tracker_types.cpp @@ -7,38 +7,38 @@ 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_A(A.axes[RX]*D2R, A.axes[RY]*D2R, A.axes[RZ]*D2R); + Rotation R_B(B.axes[RX]*D2R, B.axes[RY]*D2R, B.axes[RZ]*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; + R_C.toEuler(C.axes[RX], C.axes[RY], C.axes[RZ]); + C.axes[RX] *= R2D; + C.axes[RY] *= R2D; + C.axes[RZ] *= R2D; - C.x = A.x - B.x; - C.y = A.y - B.y; - C.z = A.z - B.z; + C.axes[TX] = A.axes[TX] + B.axes[TX]; + C.axes[TY] = A.axes[TY] + B.axes[TY]; + C.axes[TZ] = A.axes[TZ] + B.axes[TZ]; //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_A(A.axes[RX]*D2R, A.axes[RY]*D2R, A.axes[RZ]*D2R); + Rotation R_B(B.axes[RX]*D2R, B.axes[RY]*D2R, B.axes[RZ]*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; + R_C.toEuler(C.axes[RX], C.axes[RY], C.axes[RZ]); + C.axes[RX] *= R2D; + C.axes[RY] *= R2D; + C.axes[RZ] *= R2D; - C.x = A.x + B.x; - C.y = A.y + B.y; - C.z = A.z + B.z; + C.axes[TX] = A.axes[TX] + B.axes[TX]; + C.axes[TY] = A.axes[TY] + B.axes[TY]; + C.axes[TZ] = A.axes[TZ] + B.axes[TZ]; //C.frame_number? return C; } diff --git a/facetracknoir/tracker_types.h b/facetracknoir/tracker_types.h index d8e7ecee..94d96de5 100644 --- a/facetracknoir/tracker_types.h +++ b/facetracknoir/tracker_types.h @@ -30,13 +30,9 @@ #include "ftnoir_tracker_base/ftnoir_tracker_types.h" -class T6DOF : public THeadPoseData -{ +struct T6DOF { public: - T6DOF() : THeadPoseData() {} - - T6DOF(double x, double y, double z, double yaw, double pitch, double roll) - : THeadPoseData(x,y,z, yaw,pitch,roll) {} + double axes[6]; }; T6DOF operator-(const T6DOF& A, const T6DOF& B); // get new pose with respect to reference pose B diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 3e931df4..cfd98742 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -53,45 +53,31 @@ void FTNoIR_Filter::loadSettings() { iniFile.endGroup (); } -void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, - THeadPoseData *target_camera_position, - THeadPoseData *new_camera_position, - THeadPoseData *last_post_filter_values, - bool newTarget) +void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, + double *target_camera_position, + double *new_camera_position, + double *last_post_filter_values) { double target[6]; double prev_output[6]; float output[6]; - int i=0; - - prev_output[0]=current_camera_position->x; - prev_output[1]=current_camera_position->y; - prev_output[2]=current_camera_position->z; - prev_output[3]=current_camera_position->yaw; - prev_output[4]=current_camera_position->pitch; - prev_output[5]=current_camera_position->roll; - - target[0]=target_camera_position->x; - target[1]=target_camera_position->y; - target[2]=target_camera_position->z; - target[3]=target_camera_position->yaw; - target[4]=target_camera_position->pitch; - target[5]=target_camera_position->roll; + + for (int i = 0; i < 6; i++) + { + prev_output[i] = current_camera_position[i]; + target[i] = target_camera_position[i]; + } if (first_run) { - new_camera_position->x=target[0]; - new_camera_position->y=target[1]; - new_camera_position->z=target[2]; - new_camera_position->yaw=target[3]; - new_camera_position->pitch=target[4]; - new_camera_position->roll=target[5]; + for (int i = 0; i < 6; i++) + new_camera_position[i] = target[i]; first_run=false; return; } - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { if (_isnan(target[i])) return; @@ -123,7 +109,7 @@ void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, // useful for filtering, as skipping them would result in jerky output. // the magic "100" is the amount of calls to the filter by FTNOIR per sec. // WVR: Added kMagicNumber for Patrick - double velocity = foo / (kMagicNumber > 0 ? kMagicNumber : 100.0) * (1 / std::max(1.0, 1 + kZoomSlowness * -last_post_filter_values->z / 100)); + double velocity = foo / (kMagicNumber > 0 ? kMagicNumber : 100.0) * (1 / std::max(1.0, 1 + kZoomSlowness * -last_post_filter_values[TZ] / 100)); double sum = start + velocity * sign; bool done = (sign > 0 ? sum >= e2 : sum <= e2); if (done) { @@ -136,19 +122,11 @@ void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, return; } - new_camera_position->x=output[0]; - new_camera_position->y=output[1]; - new_camera_position->z=output[2]; - new_camera_position->yaw=output[3]; - new_camera_position->pitch=output[4]; - new_camera_position->roll=output[5]; - - current_camera_position->x=output[0]; - current_camera_position->y=output[1]; - current_camera_position->z=output[2]; - current_camera_position->yaw=output[3]; - current_camera_position->pitch=output[4]; - current_camera_position->roll=output[5]; + for (int i = 0; i < 6; i++) + { + new_camera_position[i] = output[i]; + current_camera_position[i] = output[i]; + } } //////////////////////////////////////////////////////////////////////////////// diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h index 701dabaa..48efbc59 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -49,11 +49,11 @@ public: ~FTNoIR_Filter(); void Initialize(); - void FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter_values, bool newTarget); + void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter_values); private: void loadSettings(); // Load the settings from the INI-file - THeadPoseData newHeadPose; // Structure with new headpose + double newHeadPose[6]; // Structure with new headpose bool first_run; double kFactor, kFactorTranslation; diff --git a/ftnoir_filter_base/ftnoir_filter_base.h b/ftnoir_filter_base/ftnoir_filter_base.h index 37309077..5cd7b1d9 100644 --- a/ftnoir_filter_base/ftnoir_filter_base.h +++ b/ftnoir_filter_base/ftnoir_filter_base.h @@ -27,7 +27,7 @@ struct IFilter { virtual ~IFilter() {} virtual void Initialize() = 0; - virtual void FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter, bool newTarget) = 0; + virtual void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter) = 0; }; // Factory function that creates instances of the Filter object. diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp index 5f196533..c08b7e23 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp @@ -79,57 +79,33 @@ void FTNoIR_Filter::loadSettings() { } -void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter, bool newTarget) +void FTNoIR_Filter::FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter) { //non-optimised version for clarity - float prev_output[6]; - float target[6]; - float output_delta[6]; - float scale[]={0.025f,0.025f,0.025f,6.0f,6.0f,6.0f}; - float norm_output_delta[6]; - float output[6]; - int i=0; - - #if PRE_FILTER_SCALING - //compensate for any prefilter scaling - scale[0]*=X_POS_SCALE; - scale[1]*=Y_POS_SCALE; - scale[2]*=Z_POS_SCALE; - scale[3]*=X_ROT_SCALE; - scale[4]*=Y_ROT_SCALE; - scale[5]*=Z_ROT_SCALE; - #endif - - //find out how far the head has moved - prev_output[0]=current_camera_position->x; - prev_output[1]=current_camera_position->y; - prev_output[2]=current_camera_position->z; - prev_output[3]=current_camera_position->yaw; - prev_output[4]=current_camera_position->pitch; - prev_output[5]=current_camera_position->roll; - - target[0]=target_camera_position->x; - target[1]=target_camera_position->y; - target[2]=target_camera_position->z; - target[3]=target_camera_position->yaw; - target[4]=target_camera_position->pitch; - target[5]=target_camera_position->roll; + double prev_output[6]; + double target[6]; + double output_delta[6]; + double scale[]={0.025f,0.025f,0.025f,6.0f,6.0f,6.0f}; + double norm_output_delta[6]; + double output[6]; + + for (int i = 0; i < 6; i++) + { + prev_output[i] = current_camera_position[i]; + target[i] = target_camera_position[i]; + } if (first_run==true) { //on the first run, output=target - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { output[i]=target[i]; prev_alpha[i] = 0.0f; } - new_camera_position->x=target[0]; - new_camera_position->y=target[1]; - new_camera_position->z=target[2]; - new_camera_position->yaw=target[3]; - new_camera_position->pitch=target[4]; - new_camera_position->roll=target[5]; + for (int i = 0; i < 6; i++) + new_camera_position[i] = target[i]; first_run=false; @@ -138,13 +114,13 @@ void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, T } //how far does the camera need to move to catch up? - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { output_delta[i]=(target[i]-prev_output[i]); } //normalise the deltas - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { norm_output_delta[i]=std::min(std::max(fabs(output_delta[i])/scale[i],0.0),1.0); } @@ -152,9 +128,9 @@ void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, T //calculate the alphas //work out the dynamic smoothing factors // if (newTarget) { - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { - alpha[i]=1.0f/(kMinSmoothing+((1.0f-pow(norm_output_delta[i],kSmoothingScaleCurve))*smoothing_frames_range)); + alpha[i]=1.0/(kMinSmoothing+((1.0-pow(norm_output_delta[i],kSmoothingScaleCurve))*smoothing_frames_range)); smoothed_alpha[i]=(alpha_smoothing*alpha[i])+((1.0f-alpha_smoothing)*prev_alpha[i]); } // } @@ -165,7 +141,7 @@ void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, T //use the same (largest) smoothed alpha for each channel //NB: larger alpha = *less* lag (opposite to what you'd expect) float largest_alpha=0.0f; - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { if (smoothed_alpha[i]>=largest_alpha) { @@ -174,7 +150,7 @@ void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, T } //move the camera - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { output[i]=(largest_alpha*target[i])+((1.0f-largest_alpha)*prev_output[i]); // output[i]=(smoothed_alpha[i]*target[i])+((1.0f-smoothed_alpha[i])*prev_output[i]); @@ -193,25 +169,14 @@ void FTNoIR_Filter::FilterHeadPoseData(THeadPoseData *current_camera_position, T } #endif - new_camera_position->x=output[0]; - new_camera_position->y=output[1]; - new_camera_position->z=output[2]; - new_camera_position->yaw=output[3]; - new_camera_position->pitch=output[4]; - new_camera_position->roll=output[5]; - - // - // Also update the 'current' position, for the next iteration. - // - current_camera_position->x=output[0]; - current_camera_position->y=output[1]; - current_camera_position->z=output[2]; - current_camera_position->yaw=output[3]; - current_camera_position->pitch=output[4]; - current_camera_position->roll=output[5]; + for (int i = 0; i < 6; i++) + { + new_camera_position[i] = output[i]; + current_camera_position[i] = output[i]; + } //update filter memories ready for next sample - for (i=0;i<6;i++) + for (int i=0;i<6;i++) { prev_alpha[i]=smoothed_alpha[i]; } diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h index cade740f..b35d5cc3 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -43,22 +43,22 @@ public: ~FTNoIR_Filter(); void Initialize(); - void FilterHeadPoseData(THeadPoseData *current_camera_position, THeadPoseData *target_camera_position, THeadPoseData *new_camera_position, THeadPoseData *last_post_filter, bool newTarget); + void FilterHeadPoseData(double *current_camera_position, double *target_camera_position, double *new_camera_position, double *last_post_filter); private: void loadSettings(); // Load the settings from the INI-file - THeadPoseData newHeadPose; // Structure with new headpose + double newHeadPose; // Structure with new headpose bool first_run; - float smoothing_frames_range; - float alpha_smoothing; - float prev_alpha[6]; - float alpha[6]; - float smoothed_alpha[6]; - - float kMinSmoothing; - float kMaxSmoothing; - float kSmoothingScaleCurve; + double smoothing_frames_range; + double alpha_smoothing; + double prev_alpha[6]; + double alpha[6]; + double smoothed_alpha[6]; + + double kMinSmoothing; + double kMaxSmoothing; + double kSmoothingScaleCurve; }; //******************************************************************************************************* diff --git a/ftnoir_protocol_base/ftnoir_protocol_base.h b/ftnoir_protocol_base/ftnoir_protocol_base.h index 5d88a508..f1985677 100644 --- a/ftnoir_protocol_base/ftnoir_protocol_base.h +++ b/ftnoir_protocol_base/ftnoir_protocol_base.h @@ -59,7 +59,7 @@ struct IProtocol virtual ~IProtocol() {} virtual void Initialize() = 0; virtual bool checkServerInstallationOK() = 0; - virtual void sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ) = 0; + virtual void sendHeadposeToGame( double *headpose, double *rawheadpose ) = 0; virtual void getNameFromGame( char *dest ) = 0; // Take care dest can handle up to 100 chars... }; diff --git a/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp b/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp index 0c259d61..1303d051 100644 --- a/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp +++ b/ftnoir_protocol_fg/ftnoir_protocol_fg.cpp @@ -34,6 +34,7 @@ #include "ftnoir_protocol_fg.h" #include #include "facetracknoir/global-settings.h" +#include // For Todd and Arda Kutlu //#define SEND_ASCII_DATA @@ -99,24 +100,20 @@ void FTNoIR_Protocol::loadSettings() { // // Update Headpose in Game. // -void FTNoIR_Protocol::sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ) { +void FTNoIR_Protocol::sendHeadposeToGame( double *headpose, double *rawheadpose ) { int no_bytes; QHostAddress sender; quint16 senderPort; -#ifdef SEND_ASCII_DATA -char data[100]; -#endif - // // Copy the Raw measurements directly to the client. // - 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.x = headpose[TX]; + FlightData.y = headpose[RY]; + FlightData.z = headpose[TZ]; + FlightData.p = headpose[TY]; + FlightData.h = headpose[RX]; + FlightData.r = headpose[RZ]; FlightData.status = fg_cmd; // diff --git a/ftnoir_protocol_fg/ftnoir_protocol_fg.h b/ftnoir_protocol_fg/ftnoir_protocol_fg.h index 4ff2846f..3e2a1bf4 100644 --- a/ftnoir_protocol_fg/ftnoir_protocol_fg.h +++ b/ftnoir_protocol_fg/ftnoir_protocol_fg.h @@ -51,7 +51,7 @@ public: void Initialize(); bool checkServerInstallationOK(); - void sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ); + void sendHeadposeToGame( double *headpose, double *rawheadpose ); void getNameFromGame( char *dest ); // Take care dest can handle up to 100 chars... private: diff --git a/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp b/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp index c750ae26..0c25e77e 100644 --- a/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp +++ b/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.cpp @@ -104,7 +104,7 @@ void FTNoIR_Protocol::loadSettings() { // // Update Headpose in Game. // -void FTNoIR_Protocol::sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ) { +void FTNoIR_Protocol::sendHeadposeToGame(double *headpose, double *rawheadpose ) { DWORD result; TFSState pitch; TFSState yaw; @@ -121,13 +121,13 @@ float virtRotZ; // qDebug() << "FSUIPCServer::run() says: started!"; - virtRotX = -1.0f * headpose->pitch; // degrees - virtRotY = headpose->yaw; - virtRotZ = headpose->roll; + virtRotX = -headpose[RY]; // degrees + virtRotY = headpose[RX]; + virtRotZ = headpose[RZ]; virtPosX = 0.0f; // cm, X and Y are not working for FS2002/2004! virtPosY = 0.0f; - virtPosZ = -1.0f * headpose->z; + virtPosZ = headpose[TZ]; // // 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 0d386a59..e2aed7db 100644 --- a/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.h +++ b/ftnoir_protocol_fsuipc/ftnoir_protocol_fsuipc.h @@ -66,7 +66,7 @@ public: void Initialize(); bool checkServerInstallationOK(); - void sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ); + void sendHeadposeToGame( double *headpose, double *rawheadpose ); void getNameFromGame( char *dest ); // Take care dest can handle up to 100 chars... private: diff --git a/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp b/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp index bd9b7746..99525c90 100644 --- a/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp +++ b/ftnoir_protocol_ft/ftnoir_protocol_ft.cpp @@ -208,7 +208,7 @@ void FTNoIR_Protocol::loadSettings() { // // Update Headpose in Game. // -void FTNoIR_Protocol::sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ) { +void FTNoIR_Protocol::sendHeadposeToGame(double *headpose, double *rawheadpose ) { float virtPosX; float virtPosY; float virtPosZ; @@ -228,19 +228,19 @@ float headRotZ; // // Scale the Raw measurements to the client measurements. // - 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; + headRotX = getRadsFromDegrees(headpose[RY]); + headRotY = getRadsFromDegrees(headpose[RX]); + headRotZ = getRadsFromDegrees(headpose[RZ]); + headPosX = headpose[TX] * 10; + headPosY = headpose[TY] * 10; + headPosZ = headpose[TZ] * 10; + + virtRotX = getRadsFromDegrees(headpose[RY]); + virtRotY = getRadsFromDegrees(headpose[RX]); + virtRotZ = getRadsFromDegrees(headpose[RZ]); + virtPosX = headpose[TX] * 10; + virtPosY = headpose[TY] * 10; + virtPosZ = headpose[TZ] * 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 15ee5149..526a6aea 100644 --- a/ftnoir_protocol_ft/ftnoir_protocol_ft.h +++ b/ftnoir_protocol_ft/ftnoir_protocol_ft.h @@ -55,7 +55,7 @@ public: void Initialize(); bool checkServerInstallationOK( ); - void sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ); + void sendHeadposeToGame( double *headpose, double *rawheadpose ); void getNameFromGame( char *dest ); // Take care dest can handle up to 100 chars... private: diff --git a/ftnoir_protocol_ftn/ftnoir_protocol_ftn.cpp b/ftnoir_protocol_ftn/ftnoir_protocol_ftn.cpp index 26f331b3..b8cc0703 100644 --- a/ftnoir_protocol_ftn/ftnoir_protocol_ftn.cpp +++ b/ftnoir_protocol_ftn/ftnoir_protocol_ftn.cpp @@ -77,23 +77,21 @@ void FTNoIR_Protocol::loadSettings() { // // Update Headpose in Game. // -void FTNoIR_Protocol::sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ) { -int no_bytes; -THeadPoseData TestData; - +void FTNoIR_Protocol::sendHeadposeToGame(double *headpose, double *rawheadpose ) { + int no_bytes; + double test_data[6]; // // Copy the Raw measurements directly to the client. // - TestData = *headpose; - TestData.frame_number = 0; - + for (int i = 0; i < 6; i++) + test_data[i] = headpose[i]; // // Try to send an UDP-message to the receiver // //! [1] if (outSocket != 0) { - no_bytes = outSocket->writeDatagram((const char *) &TestData, sizeof( TestData ), destIP, destPort); + no_bytes = outSocket->writeDatagram((const char *) test_data, sizeof( test_data ), destIP, destPort); if ( no_bytes > 0) { // qDebug() << "FTNServer::writePendingDatagrams says: bytes send =" << no_bytes << sizeof( double ); } diff --git a/ftnoir_protocol_ftn/ftnoir_protocol_ftn.h b/ftnoir_protocol_ftn/ftnoir_protocol_ftn.h index 7dca6842..f40f30fa 100644 --- a/ftnoir_protocol_ftn/ftnoir_protocol_ftn.h +++ b/ftnoir_protocol_ftn/ftnoir_protocol_ftn.h @@ -48,7 +48,7 @@ public: void Initialize(); bool checkServerInstallationOK(); - void sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ); + void sendHeadposeToGame( double *headpose, double *rawheadpose ); void getNameFromGame( char *dest ); // Take care dest can handle up to 100 chars... private: diff --git a/ftnoir_protocol_mouse/ftnoir_protocol_mouse.cpp b/ftnoir_protocol_mouse/ftnoir_protocol_mouse.cpp index 953482a5..35198cb1 100644 --- a/ftnoir_protocol_mouse/ftnoir_protocol_mouse.cpp +++ b/ftnoir_protocol_mouse/ftnoir_protocol_mouse.cpp @@ -106,7 +106,7 @@ void FTNoIR_Protocol::loadSettings() { // // Update Headpose in Game. // -void FTNoIR_Protocol::sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ) { +void FTNoIR_Protocol::sendHeadposeToGame(double *headpose, double *rawheadpose ) { float fMouse_X = 0; // The actual value float fMouse_Y = 0; float fMouse_Wheel = 0; @@ -116,103 +116,25 @@ float fMouse_Wheel = 0; // Determine which of the 6DOF's is used. // The rotations are from -180 to +180 and the translations from -50cm to +50cm. // Let's scale the translations to the degrees for simplicity sake... - // - switch (Mouse_X) { - case FTN_PITCH: - fMouse_X = headpose->pitch; - break; - - case FTN_YAW: - fMouse_X = headpose->yaw; - break; - - case FTN_ROLL: - fMouse_X = headpose->roll; - break; - - case FTN_X: - fMouse_X = headpose->x * 3.0f; - break; - - case FTN_Y: - fMouse_X = headpose->y * 3.0f; - break; - - case FTN_Z: - fMouse_X = headpose->z * 3.0f; - break; - - default: - break; - } - - // - // Determine which of the 6DOF's is used. - // The rotations are from -180 to +180 and the translations from -50cm to +50cm. - // Let's scale the translations to the degrees for simplicity sake... - // - switch (Mouse_Y) { - case FTN_PITCH: - fMouse_Y = headpose->pitch; - break; + // - case FTN_YAW: - fMouse_Y = headpose->yaw; - break; + if (Mouse_X > 0 && Mouse_X < 6+1) + fMouse_X = headpose[Mouse_X-1]; - case FTN_ROLL: - fMouse_Y = headpose->roll; - break; + if (Mouse_X < 3+1 && Mouse_X > 0) + fMouse_X *= 3; - case FTN_X: - fMouse_Y = headpose->x * 3.0f; - break; + if (Mouse_Y > 0 && Mouse_Y < 6+1) + fMouse_Y = headpose[Mouse_Y-1]; - case FTN_Y: - fMouse_Y = headpose->y * 3.0f; - break; + if (Mouse_Y < 3+1 && Mouse_Y > 0) + fMouse_Y *= 3; - case FTN_Z: - fMouse_Y = headpose->z * 3.0f; - break; + if (Mouse_Wheel > 0 && Mouse_Wheel < 6+1) + fMouse_Wheel = headpose[Mouse_Wheel-1]; - default: - break; - } - - // - // Determine which of the 6DOF's is used. - // The rotations are from -180 to +180 and the translations from -50cm to +50cm. - // Let's scale the translations to the degrees for simplicity sake... - // - switch (Mouse_Wheel) { - case FTN_PITCH: - fMouse_Wheel = headpose->pitch; - break; - - case FTN_YAW: - fMouse_Wheel = headpose->yaw; - break; - - case FTN_ROLL: - fMouse_Wheel = headpose->roll; - break; - - case FTN_X: - fMouse_Wheel = headpose->x * 3.0f; - break; - - case FTN_Y: - fMouse_Wheel = headpose->y * 3.0f; - break; - - case FTN_Z: - fMouse_Wheel = headpose->z * 3.0f; - break; - - default: - break; - } + if (Mouse_Wheel < 3+1 && Mouse_Wheel > 0) + fMouse_Wheel *= 3; // // Only send Input, when it has changed. diff --git a/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h b/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h index e4bc6256..e1305a6f 100644 --- a/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h +++ b/ftnoir_protocol_mouse/ftnoir_protocol_mouse.h @@ -46,12 +46,12 @@ #define MOUSE_AXIS_MAX 65535 enum FTN_AngleName { - FTN_PITCH = 1, - FTN_YAW = 2, - FTN_ROLL = 3, - FTN_X = 4, - FTN_Y = 5, - FTN_Z = 6 + FTN_PITCH = RX, + FTN_YAW = TY, + FTN_ROLL = RZ, + FTN_X = TX, + FTN_Y = TY, + FTN_Z = TZ }; enum FTN_MouseStyle { @@ -70,7 +70,7 @@ public: void Initialize(); bool checkServerInstallationOK(); - void sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ); + void sendHeadposeToGame( double *headpose, double *rawheadpose ); void getNameFromGame( char *dest ); // Take care dest can handle up to 100 chars... private: diff --git a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp index 90ca3dba..39c98ea7 100644 --- a/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp +++ b/ftnoir_protocol_sc/ftnoir_protocol_sc.cpp @@ -86,17 +86,17 @@ void FTNoIR_Protocol::loadSettings() { // // Update Headpose in Game. // -void FTNoIR_Protocol::sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ) { +void FTNoIR_Protocol::sendHeadposeToGame( double *headpose, double *rawheadpose ) { PDWORD_PTR MsgResult = 0; - virtSCRotX = -1.0f * headpose->pitch; // degrees - virtSCRotY = -1.0f * headpose->yaw; - virtSCRotZ = headpose->roll; + virtSCRotX = -headpose[RY]; // degrees + virtSCRotY = -headpose[RX]; + virtSCRotZ = headpose[RZ]; - virtSCPosX = headpose->x/100.f; // cm to meters - virtSCPosY = headpose->y/100.f; - virtSCPosZ = -1.0f * headpose->z/100.f; + virtSCPosX = headpose[TX]/100.f; // cm to meters + virtSCPosY = headpose[TY]/100.f; + virtSCPosZ = -headpose[TZ]/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 57919193..f49860b7 100644 --- a/ftnoir_protocol_sc/ftnoir_protocol_sc.h +++ b/ftnoir_protocol_sc/ftnoir_protocol_sc.h @@ -85,7 +85,7 @@ public: ~FTNoIR_Protocol(); void Initialize() {} bool checkServerInstallationOK(); - void sendHeadposeToGame( THeadPoseData *headpose, THeadPoseData *rawheadpose ); + void sendHeadposeToGame( double *headpose, double *rawheadpose ); void getNameFromGame( char *dest ); // Take care dest can handle up to 100 chars... private: diff --git a/ftnoir_tracker_base/ftnoir_tracker_base.h b/ftnoir_tracker_base/ftnoir_tracker_base.h index 416d3944..b72be93e 100644 --- a/ftnoir_tracker_base/ftnoir_tracker_base.h +++ b/ftnoir_tracker_base/ftnoir_tracker_base.h @@ -56,7 +56,7 @@ struct ITracker { virtual ~ITracker() {} virtual void StartTracker( QFrame* frame ) = 0; - virtual bool GiveHeadPoseData(THeadPoseData *data) = 0; + virtual bool GiveHeadPoseData(double *data) = 0; virtual void WaitForExit() = 0; }; diff --git a/ftnoir_tracker_base/ftnoir_tracker_types.h b/ftnoir_tracker_base/ftnoir_tracker_types.h index ee443b95..d2981701 100644 --- a/ftnoir_tracker_base/ftnoir_tracker_types.h +++ b/ftnoir_tracker_base/ftnoir_tracker_types.h @@ -30,22 +30,8 @@ #ifndef FTNOIR_TRACKER_TYPES_H #define FTNOIR_TRACKER_TYPES_H -// -// x,y,z position in centimetres, yaw, pitch and roll in degrees... -// -#pragma pack(push, 2) -struct THeadPoseData { - - THeadPoseData() - : x(0.0), y(0.0), z(0.0), yaw(0.0), pitch(0.0), roll(0.0), frame_number(0) {} - - 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) {} - - double x, y, z, yaw, pitch, roll; - unsigned char frame_number; +enum Axis { + TX = 0, TY, TZ, RX, RY, RZ }; -#pragma pack(pop) #endif // FTNOIR_TRACKER_TYPES_H diff --git a/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp b/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp index d37ab9b2..224fff4a 100644 --- a/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp +++ b/ftnoir_tracker_ht/ftnoir_tracker_ht.cpp @@ -218,7 +218,7 @@ void Tracker::paint_widget() { } } -bool Tracker::GiveHeadPoseData(THeadPoseData* data) +bool Tracker::GiveHeadPoseData(double *data) { bool ret = false; @@ -233,25 +233,25 @@ bool Tracker::GiveHeadPoseData(THeadPoseData* data) } if (shm->result.filled) { if (enableRX) - data->yaw = shm->result.rotx; + data[RX] = shm->result.rotx; if (enableRY) { - data->pitch = shm->result.roty; - double sign = data->pitch >= 0 ? 1 : -1; - if (fabs(fabs(data->pitch) - 180) < fabs(data->pitch)) - data->pitch = fabs(fabs(data->pitch) - 180) * sign; + data[RY] = shm->result.roty; + double sign = data[RY] >= 0 ? 1 : -1; + if (fabs(fabs(data[RY]) - 180) < fabs(data[RY])) + data[RY] = fabs(fabs(data[RY]) - 180) * sign; } if (enableRZ) { - data->roll = shm->result.rotz; - double sign = data->roll >= 0 ? 1 : -1; - if (fabs(fabs(data->roll) - 180) < fabs(data->roll)) - data->roll = fabs(fabs(data->roll) - 180) * sign; + data[RZ] = shm->result.rotz; + double sign = data[RZ] >= 0 ? 1 : -1; + if (fabs(fabs(data[RZ]) - 180) < fabs(data[RZ])) + data[RZ] = fabs(fabs(data[RZ]) - 180) * sign; } if (enableTX) - data->x = shm->result.tx; + data[TX] = shm->result.tx; if (enableTY) - data->y = shm->result.ty; + data[TY] = shm->result.ty; if (enableTZ) - data->z = shm->result.tz; + data[TZ] = shm->result.tz; ret = true; } lck_shm.unlock(); diff --git a/ftnoir_tracker_ht/ftnoir_tracker_ht.h b/ftnoir_tracker_ht/ftnoir_tracker_ht.h index 5a17e4af..a08f2437 100644 --- a/ftnoir_tracker_ht/ftnoir_tracker_ht.h +++ b/ftnoir_tracker_ht/ftnoir_tracker_ht.h @@ -24,7 +24,7 @@ public: Tracker(); ~Tracker(); void StartTracker(QFrame* frame); - bool GiveHeadPoseData(THeadPoseData *data); + bool GiveHeadPoseData(double *data); bool enableTX, enableTY, enableTZ, enableRX, enableRY, enableRZ; ht_shm_t* shm; bool NeedsTimeToFinish() { diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 96017d52..654e9924 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -192,7 +192,7 @@ void Tracker::paint_widget() { } } -bool Tracker::GiveHeadPoseData(THeadPoseData *data) +bool Tracker::GiveHeadPoseData(double *data) { const float rad2deg = 180.0/3.14159265; const float deg2rad = 1.0/rad2deg; @@ -217,13 +217,13 @@ bool Tracker::GiveHeadPoseData(THeadPoseData *data) // get translation(s) if (bEnableX) { - data->x = t[0] / 10.0; // convert to cm + data[TX] = t[0] / 10.0; // convert to cm } if (bEnableY) { - data->y = t[1] / 10.0; + data[TY] = t[1] / 10.0; } if (bEnableZ) { - data->z = t[2] / 10.0; + data[TZ] = t[2] / 10.0; } // translate rotation matrix from opengl (G) to roll-pitch-yaw (R) frame @@ -241,13 +241,13 @@ bool Tracker::GiveHeadPoseData(THeadPoseData *data) gamma = atan2( R(2,1), R(2,2)); if (bEnableYaw) { - data->yaw = rad2deg * alpha; + data[RX] = rad2deg * alpha; } if (bEnablePitch) { - data->pitch = rad2deg * beta; + data[RY] = rad2deg * beta; } if (bEnableRoll) { - data->roll = rad2deg * gamma; + data[RZ] = rad2deg * gamma; } } return true; diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h index 49881b69..014cab76 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h @@ -33,7 +33,7 @@ public: // ITracker interface void StartTracker(QFrame* videoFrame); - bool GiveHeadPoseData(THeadPoseData *data); + bool GiveHeadPoseData(double *data); void refreshVideo(); diff --git a/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp b/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp index 09d7432b..81be669a 100644 --- a/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp +++ b/ftnoir_tracker_sm/ftnoir_tracker_faceapi.cpp @@ -92,7 +92,7 @@ void FTNoIR_Tracker::WaitForExit() } } -bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data) +bool FTNoIR_Tracker::GiveHeadPoseData(double *data) { // // Check if the pointer is OK and wait for the Mutex. @@ -103,22 +103,22 @@ bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data) // Copy the measurements to FaceTrackNoIR. // if (bEnableX) { - data->x = pMemData->data.new_pose.head_pos.x * 100.0f; // From meters to centimeters + data[TX] = pMemData->data.new_pose.head_pos.x * 100.0f; // From meters to centimeters } if (bEnableY) { - data->y = pMemData->data.new_pose.head_pos.y * 100.0f; + data[TY] = pMemData->data.new_pose.head_pos.y * 100.0f; } if (bEnableZ) { - data->z = pMemData->data.new_pose.head_pos.z * 100.0f; + data[TZ] = pMemData->data.new_pose.head_pos.z * 100.0f; } if (bEnableYaw) { - data->yaw = pMemData->data.new_pose.head_rot.y_rads * 57.295781f; // From rads to degrees + data[RX] = pMemData->data.new_pose.head_rot.y_rads * 57.295781f; // From rads to degrees } if (bEnablePitch) { - data->pitch = pMemData->data.new_pose.head_rot.x_rads * 57.295781f; + data[RY] = pMemData->data.new_pose.head_rot.x_rads * 57.295781f; } if (bEnableRoll) { - data->roll = pMemData->data.new_pose.head_rot.z_rads * 57.295781f; + data[RZ] = pMemData->data.new_pose.head_rot.z_rads * 57.295781f; } // diff --git a/ftnoir_tracker_sm/ftnoir_tracker_sm.h b/ftnoir_tracker_sm/ftnoir_tracker_sm.h index ae79436d..665507e6 100644 --- a/ftnoir_tracker_sm/ftnoir_tracker_sm.h +++ b/ftnoir_tracker_sm/ftnoir_tracker_sm.h @@ -45,7 +45,7 @@ public: void StartTracker( QFrame* parent_window ); void StopTracker( bool exit ); - bool GiveHeadPoseData(THeadPoseData *data); // Returns true if confidence is good + bool GiveHeadPoseData(double *data); // Returns true if confidence is good void WaitForExit(); void loadSettings(); diff --git a/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp b/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp index 4978fa8d..58316298 100644 --- a/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp +++ b/ftnoir_tracker_udp/ftnoir_tracker_udp.cpp @@ -38,12 +38,8 @@ FTNoIR_Tracker::FTNoIR_Tracker() bEnableZ = true; portAddress = 5551; - newHeadPose.x = 0.0f; - newHeadPose.y = 0.0f; - newHeadPose.z = 0.0f; - newHeadPose.yaw = 0.0f; - newHeadPose.pitch = 0.0f; - newHeadPose.roll = 0.0f; + for (int i = 0; i < 6; i++) + newHeadPose[i] = 0; } FTNoIR_Tracker::~FTNoIR_Tracker() @@ -111,26 +107,26 @@ void FTNoIR_Tracker::StartTracker(QFrame* videoFrame) return; } -bool FTNoIR_Tracker::GiveHeadPoseData(THeadPoseData *data) +bool FTNoIR_Tracker::GiveHeadPoseData(double *data) { mutex.lock(); if (bEnableX) { - data->x = newHeadPose.x; + data[TX] = newHeadPose[TX]; } if (bEnableX) { - data->y = newHeadPose.y; + data[TY] = newHeadPose[TY]; } if (bEnableX) { - data->z = newHeadPose.z; + data[TZ] = newHeadPose[TZ]; } if (bEnableYaw) { - data->yaw = newHeadPose.yaw; + data[RX] = newHeadPose[RX]; } if (bEnablePitch) { - data->pitch = newHeadPose.pitch; + data[RY] = newHeadPose[RY]; } if (bEnableRoll) { - data->roll = newHeadPose.roll; + data[RZ] = newHeadPose[RZ]; } mutex.unlock(); return true; diff --git a/ftnoir_tracker_udp/ftnoir_tracker_udp.h b/ftnoir_tracker_udp/ftnoir_tracker_udp.h index d60b27a4..ccb42ac1 100644 --- a/ftnoir_tracker_udp/ftnoir_tracker_udp.h +++ b/ftnoir_tracker_udp/ftnoir_tracker_udp.h @@ -16,7 +16,7 @@ public: ~FTNoIR_Tracker(); void StartTracker( QFrame *videoframe ); - bool GiveHeadPoseData(THeadPoseData *data); + bool GiveHeadPoseData(double *data); void loadSettings(); volatile bool should_quit; void WaitForExit() { @@ -34,7 +34,7 @@ private: QHostAddress destIP; // Destination IP-address QHostAddress srcIP; // Source IP-address - THeadPoseData newHeadPose; // Structure with new headpose + double newHeadPose[6]; // Structure with new headpose float portAddress; // Port-number bool bEnableRoll; -- cgit v1.2.3