@@ -54,179 +54,176 @@ int color = 1; // Use the RGB texture or just draw it as color
54
54
// nicolas burrus' posts.
55
55
void LoadVertexMatrix ()
56
56
{
57
- float f = 590.0f ;
58
- float a = -0.0030711f ;
59
- float b = 3.3309495f ;
60
- float cx = 340.0f ;
61
- float cy = 240.0f ;
62
- GLfloat mat [16 ] = {
63
- 1 /f , 0 , 0 , 0 ,
64
- 0 , -1 /f , 0 , 0 ,
65
- 0 , 0 , 0 , a ,
66
- - cx /f ,cy /f , -1 , b
67
- };
68
- glMultMatrixf (mat );
57
+ float fx = 594.21f ;
58
+ float fy = 591.04f ;
59
+ float a = -0.0030711f ;
60
+ float b = 3.3309495f ;
61
+ float cx = 339.5f ;
62
+ float cy = 242.7f ;
63
+ GLfloat mat [16 ] = {
64
+ 1 /fx , 0 , 0 , 0 ,
65
+ 0 , -1 /fy , 0 , 0 ,
66
+ 0 , 0 , 0 , a ,
67
+ - cx /fx , cy /fy , -1 , b
68
+ };
69
+ glMultMatrixf (mat );
69
70
}
70
71
71
72
72
73
// This matrix comes from a combination of nicolas burrus's calibration post
73
74
// and some python code I haven't documented yet.
74
75
void LoadRGBMatrix ()
75
76
{
76
- float mat [16 ] = {
77
- 5.34866271e+02 , 3.89654806e+00 , 0.00000000e+00 , 1.74704200e-02 ,
78
- -4.70724694e+00 , -5.28843603e+02 , 0.00000000e+00 , -1.22753400e-02 ,
79
- -3.19670762e+02 , -2.60999685e+02 , 0.00000000e+00 , -9.99772000e-01 ,
80
- -6.98445586e+00 , 3.31139785e+00 , 0.00000000e+00 , 1.09167360e-02
81
- };
82
- glMultMatrixf (mat );
77
+ float mat [16 ] = {
78
+ 5.34866271e+02 , 3.89654806e+00 , 0.00000000e+00 , 1.74704200e-02 ,
79
+ -4.70724694e+00 , -5.28843603e+02 , 0.00000000e+00 , -1.22753400e-02 ,
80
+ -3.19670762e+02 , -2.60999685e+02 , 0.00000000e+00 , -9.99772000e-01 ,
81
+ -6.98445586e+00 , 3.31139785e+00 , 0.00000000e+00 , 1.09167360e-02
82
+ };
83
+ glMultMatrixf (mat );
83
84
}
84
85
85
86
void mouseMoved (int x , int y )
86
87
{
87
- if (mx >=0 && my >=0 ) {
88
- rotangles [0 ] += y - my ;
89
- rotangles [1 ] += x - mx ;
90
- }
91
- mx = x ;
92
- my = y ;
88
+ if (mx >=0 && my >=0 ) {
89
+ rotangles [0 ] += y - my ;
90
+ rotangles [1 ] += x - mx ;
91
+ }
92
+ mx = x ;
93
+ my = y ;
93
94
}
94
95
95
96
void mousePress (int button , int state , int x , int y )
96
97
{
97
- if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN ) {
98
- mx = x ;
99
- my = y ;
100
- }
101
- if (button == GLUT_LEFT_BUTTON && state == GLUT_UP ) {
102
- mx = -1 ;
103
- my = -1 ;
104
- }
98
+ if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN ) {
99
+ mx = x ;
100
+ my = y ;
101
+ }
102
+ if (button == GLUT_LEFT_BUTTON && state == GLUT_UP ) {
103
+ mx = -1 ;
104
+ my = -1 ;
105
+ }
105
106
}
106
107
107
108
void DrawGLScene ()
108
109
{
109
- short * depth = 0 ;
110
- char * rgb = 0 ;
111
- uint32_t ts ;
112
- if (freenect_sync_get_depth ((void * * )& depth , & ts ) < 0 )
113
- return ;
114
- if (freenect_sync_get_rgb ((void * * )& rgb , & ts ) < 0 ) {
115
- free (depth );
116
- return ;
117
- }
118
-
119
- static unsigned int indices [480 ][640 ];
120
- static short xyz [480 ][640 ][3 ];
121
- int i ,j ;
122
- for (i = 0 ; i < 480 ; i ++ ) {
123
- for (j = 0 ; j < 640 ; j ++ ) {
124
- xyz [i ][j ][0 ] = j ;
125
- xyz [i ][j ][1 ] = i ;
126
- xyz [i ][j ][2 ] = depth [i * 640 + j ];
127
- indices [i ][j ] = i * 640 + j ;
128
- }
129
- }
130
-
131
- glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
132
- glLoadIdentity ();
133
-
134
- glPushMatrix ();
135
- glScalef (zoom ,zoom ,1 );
136
- glTranslatef (0 ,0 ,-3.5 );
137
- glRotatef (rotangles [0 ], 1 ,0 ,0 );
138
- glRotatef (rotangles [1 ], 0 ,1 ,0 );
139
- glTranslatef (0 ,0 ,1.5 );
140
-
141
- LoadVertexMatrix ();
142
-
143
- // Set the projection from the XYZ to the texture image
144
- glMatrixMode (GL_TEXTURE );
145
- glLoadIdentity ();
146
- glScalef (1 /640.0f ,1 /480.0f ,1 );
147
- LoadRGBMatrix ();
148
- LoadVertexMatrix ();
149
- glMatrixMode (GL_MODELVIEW );
150
-
151
- glPointSize (1 );
152
-
153
- glEnableClientState (GL_VERTEX_ARRAY );
154
- glVertexPointer (3 , GL_SHORT , 0 , xyz );
155
- glEnableClientState (GL_TEXTURE_COORD_ARRAY );
156
- glTexCoordPointer (3 , GL_SHORT , 0 , xyz );
157
-
158
- if (color )
159
- glEnable (GL_TEXTURE_2D );
160
- glBindTexture (GL_TEXTURE_2D , gl_rgb_tex );
161
- glTexImage2D (GL_TEXTURE_2D , 0 , 3 , 640 , 480 , 0 , GL_RGB , GL_UNSIGNED_BYTE , rgb );
162
-
163
- glPointSize (2.0f );
164
- glDrawElements (GL_POINTS , 640 * 480 , GL_UNSIGNED_INT , indices );
165
- glPopMatrix ();
166
- glDisable (GL_TEXTURE_2D );
167
-
168
- free (rgb );
169
- free (depth );
170
-
171
- glutSwapBuffers ();
110
+ short * depth = 0 ;
111
+ char * rgb = 0 ;
112
+ uint32_t ts ;
113
+ if (freenect_sync_get_depth ((void * * )& depth , & ts , 0 , FREENECT_DEPTH_11BIT ) < 0 )
114
+ return ;
115
+ if (freenect_sync_get_video ((void * * )& rgb , & ts , 0 , FREENECT_VIDEO_RGB ) < 0 ) {
116
+ free (depth );
117
+ return ;
118
+ }
119
+
120
+ static unsigned int indices [480 ][640 ];
121
+ static short xyz [480 ][640 ][3 ];
122
+ int i ,j ;
123
+ for (i = 0 ; i < 480 ; i ++ ) {
124
+ for (j = 0 ; j < 640 ; j ++ ) {
125
+ xyz [i ][j ][0 ] = j ;
126
+ xyz [i ][j ][1 ] = i ;
127
+ xyz [i ][j ][2 ] = depth [i * 640 + j ];
128
+ indices [i ][j ] = i * 640 + j ;
129
+ }
130
+ }
131
+
132
+ glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
133
+ glLoadIdentity ();
134
+
135
+ glPushMatrix ();
136
+ glScalef (zoom ,zoom ,1 );
137
+ glTranslatef (0 ,0 ,-3.5 );
138
+ glRotatef (rotangles [0 ], 1 ,0 ,0 );
139
+ glRotatef (rotangles [1 ], 0 ,1 ,0 );
140
+ glTranslatef (0 ,0 ,1.5 );
141
+
142
+ LoadVertexMatrix ();
143
+
144
+ // Set the projection from the XYZ to the texture image
145
+ glMatrixMode (GL_TEXTURE );
146
+ glLoadIdentity ();
147
+ glScalef (1 /640.0f ,1 /480.0f ,1 );
148
+ LoadRGBMatrix ();
149
+ LoadVertexMatrix ();
150
+ glMatrixMode (GL_MODELVIEW );
151
+
152
+ glPointSize (1 );
153
+
154
+ glEnableClientState (GL_VERTEX_ARRAY );
155
+ glVertexPointer (3 , GL_SHORT , 0 , xyz );
156
+ glEnableClientState (GL_TEXTURE_COORD_ARRAY );
157
+ glTexCoordPointer (3 , GL_SHORT , 0 , xyz );
158
+
159
+ if (color )
160
+ glEnable (GL_TEXTURE_2D );
161
+ glBindTexture (GL_TEXTURE_2D , gl_rgb_tex );
162
+ glTexImage2D (GL_TEXTURE_2D , 0 , 3 , 640 , 480 , 0 , GL_RGB , GL_UNSIGNED_BYTE , rgb );
163
+
164
+ glPointSize (2.0f );
165
+ glDrawElements (GL_POINTS , 640 * 480 , GL_UNSIGNED_INT , indices );
166
+ glPopMatrix ();
167
+ glDisable (GL_TEXTURE_2D );
168
+ glutSwapBuffers ();
172
169
}
173
170
174
171
void keyPressed (unsigned char key , int x , int y )
175
172
{
176
- if (key == 27 ) {
177
- freenect_sync_stop ();
178
- glutDestroyWindow (window );
179
- exit (0 );
180
- }
181
- if (key == 'w' )
182
- zoom *= 1.1f ;
183
- if (key == 's' )
184
- zoom /= 1.1f ;
185
- if (key == 'c' )
186
- color = !color ;
173
+ if (key == 27 ) {
174
+ freenect_sync_stop ();
175
+ glutDestroyWindow (window );
176
+ exit (0 );
177
+ }
178
+ if (key == 'w' )
179
+ zoom *= 1.1f ;
180
+ if (key == 's' )
181
+ zoom /= 1.1f ;
182
+ if (key == 'c' )
183
+ color = !color ;
187
184
}
188
185
189
186
void ReSizeGLScene (int Width , int Height )
190
187
{
191
- glViewport (0 ,0 ,Width ,Height );
192
- glMatrixMode (GL_PROJECTION );
193
- glLoadIdentity ();
194
- gluPerspective (60 , 4 /3. , 0.3 , 200 );
195
- glMatrixMode (GL_MODELVIEW );
188
+ glViewport (0 ,0 ,Width ,Height );
189
+ glMatrixMode (GL_PROJECTION );
190
+ glLoadIdentity ();
191
+ gluPerspective (60 , 4 /3. , 0.3 , 200 );
192
+ glMatrixMode (GL_MODELVIEW );
196
193
}
197
194
198
195
void InitGL (int Width , int Height )
199
196
{
200
- glClearColor (0.0f , 0.0f , 0.0f , 0.0f );
201
- glEnable (GL_DEPTH_TEST );
202
- glGenTextures (1 , & gl_rgb_tex );
203
- glBindTexture (GL_TEXTURE_2D , gl_rgb_tex );
204
- glTexParameteri (GL_TEXTURE_2D , GL_TEXTURE_MIN_FILTER , GL_LINEAR );
205
- glTexParameteri (GL_TEXTURE_2D , GL_TEXTURE_MAG_FILTER , GL_LINEAR );
206
- ReSizeGLScene (Width , Height );
197
+ glClearColor (0.0f , 0.0f , 0.0f , 0.0f );
198
+ glEnable (GL_DEPTH_TEST );
199
+ glGenTextures (1 , & gl_rgb_tex );
200
+ glBindTexture (GL_TEXTURE_2D , gl_rgb_tex );
201
+ glTexParameteri (GL_TEXTURE_2D , GL_TEXTURE_MIN_FILTER , GL_LINEAR );
202
+ glTexParameteri (GL_TEXTURE_2D , GL_TEXTURE_MAG_FILTER , GL_LINEAR );
203
+ ReSizeGLScene (Width , Height );
207
204
}
208
205
209
206
int main (int argc , char * * argv )
210
207
{
211
- glutInit (& argc , argv );
208
+ glutInit (& argc , argv );
212
209
213
- glutInitDisplayMode (GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH );
214
- glutInitWindowSize (640 , 480 );
215
- glutInitWindowPosition (0 , 0 );
210
+ glutInitDisplayMode (GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH );
211
+ glutInitWindowSize (640 , 480 );
212
+ glutInitWindowPosition (0 , 0 );
216
213
217
- window = glutCreateWindow ("LibFreenect" );
214
+ window = glutCreateWindow ("LibFreenect" );
218
215
219
- glutDisplayFunc (& DrawGLScene );
220
- glutIdleFunc (& DrawGLScene );
221
- glutReshapeFunc (& ReSizeGLScene );
222
- glutKeyboardFunc (& keyPressed );
223
- glutMotionFunc (& mouseMoved );
224
- glutMouseFunc (& mousePress );
216
+ glutDisplayFunc (& DrawGLScene );
217
+ glutIdleFunc (& DrawGLScene );
218
+ glutReshapeFunc (& ReSizeGLScene );
219
+ glutKeyboardFunc (& keyPressed );
220
+ glutMotionFunc (& mouseMoved );
221
+ glutMouseFunc (& mousePress );
225
222
226
- InitGL (640 , 480 );
223
+ InitGL (640 , 480 );
227
224
228
- glutMainLoop ();
225
+ glutMainLoop ();
229
226
230
- return 0 ;
227
+ return 0 ;
231
228
}
232
229
0 commit comments