diff options
Diffstat (limited to 'headtrack_stereo_demo/src/stereoheadtrackfrustum.cpp')
| -rw-r--r-- | headtrack_stereo_demo/src/stereoheadtrackfrustum.cpp | 138 |
1 files changed, 138 insertions, 0 deletions
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();
+}
|