2 files changed, 113 insertions, 0 deletions

diff --git a/matchblox/common/stereoheadtrackfrustum.cpp b/matchblox/common/stereoheadtrackfrustum.cpp
new file mode 100644
index 0000000..d43ba4e
--- /dev/null
+++ b/matchblox/common/stereoheadtrackfrustum.cpp
@@ -0,0 +1,101 @@
+#include <GL\glut.h>

+

+#define _USE_MATH_DEFINES

+#include <math.h>

+

+#include "stereoheadtrackfrustum.h"

+#include "typedefs.h"

+#include "message_input.h"

+

+bool CalcEyePosInMM(input_payload_wiimote *f_pWiimote, EyeOrigin_t f_Eye, FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM)

+{

+  Vect3D_t l_Dot[2];

+

+  if (f_pWiimote->posDataValid)

+  {

+    l_Dot[0] = Vect3D_t(f_pWiimote->SensorBarDot[0].sx, f_pWiimote->SensorBarDot[0].sy, 0.0);

+    l_Dot[1] = Vect3D_t(f_pWiimote->SensorBarDot[1].sx, f_pWiimote->SensorBarDot[1].sy, 0.0);

+

+    //invert the y-axis

+    //double l_dCamViewHeight = 2.0 * f_FrustumParms.m_dCameraYCenter;

+    l_Dot[0].x = 1016.0 - l_Dot[0].x;

+    l_Dot[1].x = 1016.0 - l_Dot[1].x;

+

+    //calculate the distance of the head (in mm)

+    double l_dHeadDistInMM = f_pWiimote->Zdist,

+           l_dDistanceFactor = f_FrustumParms.m_dEyeDistMM / f_FrustumParms.m_dHeadTrackLedDist;

+    Vect3D_t  l_EyeCam,    //the coords of the eye camera (in wiimote ir-camera coords)

+              l_HeadCenter = (l_Dot[0] + l_Dot[1]) / 2.0;

+

+    //determine the eye position in camera coordinates

+    switch(f_Eye)

+    {

+    case STEREO_LEFT_EYE:

+    case STEREO_RIGHT_EYE:

+        //in order to determine the position of the left and right eyes, we first determine which IR dot is the left one (then 

+        //we also know the right dot). Then we determine the center on the line between the two dots and the direction vector from 

+        //the center to the left/right dot, and add the vector multiplied by the factor between the led distance and eye distance, to

+        //the center coordinates.

+        l_EyeCam = l_HeadCenter + (l_Dot[(int)f_Eye] - l_HeadCenter) * l_dDistanceFactor;

+      break;

+

+    case MONO_CENTER:

+    default:

+        l_EyeCam = l_HeadCenter;

+      break;

+    }

+

+    //calculate the x and y angles of the eye relative to the camera 

+    double l_dEyeXAngle = f_FrustumParms.m_dRadPerCameraPixel * (l_EyeCam.x - f_FrustumParms.m_dCameraXCenter),

+      l_dEyeYAngle = f_FrustumParms.m_dRadPerCameraPixel * (l_EyeCam.y - f_FrustumParms.m_dCameraYCenter);

+

+    //correct the y angle

+    l_dEyeYAngle += f_FrustumParms.m_dYAngleCorrection;

+

+    //calculate the the x,y,z coordinates of the eye relative to the screen (in mm), note that we assume that the camera

+    //is placed above or underneath the center of the screen, so for the x axis we dont correct the position, but for the 

+    //y axis we have to take the y-offset of the camera relative to the center of the screen into account.

+    f_HeadPosInMM.x = sin(l_dEyeXAngle) * l_dHeadDistInMM;

+    f_HeadPosInMM.y = sin(l_dEyeYAngle) * l_dHeadDistInMM - f_FrustumParms.m_dCameraYOffset;

+    f_HeadPosInMM.z = sqrt(l_dHeadDistInMM*l_dHeadDistInMM - 

+                              (f_HeadPosInMM.x*f_HeadPosInMM.x + f_HeadPosInMM.y*f_HeadPosInMM.y)

+                             ); //pythagoras

+    

+    return true;

+  }

+  

+  return false;

+}

+

+void SetFrustum(FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM, bool f_bTranslateEye)

+{

+  //convert the eye position from mm to world coordinates

+  double l_dMmPerWorldCoord = f_FrustumParms.m_dScreenHeightMM / f_FrustumParms.m_dScreenHeightWorld;

+  Vect3D_t l_Eye = f_HeadPosInMM/l_dMmPerWorldCoord;

+

+  //now set a frustum with the near clipping plane at (-1/2 width, -1/2 height, 0), (-1/2 width, -1/2 height, 0) with the 

+  //eye at position (l_dEyeX, l_dEyeY, l_dEyeZ). But because OpenGL expects the eye at position (0,0,0) when specifying a frustum,

+  //we have to specify our near clipping plane with (-l_dEyeX, -l_dEyeY, -l_dEyeZ) as its origin (center) and translate the modelview

+  //matrix to (-l_dEyeX, -l_dEyeY, -l_dEyeZ), such that the world origin is in the center of the screen

+

+  //in order to allow objects to appear in front of the screen we need to move the near clipping plane closer to the eye position,

+  //without changing the frustum. 

+  double l_dHalfHeight = 0.5 * f_FrustumParms.m_dScreenHeightWorld,

+         l_dHalfWidth = l_dHalfHeight * f_FrustumParms.m_dScreenAspect;

+  glMatrixMode(GL_PROJECTION);

+  glLoadIdentity();

+  GLdouble  l_dZNear  = 1.0,

+            l_dFactor = l_dZNear/l_Eye.z,

+            l_dLeft   = (-l_Eye.x - l_dHalfWidth) * l_dFactor,

+            l_dRight  = (-l_Eye.x + l_dHalfWidth) * l_dFactor,

+            l_dBottom = (-l_Eye.y - l_dHalfHeight) * l_dFactor,

+            l_dTop    = (-l_Eye.y + l_dHalfHeight) * l_dFactor;

+

+  glFrustum(l_dLeft, l_dRight, l_dBottom, l_dTop, l_dZNear, l_Eye.z + 100.0);

+

+  if (f_bTranslateEye)

+    glTranslated(-l_Eye.x, -l_Eye.y, -l_Eye.z); //move the frustum back to the position of the eye

+

+  glMatrixMode(GL_MODELVIEW);

+  glLoadIdentity();

+}

diff --git a/matchblox/common/stereoheadtrackfrustum.h b/matchblox/common/stereoheadtrackfrustum.h
new file mode 100644
index 0000000..ed14914
--- /dev/null
+++ b/matchblox/common/stereoheadtrackfrustum.h
@@ -0,0 +1,12 @@
+#ifndef STEREOHEADTRACKFRUSTUM_HEADER_FILE

+

+#define STEREOHEADTRACKFRUSTUM_HEADER_FILE

+

+#include "message_input.h"

+#include "typedefs.h"

+

+

+bool CalcEyePosInMM(input_payload_wiimote *f_pWiimote, EyeOrigin_t f_Eye, FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM);

+void SetFrustum(FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM, bool f_bTranslateEye = true);

+

+#endif //STEREOHEADTRACKFRUSTUM_HEADER_FILE
\ No newline at end of file