Initial checkin headtrack_stereo_demo

11 files changed, 876 insertions, 0 deletions

diff --git a/headtrack_stereo_demo/src/glutcallbacks.cpp b/headtrack_stereo_demo/src/glutcallbacks.cpp
new file mode 100644
index 0000000..9b582f8
--- /dev/null
+++ b/headtrack_stereo_demo/src/glutcallbacks.cpp
@@ -0,0 +1,187 @@
+#include <wiimote.h>

+#include <GL\glew.h>

+#include <GL\glut.h>

+

+#include "typedefs.h"

+#include "stereoheadtrackfrustum.h"

+#include "scenerenderer.h"

+

+extern GameState_t g_GameState;

+

+void ExitProgram()

+{

+  printf("Finished!\n");

+

+

+  //we prolly should do some cleanup

+  if (g_GameState.m_pTrackingWiimote != NULL)

+  {

+    g_GameState.m_pTrackingWiimote->Disconnect();

+    delete g_GameState.m_pTrackingWiimote;

+  }

+

+  exit(0);

+}

+

+void Keyboard(unsigned char f_cKey, int f_iX, int f_iY)

+{

+  switch(f_cKey)

+  {

+  case 'q':

+  case 'Q':

+  case 27 :

+    ExitProgram();

+    break;

+

+  case 's':

+  case 'S': //enable /disable stereo vision

+    g_GameState.m_bStereoEnabled = !g_GameState.m_bStereoEnabled;

+

+    if (g_GameState.m_bStereoEnabled)

+    {

+      //enable greyscale shader

+      glUseProgram(g_GameState.m_GreyScaleShaderProgram);

+    }

+    else

+    {

+      //disable greyscale shader

+      glUseProgram(0);

+    }

+    break;

+

+  case 'h':

+  case 'H': //enable disable head tracking

+    if (g_GameState.m_pTrackingWiimote != NULL && g_GameState.m_pTrackingWiimote->IsConnected())

+    {

+      g_GameState.m_bHeadTrackingEnabled = !g_GameState.m_bHeadTrackingEnabled;

+    }

+    break;

+

+  case ' ':

+    ////set y angle correction to current y angle of the the center of the two ir dots

+    //if (g_Wiimote.IsConnected())

+    //{

+    //  wiimote_state::ir::dot l_Dot[2];  //shorthand to the ir dots

+    //  //check if we see at least 2 IR dots

+    //  if (FindIRDots(&g_Wiimote.IR, l_Dot))

+    //  {      

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

+    //    double  l_dHeadDistInMM = CalcHeadDistInMM(l_Dot, &g_GameState.m_HeadTrackParms), //the distance between the head and camera (in mm)

+    //            l_dEyeYCam = (double)(l_Dot[0].RawY + l_Dot[1].RawY) / 2.0;

+    //  

+    //    // of the eye relative to the camera 

+    //    g_GameState.m_HeadTrackParms.m_dYAngleCorrection = g_GameState.m_HeadTrackParms.m_dRadPerCameraPixel * (l_dEyeYCam - g_GameState.m_HeadTrackParms.m_dCameraYCenter);

+    //  }

+    //}

+    break;

+  }

+}

+

+void Display(void)

+{

+  glClear(GL_COLOR_BUFFER_BIT);

+

+  double l_d3EyePosInMM[3];

+  bool   l_bEyeLocated;

+

+  if (g_GameState.m_bStereoEnabled)

+  {

+    for (int CurrentEye=0; CurrentEye<2; CurrentEye++) //STEREO_LEFT_EYE = 0, STEREO_RIGHT_EYE = 1

+    {

+      l_bEyeLocated = false;

+      if (g_GameState.m_bHeadTrackingEnabled)

+      {

+        l_bEyeLocated = CalcEyePosInMM(g_GameState.m_pTrackingWiimote, 

+                                       (EyeOrigin_t)CurrentEye, 

+                                       g_GameState.m_FrustumParms, 

+                                       l_d3EyePosInMM);

+      }

+      if(!l_bEyeLocated)

+      {

+        //set default eye coordinates

+        l_d3EyePosInMM[0] = g_GameState.m_FrustumParms.m_dEyeDistMM * ((double)CurrentEye - 0.5);

+        l_d3EyePosInMM[1] = 0.0;

+        l_d3EyePosInMM[2] = g_GameState.m_FrustumParms.m_dDefHeadDistMM;

+      }

+

+      //set colormask

+      if (CurrentEye == STEREO_LEFT_EYE)

+      {

+        glColorMask(GL_TRUE, GL_FALSE, GL_FALSE, GL_TRUE);

+      }

+      else

+      {

+        glColorMask(GL_FALSE, GL_TRUE, GL_TRUE, GL_TRUE);

+      }

+

+      //clear depth buffers

+      glClear(GL_DEPTH_BUFFER_BIT);

+

+      //set frustum

+      SetFrustum(g_GameState.m_FrustumParms, l_d3EyePosInMM);

+

+      RenderScene();

+    }

+    glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);

+  }

+  else // !g_GameState.m_bStereoEnabled

+  {

+    l_bEyeLocated = false;

+    if (g_GameState.m_bHeadTrackingEnabled)

+    {

+      l_bEyeLocated = CalcEyePosInMM(g_GameState.m_pTrackingWiimote, 

+                                     MONO_CENTER, 

+                                     g_GameState.m_FrustumParms, 

+                                     l_d3EyePosInMM);

+    }

+    if(!l_bEyeLocated)

+    {

+      //set default eye coordinates

+      l_d3EyePosInMM[0] = 0.0;

+      l_d3EyePosInMM[1] = 0.0;

+      l_d3EyePosInMM[2] = g_GameState.m_FrustumParms.m_dDefHeadDistMM;

+    }

+

+    //clear depth buffer

+    glClear(GL_DEPTH_BUFFER_BIT);

+

+    //set frustum

+    SetFrustum(g_GameState.m_FrustumParms, l_d3EyePosInMM);

+

+    RenderScene();

+  }

+    

+  glutSwapBuffers();

+

+} // Display

+

+

+void Idle(void)

+{

+  if (g_GameState.m_pTrackingWiimote != NULL && g_GameState.m_pTrackingWiimote->IsConnected())

+  {

+    g_GameState.m_pTrackingWiimote->RefreshState();

+  }

+

+  glutPostRedisplay();

+}

+

+void Reshape(int f_iWidth, int f_iHeight)

+{

+  //set the new viewport dimension

+  glViewport(0, 0, f_iWidth, f_iHeight);

+

+  //should we set a new projection matrix?

+  glMatrixMode(GL_PROJECTION);

+  glLoadIdentity();

+

+  double l_dHalfHeight = 0.5 * g_GameState.m_FrustumParms.m_dScreenHeightWorld,

+         l_dHalfWidth = l_dHalfHeight * g_GameState.m_FrustumParms.m_dScreenAspect,

+         l_zNear = 2.0 * g_GameState.m_FrustumParms.m_dScreenHeightWorld;

+

+  glFrustum(-l_dHalfWidth, l_dHalfWidth, -l_dHalfHeight, l_dHalfHeight, l_zNear, l_zNear + 1000.0);

+  glTranslated(0.0, 0.0, -l_zNear);

+

+  glMatrixMode(GL_MODELVIEW);

+  glLoadIdentity();

+}
\ No newline at end of file
diff --git a/headtrack_stereo_demo/src/glutcallbacks.h b/headtrack_stereo_demo/src/glutcallbacks.h
new file mode 100644
index 0000000..881ddb2
--- /dev/null
+++ b/headtrack_stereo_demo/src/glutcallbacks.h
@@ -0,0 +1,10 @@
+#ifndef GLUTCALLBACKS_HEADER_FILE

+

+#define GLUTCALLBACKS_HEADER_FILE

+

+void Keyboard(unsigned char f_cKey, int f_iX, int f_iY);

+void Reshape(int f_iWidth, int f_iHeight);

+void Idle(void);

+void Display(void);

+

+#endif //GLUTCALLBACKS_HEADER_FILE
\ No newline at end of file
diff --git a/headtrack_stereo_demo/src/main.cpp b/headtrack_stereo_demo/src/main.cpp
new file mode 100644
index 0000000..40e726f
--- /dev/null
+++ b/headtrack_stereo_demo/src/main.cpp
@@ -0,0 +1,181 @@
+#include <wiimote.h>

+#include <GL/glew.h>

+#include <GL/glut.h>

+

+#define _USE_MATH_DEFINES

+#include <math.h>

+#include <stdio.h>

+#include <string.h>

+

+#include "typedefs.h"

+#include "glutcallbacks.h"

+#include "textfile.h"

+

+//Global variables

+GameState_t  g_GameState;

+

+void InitGL(void)

+{

+  GLfloat l_f4LightPos[]      = {0.0, 1.0, 1.0, 0.0},

+          l_f4GlobalAmbient[] = {0.3, 0.3, 0.3, 1.0};

+  //init OpenGL

+  glLightfv(GL_LIGHT0, GL_POSITION, l_f4LightPos);

+  glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, 0);

+  glLightModelfv(GL_LIGHT_MODEL_AMBIENT, l_f4GlobalAmbient);

+  glEnable(GL_LIGHTING);

+  glEnable(GL_LIGHT0);

+  

+  glEnable(GL_DEPTH_TEST);

+  glEnable(GL_COLOR_MATERIAL);

+  glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);

+

+  glClearColor(1.0, 1.0, 1.0, 1.0);

+  glClearDepth(1000.0);

+  glDisable(GL_DITHER);

+  glShadeModel(GL_SMOOTH);

+  //glEnable(GL_NORMALIZE); //why??

+  glEnable(GL_CULL_FACE);

+  

+}

+

+bool InitWiiMotes()

+{

+  g_GameState.m_pTrackingWiimote = new wiimote();

+

+  g_GameState.m_pTrackingWiimote->ChangedCallback = NULL; //no callbacks, we just poll...

+	g_GameState.m_pTrackingWiimote->CallbackTriggerFlags = NO_CHANGE;

+

+  /*printf("connecting:\n");

+  while(!g_Wiimote.Connect(wiimote::FIRST_AVAILABLE))

+  {

+    Sleep(1000);

+    printf(".");

+  }

+  

+  printf("connected\n");*/

+

+  if (g_GameState.m_pTrackingWiimote->Connect(wiimote::FIRST_AVAILABLE))

+  {

+    g_GameState.m_pTrackingWiimote->SetLEDs(0x0f);

+    

+    g_GameState.m_pTrackingWiimote->SetReportType(wiimote::IN_BUTTONS_ACCEL_IR_EXT);	 //    IR dots

+    /*g_GameState.m_pTrackingWiimote->SetRumble(true);  

+    Sleep(500);

+    g_GameState.m_pTrackingWiimote->SetRumble(false); */ 

+

+    return true;

+  }

+  else

+  {

+    return false;

+  }

+}

+

+bool LoadGreyScaleShader()

+{

+  char *vs_text = NULL,*fs_text = NULL;

+  GLuint  vs = glCreateShader(GL_VERTEX_SHADER),

+	        fs = glCreateShader(GL_FRAGMENT_SHADER);

+	

+	vs_text = textFileRead("shaders\\greyscale.vert");

+  if (vs_text == NULL)

+  {

+    return false;

+  }

+	fs_text = textFileRead("shaders\\greyscale.frag");

+  if (fs_text == NULL)

+  {

+    free(vs_text);

+    return false;

+  }

+

+	glShaderSource(vs, 1, (const char**)&vs_text,NULL);

+	glShaderSource(fs, 1, (const char**)&fs_text,NULL);

+	

+	free(vs_text);

+  free(fs_text);

+

+	glCompileShader(vs);

+	glCompileShader(fs);

+	

+  g_GameState.m_GreyScaleShaderProgram = glCreateProgram();

+	glAttachShader(g_GameState.m_GreyScaleShaderProgram,fs);

+	glAttachShader(g_GameState.m_GreyScaleShaderProgram,vs);

+

+	glLinkProgram(g_GameState.m_GreyScaleShaderProgram);

+

+  return true;

+}

+

+int main(int argc, char **argv)

+{ 

+  //init the GL Utility Toolkit

+  glutInit(&argc, argv);

+  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);

+  glutCreateWindow("HeadTrackDemo");

+

+  //register GLUT callback routines

+  glutDisplayFunc(Display);

+  glutIdleFunc(Idle);

+  glutReshapeFunc(Reshape);

+  glutKeyboardFunc(Keyboard);

+

+  //init openGL state variables

+  InitGL();

+  

+  //init the GL Extension Wrangler library

+  glewInit();

+

+  //check for OpenGL 2.0 support

+	if (!glewIsSupported("GL_VERSION_2_0"))

+  {

+		printf("Your graphics hardware/software does not support OpenGL 2.0\n");

+		exit(1);

+	}

+

+  //try to load the grayscale shader

+	if (!LoadGreyScaleShader())

+  {

+    printf("Failed to load vertex/fragment shader.\n");

+		exit(1);

+  }

+

+  // initialise the stereo vision parameters

+  g_GameState.m_FrustumParms.m_dEyeDistMM = 70.0; //7 cm

+  g_GameState.m_FrustumParms.m_dDefHeadDistMM = 1000.0; //100 cm

+  g_GameState.m_FrustumParms.m_dHeadTrackLedDist = 205.0; 

+  g_GameState.m_FrustumParms.m_dRadPerCameraPixel = (41.0*(M_PI/180.0))/1024.0;

+  g_GameState.m_FrustumParms.m_dCameraXCenter = 1024.0/2.0;

+  g_GameState.m_FrustumParms.m_dCameraYCenter = 768.0/2.0;

+  g_GameState.m_FrustumParms.m_dYAngleCorrection = 0.0; //to be initialised correctly

+  g_GameState.m_FrustumParms.m_dScreenHeightWorld = 20.0;

+  g_GameState.m_FrustumParms.m_dCameraYOffset = 130.0;

+  g_GameState.m_FrustumParms.m_dScreenHeightMM = 210.0;

+  g_GameState.m_FrustumParms.m_dScreenAspect = 4.0/3.0;

+  //g_GameState.m_FrustumParms.m_dCameraYOffset = 205.0;

+  //g_GameState.m_FrustumParms.m_dScreenHeightMM = 320.0;

+  //g_GameState.m_FrustumParms.m_dScreenAspect = 16.0/10.0;

+  

+  g_GameState.m_bHeadTrackingEnabled = false;

+  g_GameState.m_bStereoEnabled = false;

+

+  InitWiiMotes();

+

+  glutFullScreen();

+  

+  //action!!

+  glutMainLoop();

+

+  return 0;

+}

+

+////initialise the parameters needed for perspective correction when head tracking

+  //g_HeadTrackParms.m_dHeadTrackLedDist = 205.0; 

+  //g_HeadTrackParms.m_dRadPerCameraPixel = (41.0*(M_PI/180.0))/1024.0;

+  //g_HeadTrackParms.m_dCameraXCenter = 1024.0/2.0;

+  //g_HeadTrackParms.m_dCameraYCenter = 768.0/2.0;

+  //g_HeadTrackParms.m_dYAngleCorrection = 0.0; //to be initialised correctly

+  //g_HeadTrackParms.m_dCameraYOffset = 205.0;

+  //g_HeadTrackParms.m_dScreenHeightMM = 320.0;

+  //g_HeadTrackParms.m_dScreenAspect = 16.0/10.0;

+  //g_HeadTrackParms.m_dScreenHeightWorld = 20.0;
\ No newline at end of file
diff --git a/headtrack_stereo_demo/src/scenerenderer.cpp b/headtrack_stereo_demo/src/scenerenderer.cpp
new file mode 100644
index 0000000..15c0a73
--- /dev/null
+++ b/headtrack_stereo_demo/src/scenerenderer.cpp
@@ -0,0 +1,164 @@
+#include <GL\glut.h>

+#include "typedefs.h"

+

+extern GameState_t g_GameState;

+

+void DrawRasterBox()

+{

+  double l_dHalfHeight = 0.5 * g_GameState.m_FrustumParms.m_dScreenHeightWorld,

+         l_dHalfWidth = l_dHalfHeight * g_GameState.m_FrustumParms.m_dScreenAspect,

+         l_dDH = g_GameState.m_FrustumParms.m_dScreenHeightWorld/20.0,

+         l_dDW = l_dDH * g_GameState.m_FrustumParms.m_dScreenAspect,

+         l_dBoxDepth = g_GameState.m_FrustumParms.m_dScreenHeightWorld,

+         l_dVariable;

+  

+  for (int i = 0; i <= 20; i++)

+  {

+    l_dVariable = -l_dHalfHeight + (double)i * l_dDH;

+    //draw U on xz

+    glBegin(GL_LINE_STRIP);

+    glColor3d(0.0, 0.0, 1.0);

+    glVertex3d(-l_dHalfWidth, l_dVariable, 0.0);

+    glVertex3d(-l_dHalfWidth, l_dVariable, -l_dBoxDepth);

+    glVertex3d(l_dHalfWidth, l_dVariable, -l_dBoxDepth);

+    glVertex3d(l_dHalfWidth, l_dVariable, 0.0);

+    glEnd();

+

+    //draw U on yz

+    l_dVariable = -l_dHalfWidth + (double)i * l_dDW;

+    glBegin(GL_LINE_STRIP);

+    glVertex3d(l_dVariable, -l_dHalfHeight, 0.0);

+    glVertex3d(l_dVariable, -l_dHalfHeight, -l_dBoxDepth);

+    glVertex3d(l_dVariable, l_dHalfHeight, -l_dBoxDepth);

+    glVertex3d(l_dVariable, l_dHalfHeight, 0.0);

+    glEnd();

+

+    //draw square on xy

+    l_dVariable = (double)i * -l_dDH;

+    glBegin(GL_LINE_LOOP);

+    glVertex3d(-l_dHalfWidth, -l_dHalfHeight, l_dVariable);

+    glVertex3d(l_dHalfWidth, -l_dHalfHeight, l_dVariable);

+    glVertex3d(l_dHalfWidth, l_dHalfHeight, l_dVariable);

+    glVertex3d(-l_dHalfWidth, l_dHalfHeight, l_dVariable);

+    glEnd();

+  }

+}

+

+//void RenderTimers()

+//{

+//  int l_iWidth = glutGet(GLUT_WINDOW_WIDTH),

+//      l_iHeight = glutGet(GLUT_WINDOW_HEIGHT);

+//

+//  //set up a temporary orthographic projection

+//  //save the current projection matrix

+//  glMatrixMode(GL_PROJECTION);

+//  glPushMatrix();

+//  glLoadIdentity();

+//  //set up an orthographic projection matrix

+//  glOrtho(0.0, (GLdouble)l_iWidth, 0.0, (GLdouble)l_iHeight, -1.0, 1.0);

+//  glScaled(1.0, -1.0, 1.0); //invert y-axis

+//  glTranslated(0.0, (GLdouble)-l_iHeight, 0.0); //translate the origin to the upper left corner of the screen

+//

+//  glMatrixMode(GL_MODELVIEW);

+//  glLoadIdentity();

+//

+//  //save the current opengl lighting and depth buffering attributes

+//  glPushAttrib(GL_LIGHTING_BIT | GL_DEPTH_BUFFER_BIT);

+//  glDisable(GL_LIGHTING); //disable lighting

+//  glDisable(GL_DEPTH_TEST); //disable depth testing

+//

+//  //draw a background rectangle

+//  glBegin(GL_POLYGON);

+//    glColor3d(0.5, 0.5, 0.5);

+//    glVertex2i(0, 0);

+//    glVertex2i(0, 50);

+//    glVertex2i(250, 50);

+//    glVertex2i(250, 0);

+//  glEnd();

+//

+//  //draw the frame timer

+//  char l_pFpsString[256];

+//

+//  sprintf_s(l_pFpsString, 256, "FPS  : %03.1f", g_pFrameTimer->getTicksPerSecond());

+//  size_t l_sStrLen = strlen(l_pFpsString);

+//  glColor3d(0.0, 0.0, 0.0);

+//  glRasterPos2i(5, 13);

+//  for (size_t i=0; i<l_sStrLen; i++)

+//    glutBitmapCharacter(GLUT_BITMAP_8_BY_13, l_pFpsString[i]);

+//

+//  sprintf_s(l_pFpsString, 256, "(Avg: %03.1f)", g_pFrameTimer->getAvgTicksPerSecond());

+//  l_sStrLen = strlen(l_pFpsString);

+//  //glColor3d(0.0, 0.0, 1.0);

+//  glRasterPos2i(125, 13);

+//  for (size_t i=0; i<l_sStrLen; i++)

+//    glutBitmapCharacter(GLUT_BITMAP_8_BY_13, l_pFpsString[i]);

+//

+//  //draw the poll timer

+//  sprintf_s(l_pFpsString, 256, "POLLS: %03.1f", g_pPollTimer->getTicksPerSecond());

+//  l_sStrLen = strlen(l_pFpsString);

+//  //glColor3d(0.0, 0.0, 1.0);

+//  glRasterPos2i(5, 39);

+//  for (size_t i=0; i<l_sStrLen; i++)

+//    glutBitmapCharacter(GLUT_BITMAP_8_BY_13, l_pFpsString[i]);

+//

+//  sprintf_s(l_pFpsString, 256, "(Avg: %03.1f)", g_pPollTimer->getAvgTicksPerSecond());

+//  l_sStrLen = strlen(l_pFpsString);

+//  //glColor3d(0.0, 0.0, 1.0);

+//  glRasterPos2i(125, 39);

+//  for (size_t i=0; i<l_sStrLen; i++)

+//    glutBitmapCharacter(GLUT_BITMAP_8_BY_13, l_pFpsString[i]);

+//

+//  glPopAttrib(); //restore attributes

+//

+//  glMatrixMode(GL_PROJECTION);

+//  glPopMatrix(); //restore the previous projection matrix

+//

+//  glMatrixMode(GL_MODELVIEW);

+//  glLoadIdentity();

+//}

+

+void RenderScene(void)

+{

+  //set perspective correction according to the head position

+  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

+

+  DrawRasterBox();

+

+  glPushMatrix();

+    glColor3d(1.0, 0.0, 0.0);

+    glTranslated(-5.0, 5.0, -15.0);

+    glutSolidCube(5.0);

+    glColor3d(0.0, 1.0, 0.0);

+    glTranslated(10.0, 0.0, 0.0);

+    glutSolidCube(5.0);

+    glColor3d(0.0, 0.0, 1.0);

+    glTranslated(0.0, -10.0, 0.0);

+    glutSolidCube(5.0);

+    glColor3d(1.0, 1.0, 0.0);

+    glTranslated(-10.0, 0.0, 0.0);

+    glutSolidCube(5.0);

+  glPopMatrix();

+

+  glPushMatrix();

+    glColor3d(1.0, 1.0, 0.0);

+    glTranslated(-5.0, 5.0, -5.0);

+    glutSolidCube(5.0);

+    glColor3d(0.0, 0.0, 1.0);

+    glTranslated(10.0, 0.0, 0.0);

+    glutSolidCube(5.0);

+    glColor3d(0.0, 1.0, 0.0);

+    glTranslated(0.0, -10.0, 0.0);

+    glutSolidCube(5.0);

+    glColor3d(1.0, 0.0, 0.0);

+    glTranslated(-10.0, 0.0, 0.0);

+    glutSolidCube(5.0);

+  glPopMatrix();

+  

+  glColor3d(1.0, 0.0, 1.0);

+  glPushMatrix();

+  glTranslated(0.0, 0.0, 5.0);

+  //glScaled(1.0, 1.0, 10.0);

+  glutSolidSphere(2.0, 8, 16);

+  glPopMatrix();

+ 

+}
\ No newline at end of file
diff --git a/headtrack_stereo_demo/src/scenerenderer.h b/headtrack_stereo_demo/src/scenerenderer.h
new file mode 100644
index 0000000..53e0923
--- /dev/null
+++ b/headtrack_stereo_demo/src/scenerenderer.h
@@ -0,0 +1,7 @@
+#ifndef SCENERENDERER_HEADER_FILE

+

+#define SCENERENDERER_HEADER_FILE

+

+void RenderScene(void);

+

+#endif //SCENERENDERER_HEADER_FILE
\ No newline at end of file
diff --git a/headtrack_stereo_demo/src/stereoheadtrackfrustum.cpp b/headtrack_stereo_demo/src/stereoheadtrackfrustum.cpp
new file mode 100644
index 0000000..7121d77
--- /dev/null
+++ b/headtrack_stereo_demo/src/stereoheadtrackfrustum.cpp
@@ -0,0 +1,138 @@
+#include <wiimote.h>

+#include <GL\glut.h>

+

+#define _USE_MATH_DEFINES

+#include <math.h>

+

+#include "stereoheadtrackfrustum.h"

+

+bool FindIRDots(wiimote_state::ir &f_IR, wiimote_state::ir::dot f_Dot[2])

+{

+  //get the first two visible IR dots

+  int n = 0;

+  for(int i = 0; i < 4 && n < 2; i++)

+  {

+    if (f_IR.Dot[i].bFound)

+    {

+      f_Dot[n] = f_IR.Dot[i];

+      //invert the x axis

+      f_Dot[n].RawX = f_IR.MAX_RAW_X - f_Dot[n].RawX;

+      n++;

+    }

+  }

+  return n == 2;

+}

+

+double CalcHeadDistInMM(wiimote_state::ir::dot f_Dot[2], FrustumParms_t &f_FrustumParms)

+{

+  double  l_dX = (double)f_Dot[0].RawX - f_Dot[1].RawX,            //difference in x coordinates

+          l_dY = (double)f_Dot[0].RawY - f_Dot[1].RawY,            //difference in y coordinates

+          //l_dX = f_pDot[0]->RawX - f_pDot[1]->RawX,            //difference in x coordinates

+          //l_dY = f_pDot[0]->RawY - f_pDot[1]->RawY,            //difference in y coordinates

+          l_dDotDist = sqrt(l_dX*l_dX + l_dY*l_dY),        //distance between ir dots (in camera pixels)

+          l_dDotAngle = f_FrustumParms.m_dRadPerCameraPixel * l_dDotDist; //the angle between the lines from the camera through the two ir dots (in radians)

+

+  return (0.5 * f_FrustumParms.m_dHeadTrackLedDist) / tan(0.5 * l_dDotAngle); //the distance between the head and camera (in mm)

+}

+

+

+bool CalcEyePosInMM(wiimote *f_pWiimote, EyeOrigin_t f_Eye, FrustumParms_t &f_FrustumParms, double f_d3HeadPosInMM[3])

+{

+  wiimote_state::ir::dot l_Dot[2];

+

+  if (f_pWiimote != NULL && f_pWiimote->IsConnected())

+  {

+    if (FindIRDots(f_pWiimote->IR, l_Dot))

+    {

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

+      double l_dHeadDistInMM = CalcHeadDistInMM(l_Dot, f_FrustumParms); //the distance between the head and camera (in mm)

+

+      double l_dEyeXCam,    //the x-coord (in wiimote ir-camera coords)

+             l_dEyeYCam,    //the y-coord (in wiimote ir-camera coords)

+             l_dDistanceFactor = f_FrustumParms.m_dEyeDistMM / f_FrustumParms.m_dHeadTrackLedDist, 

+             l_dHeadXCenter = (double)(l_Dot[0].RawX + l_Dot[1].RawX) / 2.0,

+             l_dHeadYCenter = (double)(l_Dot[0].RawY + l_Dot[1].RawY) / 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.

+          int i ;

+          if (l_Dot[0].RawX < l_Dot[1].RawX || (l_Dot[0].RawX == l_Dot[1].RawX && l_Dot[0].RawY < l_Dot[1].RawY))

+            //0 is the left eye 1 is the right eye

+            i = (f_Eye==STEREO_LEFT_EYE? 0: 1);

+          else

+            //1 is the left eye 0 is the right eye

+            i = (f_Eye==STEREO_LEFT_EYE? 1: 0);

+

+          l_dEyeXCam = l_dHeadXCenter + (((double)l_Dot[i].RawX - l_dHeadXCenter) * l_dDistanceFactor);

+          l_dEyeYCam = l_dHeadYCenter + (((double)l_Dot[i].RawY - l_dHeadYCenter) * l_dDistanceFactor);

+        break;

+

+      case MONO_CENTER:

+      default:

+          l_dEyeXCam = l_dHeadXCenter;

+          l_dEyeYCam = l_dHeadYCenter;

+        break;

+      }

+

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

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

+             l_dEyeYAngle = f_FrustumParms.m_dRadPerCameraPixel * (l_dEyeYCam - 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_d3HeadPosInMM[0] = sin(l_dEyeXAngle) * l_dHeadDistInMM;

+      f_d3HeadPosInMM[1] = sin(l_dEyeYAngle) * l_dHeadDistInMM - f_FrustumParms.m_dCameraYOffset;

+      f_d3HeadPosInMM[2] = sqrt(l_dHeadDistInMM*l_dHeadDistInMM - 

+                                (f_d3HeadPosInMM[0]*f_d3HeadPosInMM[0] + f_d3HeadPosInMM[1]*f_d3HeadPosInMM[1])

+                               ); //pythagoras

+      

+      return true;

+    }

+  }

+  

+  return false;

+}

+

+void SetFrustum(FrustumParms_t &f_FrustumParms, double f_d3HeadPosInMM[3])

+{

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

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

+         l_dEyeX = f_d3HeadPosInMM[0]/l_dMmPerWorldCoord,

+         l_dEyeY = f_d3HeadPosInMM[1]/l_dMmPerWorldCoord,

+         l_dEyeZ = f_d3HeadPosInMM[2]/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_dEyeZ,

+            l_dLeft   = (-l_dEyeX - l_dHalfWidth) * l_dFactor,

+            l_dRight  = (-l_dEyeX + l_dHalfWidth) * l_dFactor,

+            l_dBottom = (-l_dEyeY - l_dHalfHeight) * l_dFactor,

+            l_dTop    = (-l_dEyeY + l_dHalfHeight) * l_dFactor;

+

+  glFrustum(l_dLeft, l_dRight, l_dBottom, l_dTop, l_dZNear, l_dEyeZ + 100.0);

+  glTranslated(-l_dEyeX, -l_dEyeY, -l_dEyeZ); //move the frustum back to the position of the eye

+

+  glMatrixMode(GL_MODELVIEW);

+  glLoadIdentity();

+}

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

+

+#define STEREOHEADTRACKFRUSTUM_HEADER_FILE

+

+#include <wiimote.h>

+#include "typedefs.h"

+

+

+bool CalcEyePosInMM(wiimote *f_pWiimote, EyeOrigin_t f_Eye, FrustumParms_t &f_FrustumParms, double f_d3HeadPosInMM[3]);

+void SetFrustum(FrustumParms_t &f_FrustumParms, double f_d3HeadPosInMM[3]);

+

+#endif //STEREOHEADTRACKFRUSTUM_HEADER_FILE
\ No newline at end of file
diff --git a/headtrack_stereo_demo/src/textfile.cpp b/headtrack_stereo_demo/src/textfile.cpp
new file mode 100644
index 0000000..0b74282
--- /dev/null
+++ b/headtrack_stereo_demo/src/textfile.cpp
@@ -0,0 +1,69 @@
+// textfile.cpp

+//

+// simple reading and writing for text files

+//

+// www.lighthouse3d.com

+//

+// You may use these functions freely.

+// they are provided as is, and no warranties, either implicit,

+// or explicit are given

+//////////////////////////////////////////////////////////////////////

+

+

+#include <stdio.h>

+#include <stdlib.h>

+#include <string.h>

+

+

+char *textFileRead(char *fn) {

+

+

+	FILE *fp;

+	char *content = NULL;

+

+	int count=0;

+

+	if (fn != NULL) {

+		fp = fopen(fn,"rt");

+

+		if (fp != NULL) {

+      

+      fseek(fp, 0, SEEK_END);

+      count = ftell(fp);

+      rewind(fp);

+

+			if (count > 0) {

+				content = (char *)malloc(sizeof(char) * (count+1));

+				count = fread(content,sizeof(char),count,fp);

+				content[count] = '\0';

+			}

+			fclose(fp);

+		}

+	}

+	return content;

+}

+

+int textFileWrite(char *fn, char *s) {

+

+	FILE *fp;

+	int status = 0;

+

+	if (fn != NULL) {

+		fp = fopen(fn,"w");

+

+		if (fp != NULL) {

+			

+			if (fwrite(s,sizeof(char),strlen(s),fp) == strlen(s))

+				status = 1;

+			fclose(fp);

+		}

+	}

+	return(status);

+}

+

+

+

+

+

+

+

diff --git a/headtrack_stereo_demo/src/textfile.h b/headtrack_stereo_demo/src/textfile.h
new file mode 100644
index 0000000..1c30e53
--- /dev/null
+++ b/headtrack_stereo_demo/src/textfile.h
@@ -0,0 +1,10 @@
+// textfile.h: interface for reading and writing text files

+// www.lighthouse3d.com

+//

+// You may use these functions freely.

+// they are provided as is, and no warranties, either implicit,

+// or explicit are given

+//////////////////////////////////////////////////////////////////////

+

+char *textFileRead(char *fn);

+int textFileWrite(char *fn, char *s);

diff --git a/headtrack_stereo_demo/src/ticktimer.h b/headtrack_stereo_demo/src/ticktimer.h
new file mode 100644
index 0000000..e577255
--- /dev/null
+++ b/headtrack_stereo_demo/src/ticktimer.h
@@ -0,0 +1,62 @@
+#ifndef _TICKTIMER_H

+#define _TICKTIMER_H

+

+#include <ctime>

+#include <iostream>

+

+class TickTimer

+{

+public:

+  TickTimer(double f_dUpdateDelta)

+  {

+    m_dUpdateDelta = f_dUpdateDelta;

+    m_iNrOfTicks = 0;

+    m_dTicksPS = m_dRawTicksPS = m_dAvgTicksPS = m_dRawAvgTicksPS = 0.0;

+  }

+

+  void tick()

+  {

+    //DWORD  l_CurClock = timeGetTime();

+    time_t  l_CurClock = clock();

+    if (m_iNrOfTicks == 0)

+    {

+      m_FirstClock = m_LastClock = l_CurClock;

+    }

+        

+    //update the ticks per second, based on the last tick period

+    m_dRawTicksPS = (double)CLOCKS_PER_SEC / (double)(l_CurClock - m_LastClock);

+    //m_dRawTicksPS = 1000.0 / (double)(l_CurClock - m_LastClock);

+    double delta = (m_dRawTicksPS - m_dTicksPS);

+    if ( (delta >= m_dUpdateDelta) || (delta <= -m_dUpdateDelta) )

+      m_dTicksPS = m_dRawTicksPS;

+

+    m_LastClock = l_CurClock;

+

+    //update the ticks per second, based on the average tick period

+    m_iNrOfTicks++;

+    m_dRawAvgTicksPS = (double)m_iNrOfTicks/((double)(m_LastClock - m_FirstClock)/(double)CLOCKS_PER_SEC);

+    //m_dRawAvgTicksPS = (double)m_iNrOfTicks/((double)(m_LastClock - m_FirstClock)/1000.0);

+    delta = (m_dRawAvgTicksPS - m_dAvgTicksPS);

+    if ( (delta >= m_dUpdateDelta) || (delta <= -m_dUpdateDelta) )

+      m_dAvgTicksPS = m_dRawAvgTicksPS;

+  }

+

+  inline double getTicksPerSecond() { return m_dTicksPS; }

+  inline double getRawTicksPerSecond() { return m_dRawTicksPS; }

+  inline double getAvgTicksPerSecond() { return m_dAvgTicksPS; }

+  inline double getRawAvgTicksPerSecond() { return m_dRawAvgTicksPS; }

+

+private:

+  int     m_iNrOfTicks;

+  double  m_dUpdateDelta,

+          m_dTicksPS,

+          m_dRawTicksPS,

+          m_dAvgTicksPS,

+          m_dRawAvgTicksPS;

+  clock_t m_FirstClock,

+          m_LastClock;

+  /*DWORD   m_FirstClock,

+          m_LastClock;*/

+};

+

+#endif //_TICKTIMER_H
\ No newline at end of file
diff --git a/headtrack_stereo_demo/src/typedefs.h b/headtrack_stereo_demo/src/typedefs.h
new file mode 100644
index 0000000..55e2da5
--- /dev/null
+++ b/headtrack_stereo_demo/src/typedefs.h
@@ -0,0 +1,36 @@
+#ifndef TYPEDEFS_HEADER_FILE

+

+#define TYPEDEFS_HEADER_FILE

+

+#include <wiimote.h>

+

+typedef struct FrustumParms

+{

+  double m_dHeadTrackLedDist;       //distance between leds on head in millimeters

+  double m_dRadPerCameraPixel;      //radians per camera pixel

+  double m_dCameraXCenter;          //the coordinates of the center of the camera

+  double m_dCameraYCenter;          //

+  double m_dYAngleCorrection;       //the correction added to the verticle angle measured by the camera (in radians)

+  double m_dCameraYOffset;          //the offset in Y direction of the camera relative to the center of the screen (in mm)

+  double m_dScreenHeightMM;         //the height of the screen (in mm)

+  double m_dScreenAspect;           //the aspect ratio of the screen (width/height)

+  double m_dScreenHeightWorld;      //the height of the screen (in world coordinates)

+  double m_dEyeDistMM;              //distance between the eyes (in mm)

+  double m_dDefHeadDistMM;          //default distance between head and display (in mm)

+} FrustumParms_t;

+

+typedef enum EyeOrigin {

+    STEREO_LEFT_EYE  = 0,

+    STEREO_RIGHT_EYE = 1,

+    MONO_CENTER      = 2

+} EyeOrigin_t;

+

+typedef struct GameState

+{

+  FrustumParms_t  m_FrustumParms;

+  bool            m_bHeadTrackingEnabled;

+  bool            m_bStereoEnabled;

+  wiimote         *m_pTrackingWiimote;

+  GLuint          m_GreyScaleShaderProgram;

+} GameState_t;

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