From a5f9b9e6831a16ebf9fd331b78dbd728b4c101ef Mon Sep 17 00:00:00 2001 From: Dennis Peeten Date: Sat, 22 Mar 2008 12:21:59 +0000 Subject: Added a folder remotely --- headtrack_demo/src/main.cpp | 411 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 411 insertions(+) create mode 100644 headtrack_demo/src/main.cpp 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 +#include +#include +#include + +#define _USE_MATH_DEFINES +#include + +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 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; +} -- cgit v0.12