1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
|
#include <GL\glut.h>
#define _USE_MATH_DEFINES
#include <math.h>
#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();
}
|
