diff options
Diffstat (limited to 'FaceTrackNoIR')
| -rw-r--r-- | FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj | 16 | ||||
| -rw-r--r-- | FaceTrackNoIR/rotation.cpp | 48 | ||||
| -rw-r--r-- | FaceTrackNoIR/rotation.h | 31 | ||||
| -rw-r--r-- | FaceTrackNoIR/tracker.cpp | 116 | ||||
| -rw-r--r-- | FaceTrackNoIR/tracker.h | 1 | ||||
| -rw-r--r-- | FaceTrackNoIR/tracker_types.cpp | 44 | ||||
| -rw-r--r-- | FaceTrackNoIR/tracker_types.h | 45 |
7 files changed, 239 insertions, 62 deletions
diff --git a/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj b/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj index 9617836f..56b5e1d5 100644 --- a/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj +++ b/FaceTrackNoIR/FaceTrackNoIR_vc9.vcproj @@ -207,9 +207,17 @@ >
</File>
<File
+ RelativePath=".\rotation.cpp"
+ >
+ </File>
+ <File
RelativePath=".\tracker.cpp"
>
</File>
+ <File
+ RelativePath=".\tracker_types.cpp"
+ >
+ </File>
</Filter>
<Filter
Name="Headers"
@@ -299,6 +307,10 @@ >
</File>
<File
+ RelativePath=".\rotation.h"
+ >
+ </File>
+ <File
RelativePath=".\tracker.h"
>
<FileConfiguration
@@ -324,6 +336,10 @@ />
</FileConfiguration>
</File>
+ <File
+ RelativePath=".\tracker_types.h"
+ >
+ </File>
</Filter>
<Filter
Name="FormsQt"
diff --git a/FaceTrackNoIR/rotation.cpp b/FaceTrackNoIR/rotation.cpp new file mode 100644 index 00000000..1c89d775 --- /dev/null +++ b/FaceTrackNoIR/rotation.cpp @@ -0,0 +1,48 @@ +/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "rotation.h"
+
+#include <cmath>
+
+// ----------------------------------------------------------------------------
+Rotation Rotation::inv()
+{
+ return Rotation(a,-b,-c,-d);
+}
+
+// conversions
+// see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+void Rotation::fromEuler(double yaw, double pitch, double roll)
+{
+ double sin_phi = sin(roll/2.0);
+ double cos_phi = cos(roll/2.0);
+ double sin_the = sin(pitch/2.0);
+ double cos_the = cos(pitch/2.0);
+ double sin_psi = sin(yaw/2.0);
+ double cos_psi = cos(yaw/2.0);
+
+ a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;
+ b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;
+ c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;
+ d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
+}
+
+void Rotation::toEuler(double& yaw, double& pitch, double& roll)
+{
+ roll = atan2(2.0*(a*b + c*d), 1.0 - 2.0*(b*b + c*c));
+ pitch = asin(2.0*(a*c - b*d));
+ yaw = atan2(2.0*(a*d + b*c), 1.0 - 2.0*(c*c + d*d));
+}
+
+Rotation operator*(const Rotation& A, const Rotation& B)
+{
+ return Rotation(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d, // quaternion multiplication
+ A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c,
+ A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,
+ A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);
+}
\ No newline at end of file diff --git a/FaceTrackNoIR/rotation.h b/FaceTrackNoIR/rotation.h new file mode 100644 index 00000000..967d6661 --- /dev/null +++ b/FaceTrackNoIR/rotation.h @@ -0,0 +1,31 @@ +/* Copyright (c) 2012 Patrick Ruoff
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#ifndef ROTATION_H
+#define ROTATION_H
+
+// ----------------------------------------------------------------------------
+class Rotation {
+ friend Rotation operator*(const Rotation& A, const Rotation& B);
+public:
+ Rotation() : a(1.0),b(0.0),c(0.0),d(0.0) {}
+ Rotation(double yaw, double pitch, double roll) { fromEuler(yaw, pitch, roll); }
+ Rotation(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {}
+
+ Rotation inv(); // inverse
+
+ // conversions
+ void fromEuler(double yaw, double pitch, double roll);
+ void toEuler(double& yaw, double& pitch, double& roll);
+
+protected:
+ double a,b,c,d; // quaternion coefficients
+};
+
+Rotation operator*(const Rotation& A, const Rotation& B); // composition of rotations
+
+#endif //ROTATION_H
diff --git a/FaceTrackNoIR/tracker.cpp b/FaceTrackNoIR/tracker.cpp index 5b387555..c78644c1 100644 --- a/FaceTrackNoIR/tracker.cpp +++ b/FaceTrackNoIR/tracker.cpp @@ -334,11 +334,6 @@ T6DOF gameoutput_camera(0,0,0,0,0,0); Tracker::do_tracking = true; // Start initially
Tracker::do_center = true; // Center initially
- current_camera.initHeadPoseData();
- target_camera.initHeadPoseData();
- new_camera.initHeadPoseData();
- output_camera.initHeadPoseData();
-
//
// Test some Filter-stuff
//
@@ -582,12 +577,12 @@ T6DOF gameoutput_camera(0,0,0,0,0,0); //
if (Tracker::confid) {
- offset_camera.position.x = getSmoothFromList( &X.rawList );
- offset_camera.position.y = getSmoothFromList( &Y.rawList );
- offset_camera.position.z = getSmoothFromList( &Z.rawList );
- offset_camera.position.pitch = getSmoothFromList( &Pitch.rawList );
- offset_camera.position.yaw = getSmoothFromList( &Yaw.rawList );
- offset_camera.position.roll = getSmoothFromList( &Roll.rawList );
+ offset_camera.x = getSmoothFromList( &X.rawList );
+ offset_camera.y = getSmoothFromList( &Y.rawList );
+ offset_camera.z = getSmoothFromList( &Z.rawList );
+ offset_camera.pitch = getSmoothFromList( &Pitch.rawList );
+ offset_camera.yaw = getSmoothFromList( &Yaw.rawList );
+ offset_camera.roll = getSmoothFromList( &Roll.rawList );
}
Tracker::do_center = false;
@@ -599,50 +594,47 @@ T6DOF gameoutput_camera(0,0,0,0,0,0); //
if (Tracker::confid && Tracker::do_game_zero) {
if (!pTracker->notifyZeroed())
- gamezero_camera.position = gameoutput_camera.position;
-// gamezero_camera.position = gameoutput_camera.position;
+ gamezero_camera = gameoutput_camera;
+// gamezero_camera = gameoutput_camera;
Tracker::do_game_zero = false;
}
if (Tracker::do_tracking && Tracker::confid) {
- // Pitch
- target_camera.position.x = getSmoothFromList( &X.rawList );
- target_camera.position.y = getSmoothFromList( &Y.rawList );
- target_camera.position.z = getSmoothFromList( &Z.rawList );
- target_camera.position.pitch = getSmoothFromList( &Pitch.rawList );
- target_camera.position.yaw = getSmoothFromList( &Yaw.rawList );
- target_camera.position.roll = getSmoothFromList( &Roll.rawList );
+ // get values
+ target_camera.x = getSmoothFromList( &X.rawList );
+ target_camera.y = getSmoothFromList( &Y.rawList );
+ target_camera.z = getSmoothFromList( &Z.rawList );
+ target_camera.pitch = getSmoothFromList( &Pitch.rawList );
+ target_camera.yaw = getSmoothFromList( &Yaw.rawList );
+ target_camera.roll = getSmoothFromList( &Roll.rawList );
+
+ // do the centering
target_camera = target_camera - offset_camera;
if (Tracker::useFilter && pFilter) {
- pFilter->FilterHeadPoseData(¤t_camera.position, &target_camera.position, &new_camera.position, Tracker::Pitch.newSample);
+ pFilter->FilterHeadPoseData(¤t_camera, &target_camera, &new_camera, Tracker::Pitch.newSample);
}
else {
- new_camera.position.x = getSmoothFromList( &X.rawList ) - offset_camera.position.x;
- new_camera.position.y = getSmoothFromList( &Y.rawList ) - offset_camera.position.y;
- new_camera.position.z = getSmoothFromList( &Z.rawList ) - offset_camera.position.z;
- new_camera.position.pitch = getSmoothFromList( &Pitch.rawList ) - offset_camera.position.pitch;
- new_camera.position.yaw = getSmoothFromList( &Yaw.rawList ) - offset_camera.position.yaw;
- new_camera.position.roll = getSmoothFromList( &Roll.rawList ) - offset_camera.position.roll;
+ new_camera = target_camera;
}
- output_camera.position.x = X.invert * X.curvePtr->getValue(new_camera.position.x);
- output_camera.position.y = Y.invert * Y.curvePtr->getValue(new_camera.position.y);
- output_camera.position.z = Z.invert * Z.curvePtr->getValue(new_camera.position.z);
- bool altp = new_camera.position.pitch < 0;
+ output_camera.x = X.invert * X.curvePtr->getValue(new_camera.x);
+ output_camera.y = Y.invert * Y.curvePtr->getValue(new_camera.y);
+ output_camera.z = Z.invert * Z.curvePtr->getValue(new_camera.z);
+ bool altp = new_camera.pitch < 0;
if (altp) {
- output_camera.position.pitch = Pitch.invert * Pitch.curvePtrAlt->getValue(new_camera.position.pitch);
+ output_camera.pitch = Pitch.invert * Pitch.curvePtrAlt->getValue(new_camera.pitch);
Pitch.curvePtr->setTrackingActive( false );
Pitch.curvePtrAlt->setTrackingActive( true );
}
else {
- output_camera.position.pitch = Pitch.invert * Pitch.curvePtr->getValue(new_camera.position.pitch);
+ output_camera.pitch = Pitch.invert * Pitch.curvePtr->getValue(new_camera.pitch);
Pitch.curvePtr->setTrackingActive( true );
Pitch.curvePtrAlt->setTrackingActive( false );
}
- output_camera.position.yaw = Yaw.invert * Yaw.curvePtr->getValue(new_camera.position.yaw);
- output_camera.position.roll = Roll.invert * Roll.curvePtr->getValue(new_camera.position.roll);
+ output_camera.yaw = Yaw.invert * Yaw.curvePtr->getValue(new_camera.yaw);
+ output_camera.roll = Roll.invert * Roll.curvePtr->getValue(new_camera.roll);
X.curvePtr->setTrackingActive( true );
@@ -654,22 +646,22 @@ T6DOF gameoutput_camera(0,0,0,0,0,0); //
// Reverse Axis.
//
- actualYaw = output_camera.position.yaw; // Save the actual Yaw, otherwise we can't check for +90
- actualZ = output_camera.position.z; // Also the Z
+ actualYaw = output_camera.yaw; // Save the actual Yaw, otherwise we can't check for +90
+ actualZ = output_camera.z; // Also the Z
if (Tracker::do_axis_reverse) {
- output_camera.position.z = Z_PosWhenReverseAxis; // Set the desired Z-position
+ output_camera.z = Z_PosWhenReverseAxis; // Set the desired Z-position
}
//
// Reset value for the selected axis, if inhibition is active
//
if (Tracker::do_inhibit) {
- if (InhibitKey.doPitch) output_camera.position.pitch = 0.0f;
- if (InhibitKey.doYaw) output_camera.position.yaw = 0.0f;
- if (InhibitKey.doRoll) output_camera.position.roll = 0.0f;
- if (InhibitKey.doX) output_camera.position.x = 0.0f;
- if (InhibitKey.doY) output_camera.position.y = 0.0f;
- if (InhibitKey.doZ) output_camera.position.z = 0.0f;
+ if (InhibitKey.doPitch) output_camera.pitch = 0.0f;
+ if (InhibitKey.doYaw) output_camera.yaw = 0.0f;
+ if (InhibitKey.doRoll) output_camera.roll = 0.0f;
+ if (InhibitKey.doX) output_camera.x = 0.0f;
+ if (InhibitKey.doY) output_camera.y = 0.0f;
+ if (InhibitKey.doZ) output_camera.z = 0.0f;
}
// All Protocol server(s)
@@ -687,12 +679,12 @@ T6DOF gameoutput_camera(0,0,0,0,0,0); debug_Client->setHeadPosY( Tracker::Y.headPos );
debug_Client->setHeadPosZ( Tracker::Z.headPos );
- debug_Client->setVirtRotX ( new_camera.position.pitch ); // degrees
- debug_Client->setVirtRotY ( new_camera.position.yaw );
- debug_Client->setVirtRotZ ( new_camera.position.roll );
- debug_Client->setVirtPosX ( new_camera.position.x ); // centimeters
- debug_Client->setVirtPosY ( new_camera.position.y );
- debug_Client->setVirtPosZ ( new_camera.position.z );
+ debug_Client->setVirtRotX ( new_camera.pitch ); // degrees
+ debug_Client->setVirtRotY ( new_camera.yaw );
+ debug_Client->setVirtRotZ ( new_camera.roll );
+ debug_Client->setVirtPosX ( new_camera.x ); // centimeters
+ debug_Client->setVirtPosY ( new_camera.y );
+ debug_Client->setVirtPosZ ( new_camera.z );
# endif
@@ -702,12 +694,12 @@ T6DOF gameoutput_camera(0,0,0,0,0,0); // Go to initial position
//
if (pProtocol && setZero) {
- output_camera.position.pitch = 0.0f;
- output_camera.position.yaw = 0.0f;
- output_camera.position.roll = 0.0f;
- output_camera.position.x = 0.0f;
- output_camera.position.y = 0.0f;
- output_camera.position.z = 0.0f;
+ output_camera.pitch = 0.0f;
+ output_camera.yaw = 0.0f;
+ output_camera.roll = 0.0f;
+ output_camera.x = 0.0f;
+ output_camera.y = 0.0f;
+ output_camera.z = 0.0f;
gameoutput_camera = output_camera + gamezero_camera;
pProtocol->sendHeadposeToGame( &gameoutput_camera ); // degrees & centimeters
}
@@ -840,13 +832,13 @@ void Tracker::getHeadPose( THeadPoseData *data ) { // Get the output-headpose, so it can be displayed.
//
void Tracker::getOutputHeadPose( THeadPoseData *data ) {
- data->x = output_camera.position.x; // centimeters
- data->y = output_camera.position.y;
- data->z = output_camera.position.z;
+ data->x = output_camera.x; // centimeters
+ data->y = output_camera.y;
+ data->z = output_camera.z;
- data->pitch = output_camera.position.pitch; // degrees
- data->yaw = output_camera.position.yaw;
- data->roll = output_camera.position.roll;
+ data->pitch = output_camera.pitch; // degrees
+ data->yaw = output_camera.yaw;
+ data->roll = output_camera.roll;
}
//
diff --git a/FaceTrackNoIR/tracker.h b/FaceTrackNoIR/tracker.h index 57e8136b..4b52e438 100644 --- a/FaceTrackNoIR/tracker.h +++ b/FaceTrackNoIR/tracker.h @@ -45,6 +45,7 @@ #include "..\ftnoir_tracker_base\FTNoIR_Tracker_base.h"
#include "..\ftnoir_protocol_base\FTNoIR_Protocol_base.h"
#include "..\ftnoir_filter_base\FTNoIR_Filter_base.h"
+#include "tracker_types.h"
typedef ITrackerPtr (WINAPI *importGetTracker)(void);
typedef IProtocolPtr (WINAPI *importGetProtocol)(void);
diff --git a/FaceTrackNoIR/tracker_types.cpp b/FaceTrackNoIR/tracker_types.cpp new file mode 100644 index 00000000..e3c15807 --- /dev/null +++ b/FaceTrackNoIR/tracker_types.cpp @@ -0,0 +1,44 @@ +#include "tracker_types.h"
+#include "rotation.h"
+
+const double PI = 3.14159265358979323846264;
+const double D2R = PI/180.0;
+const double R2D = 180.0/PI;
+
+T6DOF operator-(const T6DOF& A, const T6DOF& B)
+{
+ Rotation R_A(A.yaw*D2R, A.pitch*D2R, A.roll*D2R);
+ Rotation R_B(B.yaw*D2R, B.pitch*D2R, B.roll*D2R);
+ Rotation R_C = R_A * R_B.inv();
+
+ T6DOF C;
+ R_C.toEuler(C.yaw, C.pitch, C.roll);
+ C.yaw *= R2D;
+ C.pitch *= R2D;
+ C.roll *= R2D;
+
+ C.x = A.x - B.x;
+ C.y = A.y - B.y;
+ C.z = A.z - B.z;
+ //C.frame_number?
+ return C;
+}
+
+T6DOF operator+(const T6DOF& A, const T6DOF& B)
+{
+ Rotation R_A(A.yaw*D2R, A.pitch*D2R, A.roll*D2R);
+ Rotation R_B(B.yaw*D2R, B.pitch*D2R, B.roll*D2R);
+ Rotation R_C = R_A * R_B;
+
+ T6DOF C;
+ R_C.toEuler(C.yaw, C.pitch, C.roll);
+ C.yaw *= R2D;
+ C.pitch *= R2D;
+ C.roll *= R2D;
+
+ C.x = A.x + B.x;
+ C.y = A.y + B.y;
+ C.z = A.z + B.z;
+ //C.frame_number?
+ return C;
+}
\ No newline at end of file diff --git a/FaceTrackNoIR/tracker_types.h b/FaceTrackNoIR/tracker_types.h new file mode 100644 index 00000000..5a13af85 --- /dev/null +++ b/FaceTrackNoIR/tracker_types.h @@ -0,0 +1,45 @@ +/********************************************************************************
+* FaceTrackNoIR This program is a private project of the some enthusiastic *
+* gamers from Holland, who don't like to pay much for *
+* head-tracking. *
+* *
+* Copyright (C) 2010 - 2012 Wim Vriend (Developing) *
+* Ron Hendriks (Researching and Testing) *
+* *
+* Homepage * *
+* This program is free software; you can redistribute it and/or modify it *
+* under the terms of the GNU General Public License as published by the *
+* Free Software Foundation; either version 3 of the License, or (at your *
+* option) any later version. *
+* *
+* This program is distributed in the hope that it will be useful, but *
+* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
+* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for *
+* more details. *
+* *
+* You should have received a copy of the GNU General Public License along *
+* with this program; if not, see <http://www.gnu.org/licenses/>. *
+*********************************************************************************/
+/*
+ Modifications (last one on top):
+ 20120924 - C14: Moved tracker types only used by Tracker to this file (should not be part of public interface)
+ Modified operators to represent correct frame transitions
+*/
+#ifndef __TRACKER_TYPES_H__
+#define __TRACKER_TYPES_H__
+
+#include "..\ftnoir_tracker_base\ftnoir_tracker_types.h"
+
+class T6DOF : public THeadPoseData
+{
+public:
+ T6DOF() : THeadPoseData() {}
+
+ T6DOF(double x, double y, double z, double yaw, double pitch, double roll)
+ : THeadPoseData(x,y,z, yaw,pitch,roll) {}
+};
+
+T6DOF operator-(const T6DOF& A, const T6DOF& B); // get new pose with respect to reference pose B
+T6DOF operator+(const T6DOF& A, const T6DOF& B); // get new pose with respect to reference pose B^-1
+
+#endif //__TRACKER_TYPES_H__
\ No newline at end of file |