#include #include #define _USE_MATH_DEFINES #include #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(); }