Added a folder remotely

1 files changed, 411 insertions, 0 deletions

diff --git a/headtrack_demo/src/main.cpp b/headtrack_demo/src/main.cpp
new file mode 100644
index 0000000..7cf3558
--- /dev/null
+++ b/headtrack_demo/src/main.cpp
@@ -0,0 +1,411 @@
+#include <GL/glut.h>

+#include <wiiuse.h>

+#include <stdio.h>

+#include <string.h>

+

+#define _USE_MATH_DEFINES

+#include <math.h>

+

+struct HeadTrackWiimotes

+{

+  wiimote** m_ppWiimotes;

+  int m_iConnected; 

+} g_Wiimotes;

+

+struct HeadTrackParameters

+{

+  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)

+} g_HeadTrackParms;

+

+enum EyeOrigin {

+    STEREO_LEFT_EYE,

+    STEREO_RIGHT_EYE,

+    MONO_CENTER

+};

+

+inline void FindIRDots(const ir_t *f_pIR, const ir_dot_t *f_pDot[2])

+{

+  //get the first two visible IR dots

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

+  {

+    if (f_pIR->dot[i].visible)

+    {

+      f_pDot[n] = &f_pIR->dot[i];

+      n++;

+    }

+  }

+}

+

+inline double CalcHeadDistInMM(const ir_dot_t *f_pDot[2], HeadTrackParameters *f_pHTParms)

+{

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

+          l_dY = f_pDot[0]->ry - f_pDot[1]->ry,            //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_pHTParms->m_dRadPerCameraPixel * l_dDotDist; //the angle between the lines from the camera through the two ir dots (in radians)

+

+  return (0.5 * f_pHTParms->m_dHeadTrackLedDist) / tan(0.5 * l_dDotAngle); //the distance between the head and camera (in mm)

+}

+

+void SetHeadTrackedFrustum(EyeOrigin f_Eye)

+{

+  //check if there are any wiimotes connected

+  if (g_Wiimotes.m_iConnected > 0)

+  {

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

+    if (g_Wiimotes.m_ppWiimotes[0]->ir.num_dots >= 2)

+    {

+      const ir_dot_t *l_pDot[2];  //shorthand to the ir dots

+      FindIRDots(&g_Wiimotes.m_ppWiimotes[0]->ir, l_pDot);

+      

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

+      double l_dHeadDistInMM = CalcHeadDistInMM(l_pDot, &g_HeadTrackParms); //the distance between the head and camera (in mm)

+

+      double l_dEyeXCam,

+             l_dEyeYCam;

+      //determine the eye position in camera coordinates

+      switch(f_Eye)

+      {

+      case STEREO_LEFT_EYE:

+      case STEREO_RIGHT_EYE:

+          //determine the lower left point

+          int i ;

+          if (l_pDot[0]->rx < l_pDot[1]->rx || (l_pDot[0]->rx == l_pDot[1]->rx && l_pDot[0]->ry < l_pDot[1]->ry))

+            //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 = (double)l_pDot[i]->rx;

+          l_dEyeYCam = (double)l_pDot[i]->ry;

+        break;

+

+      case MONO_CENTER:

+      default:

+          l_dEyeXCam = (double)(l_pDot[0]->rx + l_pDot[1]->rx) / 2.0;

+          l_dEyeYCam = (double)(l_pDot[0]->ry + l_pDot[1]->ry) / 2.0;

+        break;

+      }

+

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

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

+             l_dEyeYAngle = g_HeadTrackParms.m_dRadPerCameraPixel * (l_dEyeYCam - g_HeadTrackParms.m_dCameraYCenter);

+

+      //correct the y angle

+      l_dEyeYAngle += g_HeadTrackParms.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.

+      double l_dEyeX = sin(l_dEyeXAngle) * l_dHeadDistInMM,

+             l_dEyeY = sin(l_dEyeYAngle) * l_dHeadDistInMM - g_HeadTrackParms.m_dCameraYOffset,

+             l_dEyeZ = sqrt(l_dHeadDistInMM*l_dHeadDistInMM - (l_dEyeX*l_dEyeX + l_dEyeY*l_dEyeY)); //pythagoras

+

+      //convert the mm to world coordinates

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

+      l_dEyeX /= l_dMmPerWorldCoord;

+      l_dEyeY /= l_dMmPerWorldCoord;

+      l_dEyeZ /= 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 and translate the modelview

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

+      double l_dHalfHeight = 0.5 * g_HeadTrackParms.m_dScreenHeightWorld,

+             l_dHalfWidth = l_dHalfHeight * g_HeadTrackParms.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);

+      glMatrixMode(GL_MODELVIEW);

+      glLoadIdentity();

+      glTranslated(-l_dEyeX, -l_dEyeY, -l_dEyeZ);

+    }

+  }

+}

+

+void DrawRasterBox()

+{

+  double l_dHalfHeight = 0.5 * g_HeadTrackParms.m_dScreenHeightWorld,

+         l_dHalfWidth = l_dHalfHeight * g_HeadTrackParms.m_dScreenAspect,

+         l_dDH = g_HeadTrackParms.m_dScreenHeightWorld/20.0,

+         l_dDW = l_dDH * g_HeadTrackParms.m_dScreenAspect,

+         l_dBoxDepth = g_HeadTrackParms.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 Display(void)

+{

+  //set perspective correction according to the head position

+  SetHeadTrackedFrustum(MONO_CENTER);

+

+  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, -10.0);

+    glutSolidSphere(5.0, 8, 16);

+  glPopMatrix();*/

+

+  glPushMatrix();

+  glTranslated(0.0, 0.0, 5.0);

+  glScaled(1.0, 1.0, 10.0);

+  glutSolidCube(2.0);

+  glPopMatrix();

+

+

+  /*glPushAttrib(GL_LIGHTING_BIT | GL_DEPTH_BUFFER_BIT);

+  glDisable(GL_LIGHTING);

+  glDisable(GL_DEPTH_TEST);*/

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

+  glRasterPos2i(0, 9);

+  char l_String[] = "TEST!!\0";

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

+   glutBitmapCharacter( GLUT_BITMAP_HELVETICA_18, l_String[i]);*/

+  //glPopAttrib();

+

+  glutSwapBuffers();

+

+}

+

+void PollWiiMotes()

+{

+  //wiimotes report back to the host with a frequency of 100 herz

+  if (wiiuse_poll(g_Wiimotes.m_ppWiimotes, 1))

+  {

+    for (int i=0; i < g_Wiimotes.m_iConnected; i++)

+    {

+      switch (g_Wiimotes.m_ppWiimotes[i]->event)

+      {

+      //default:

+        //nothing??

+      }

+    }

+  }

+}

+

+void Idle(void)

+{

+  PollWiiMotes();

+  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_HeadTrackParms.m_dScreenHeightWorld,

+         l_dHalfWidth = l_dHalfHeight * g_HeadTrackParms.m_dScreenAspect,

+         l_zNear = 2.0 * g_HeadTrackParms.m_dScreenHeightWorld;

+

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

+  glMatrixMode(GL_MODELVIEW);

+  glLoadIdentity();

+  glTranslated(0.0, 0.0, -l_zNear);

+}

+

+

+void ExitProgram()

+{

+  //disconnect wiimotes

+  for (int i=0; i<g_Wiimotes.m_iConnected; i++)

+  {

+    wiiuse_disconnect(g_Wiimotes.m_ppWiimotes[i]);

+  }

+

+  wiiuse_cleanup(g_Wiimotes.m_ppWiimotes, g_Wiimotes.m_iConnected);

+

+  printf("Finished!\n");

+

+  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 ' ':

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

+    if (g_Wiimotes.m_iConnected > 0)

+    {

+      if (g_Wiimotes.m_ppWiimotes[0]->ir.num_dots >= 2)

+      {

+        const ir_dot_t *l_pDot[2];  //shorthand to the ir dots

+        FindIRDots(&g_Wiimotes.m_ppWiimotes[0]->ir, l_pDot);

+      

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

+        double  l_dHeadDistInMM = CalcHeadDistInMM(l_pDot, &g_HeadTrackParms), //the distance between the head and camera (in mm)

+                l_dEyeYCam = (double)(l_pDot[0]->ry + l_pDot[1]->ry) / 2.0;

+      

+      // of the eye relative to the camera 

+      g_HeadTrackParms.m_dYAngleCorrection = g_HeadTrackParms.m_dRadPerCameraPixel * (l_dEyeYCam - g_HeadTrackParms.m_dCameraYCenter);

+      }

+    }

+    break;

+  }

+}

+

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

+{ 

+  //init GLUT

+  glutInit(&argc, argv);

+  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);

+  glutCreateWindow("HeadTrackDemo");

+

+  //init OpenGL

+  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);

+

+  ////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 = 130.0;

+  //g_HeadTrackParms.m_dScreenHeightMM = 210.0;

+  //g_HeadTrackParms.m_dScreenAspect = 4.0/3.0;

+  //g_HeadTrackParms.m_dScreenHeightWorld = 20.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;

+ 

+  //connect to the wiimote(s)

+  g_Wiimotes.m_ppWiimotes = wiiuse_init(1);

+

+#ifdef WIN32

+  wiiuse_set_bluetooth_stack(g_Wiimotes.m_ppWiimotes, 1, WIIUSE_STACK_MS);

+#endif //WIN32

+

+  int found = wiiuse_find(g_Wiimotes.m_ppWiimotes, 1, 30);

+  g_Wiimotes.m_iConnected = wiiuse_connect(g_Wiimotes.m_ppWiimotes , 1); 

+  if (g_Wiimotes.m_iConnected) 

+  {

+    printf("Connected to %i wiimotes (of %i found).\n", g_Wiimotes.m_iConnected, found); 

+

+    wiiuse_set_leds(g_Wiimotes.m_ppWiimotes[0], WIIMOTE_LED_1 | WIIMOTE_LED_2 | WIIMOTE_LED_3 | WIIMOTE_LED_4);

+    wiiuse_rumble(g_Wiimotes.m_ppWiimotes[0], 1);

+    //wiiuse_motion_sensing(g_Wiimotes.m_ppWiimotes[0], 1);

+    wiiuse_set_ir(g_Wiimotes.m_ppWiimotes[0], 1);

+    Sleep(200);

+    wiiuse_rumble(g_Wiimotes.m_ppWiimotes[0], 0);

+  }

+  else 

+  { 

+    printf("Failed to connect to any wiimote.\n"); 

+    return 0; 

+  }

+

+  //register GLUT callback routines

+  glutDisplayFunc(Display);

+  glutIdleFunc(Idle);

+  glutReshapeFunc(Reshape);

+  glutKeyboardFunc(Keyboard);

+  

+  glutFullScreen();

+

+  glutMainLoop();

+

+  return 0;

+}