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 +- 6 files changed, 209 insertions(+), 304 deletions(-) (limited to 'facetracknoir') 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 -- cgit v1.2.3