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
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
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();
}
|
