From 77bd36a4977e64e2cdd53c04dfa5ca212d56b14e Mon Sep 17 00:00:00 2001 From: Dennis Peeten Date: Wed, 28 May 2008 07:35:25 +0000 Subject: --- matchblox/common/stereoheadtrackfrustum.cpp | 101 ++++++++++++++++++++++++++++ matchblox/common/stereoheadtrackfrustum.h | 12 ++++ 2 files changed, 113 insertions(+) create mode 100644 matchblox/common/stereoheadtrackfrustum.cpp create mode 100644 matchblox/common/stereoheadtrackfrustum.h diff --git a/matchblox/common/stereoheadtrackfrustum.cpp b/matchblox/common/stereoheadtrackfrustum.cpp new file mode 100644 index 0000000..d43ba4e --- /dev/null +++ b/matchblox/common/stereoheadtrackfrustum.cpp @@ -0,0 +1,101 @@ +#include + +#define _USE_MATH_DEFINES +#include + +#include "stereoheadtrackfrustum.h" +#include "typedefs.h" +#include "message_input.h" + +bool CalcEyePosInMM(input_payload_wiimote *f_pWiimote, EyeOrigin_t f_Eye, FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM) +{ + Vect3D_t l_Dot[2]; + + if (f_pWiimote->posDataValid) + { + l_Dot[0] = Vect3D_t(f_pWiimote->SensorBarDot[0].sx, f_pWiimote->SensorBarDot[0].sy, 0.0); + l_Dot[1] = Vect3D_t(f_pWiimote->SensorBarDot[1].sx, f_pWiimote->SensorBarDot[1].sy, 0.0); + + //invert the y-axis + //double l_dCamViewHeight = 2.0 * f_FrustumParms.m_dCameraYCenter; + l_Dot[0].x = 1016.0 - l_Dot[0].x; + l_Dot[1].x = 1016.0 - l_Dot[1].x; + + //calculate the distance of the head (in mm) + double l_dHeadDistInMM = f_pWiimote->Zdist, + l_dDistanceFactor = f_FrustumParms.m_dEyeDistMM / f_FrustumParms.m_dHeadTrackLedDist; + Vect3D_t l_EyeCam, //the coords of the eye camera (in wiimote ir-camera coords) + l_HeadCenter = (l_Dot[0] + l_Dot[1]) / 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. + l_EyeCam = l_HeadCenter + (l_Dot[(int)f_Eye] - l_HeadCenter) * l_dDistanceFactor; + break; + + case MONO_CENTER: + default: + l_EyeCam = l_HeadCenter; + break; + } + + //calculate the x and y angles of the eye relative to the camera + double l_dEyeXAngle = f_FrustumParms.m_dRadPerCameraPixel * (l_EyeCam.x - f_FrustumParms.m_dCameraXCenter), + l_dEyeYAngle = f_FrustumParms.m_dRadPerCameraPixel * (l_EyeCam.y - 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_HeadPosInMM.x = sin(l_dEyeXAngle) * l_dHeadDistInMM; + f_HeadPosInMM.y = sin(l_dEyeYAngle) * l_dHeadDistInMM - f_FrustumParms.m_dCameraYOffset; + f_HeadPosInMM.z = sqrt(l_dHeadDistInMM*l_dHeadDistInMM - + (f_HeadPosInMM.x*f_HeadPosInMM.x + f_HeadPosInMM.y*f_HeadPosInMM.y) + ); //pythagoras + + return true; + } + + return false; +} + +void SetFrustum(FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM, bool f_bTranslateEye) +{ + //convert the eye position from mm to world coordinates + double l_dMmPerWorldCoord = f_FrustumParms.m_dScreenHeightMM / f_FrustumParms.m_dScreenHeightWorld; + Vect3D_t l_Eye = f_HeadPosInMM/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_Eye.z, + l_dLeft = (-l_Eye.x - l_dHalfWidth) * l_dFactor, + l_dRight = (-l_Eye.x + l_dHalfWidth) * l_dFactor, + l_dBottom = (-l_Eye.y - l_dHalfHeight) * l_dFactor, + l_dTop = (-l_Eye.y + l_dHalfHeight) * l_dFactor; + + glFrustum(l_dLeft, l_dRight, l_dBottom, l_dTop, l_dZNear, l_Eye.z + 100.0); + + if (f_bTranslateEye) + glTranslated(-l_Eye.x, -l_Eye.y, -l_Eye.z); //move the frustum back to the position of the eye + + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); +} diff --git a/matchblox/common/stereoheadtrackfrustum.h b/matchblox/common/stereoheadtrackfrustum.h new file mode 100644 index 0000000..ed14914 --- /dev/null +++ b/matchblox/common/stereoheadtrackfrustum.h @@ -0,0 +1,12 @@ +#ifndef STEREOHEADTRACKFRUSTUM_HEADER_FILE + +#define STEREOHEADTRACKFRUSTUM_HEADER_FILE + +#include "message_input.h" +#include "typedefs.h" + + +bool CalcEyePosInMM(input_payload_wiimote *f_pWiimote, EyeOrigin_t f_Eye, FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM); +void SetFrustum(FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM, bool f_bTranslateEye = true); + +#endif //STEREOHEADTRACKFRUSTUM_HEADER_FILE \ No newline at end of file -- cgit v0.12