GlutViewer.cpp
Go to the documentation of this file.
00001 #include "GlutViewer.h"
00002 
00003 #include "Rotation.h"
00004 #include "Platform.h"
00005 
00006 using namespace std;
00007 using namespace alvar;
00008 using namespace GlutViewer;
00009 
00010 Drawable::Drawable(double _scale, double _r, double _g, double _b) {
00011         SetScale(_scale);
00012         SetColor(_r, _g, _b);
00013 }
00014 
00015 void Drawable::SetScale(double _scale) {
00016         scale=_scale;
00017 }
00018 
00019 void Drawable::SetColor(double _r, double _g, double _b) {
00020         color[0] = _r;
00021         color[1] = _g;
00022         color[2] = _b;
00023 }
00024 
00025 void Drawable::Draw() {
00026         //double color[3] = {1, 1, 1};
00027         glPushMatrix();
00028         glMultMatrixd(gl_mat);
00029         DrawAxis(scale, color);
00030         glPopMatrix();
00031 }
00032 
00033 void Drawable::DrawAxis(double scale, double color[3])
00034 {
00035         glEnable(GL_DEPTH_TEST);
00036         glDisable(GL_CULL_FACE);
00037 
00038         glColor3d(color[0], color[1], color[2]);
00039         glBegin(GL_QUADS);
00040                 glVertex3f(-scale/2, -scale/2, 0.0);
00041                 glVertex3f(-scale/2,  scale/2, 0.0);
00042 
00043                 glVertex3f(-scale/2,  scale/2, 0.0);
00044                 glVertex3f( scale/2,  scale/2, 0.0);
00045 
00046                 glVertex3f( scale/2,  scale/2, 0.0);
00047                 glVertex3f( scale/2, -scale/2, 0.0);
00048                 
00049                 glVertex3f( scale/2, -scale/2, 0.0);
00050                 glVertex3f(-scale/2, -scale/2, 0.0);
00051         glEnd();
00052 
00053         // Z
00054         glColor3d(0.0, 0.0, 1.0);
00055         glBegin(GL_LINES);
00056                 glVertex3f(0.0, 0.0, 0.0);
00057                 glVertex3f(0.0, 0.0, scale);
00058         glEnd();
00059 
00060         glDisable(GL_DEPTH_TEST);
00061 
00062         // X
00063         glColor3d(1.0, 0.0, 0.0);
00064         glBegin(GL_LINES);
00065                 glVertex3f(0.0,   0.0, 0.0);
00066                 glVertex3f(scale, 0.0, 0.0);
00067         glEnd();
00068 
00069         // Y
00070         glColor3d(0.0, 1.0, 0.0);
00071         glBegin(GL_LINES);
00072                 glVertex3f(0.0, 0.0,   0.0);
00073                 glVertex3f(0.0, scale, 0.0);
00074         glEnd();
00075 }
00076 
00077 void Drawable::SetGLMatTraQuat(double *tra, double *quat, bool flip)
00078 {
00079         Rotation r;
00080         if (quat != 0)
00081         {
00082                 CvMat cv_mat;
00083                 cvInitMatHeader(&cv_mat, 4, 1, CV_64F, quat);
00084                 r.SetQuaternion(&cv_mat);
00085         }
00086 
00087         int flp = 1;
00088         if (flip)
00089         {
00090                 r.Transpose();
00091                 //flp=-1;
00092         }
00093 
00094         CvMat cv_gl_mat;
00095         cvInitMatHeader(&cv_gl_mat, 4, 4, CV_64F, gl_mat); cvZero(&cv_gl_mat);
00096         r.GetMatrix(&cv_gl_mat);
00097         cvSet2D(&cv_gl_mat, 0, 3, cvScalar(flp*tra[0]));
00098         cvSet2D(&cv_gl_mat, 1, 3, cvScalar(flp*tra[1]));
00099         cvSet2D(&cv_gl_mat, 2, 3, cvScalar(flp*tra[2]));
00100         cvSet2D(&cv_gl_mat, 3, 3, cvScalar(1));
00101 
00102         cvTranspose(&cv_gl_mat, &cv_gl_mat);
00103 }
00104 
00105 void Drawable::SetGLMatTraRod(double *tra, double *rod)
00106 {
00107         // This is the OpenGL augmentation matrix
00108         CvMat cv_gl_mat;
00109         cvInitMatHeader(&cv_gl_mat, 4, 4, CV_64F, gl_mat); cvSetIdentity(&cv_gl_mat);
00110 
00111         // Figure out the rotation part 
00112         double rot_mat_data[3][3];
00113         CvMat rot_mat = cvMat(3, 3, CV_64F, rot_mat_data);
00114         cvSetIdentity(&rot_mat);
00115         if (rod != 0)
00116         {
00117                 CvMat rod_mat;
00118                 cvInitMatHeader(&rod_mat, 3, 1, CV_64F, rod);
00119                 cvRodrigues2(&rod_mat, &rot_mat, 0);
00120         }
00121 
00122         // Fill in the rotation part
00123         cvmSet(&cv_gl_mat, 0, 0, cvmGet(&rot_mat, 0, 0));
00124         cvmSet(&cv_gl_mat, 0, 1, cvmGet(&rot_mat, 0, 1));
00125         cvmSet(&cv_gl_mat, 0, 2, cvmGet(&rot_mat, 0, 2));
00126         cvmSet(&cv_gl_mat, 1, 0, cvmGet(&rot_mat, 1, 0));
00127         cvmSet(&cv_gl_mat, 1, 1, cvmGet(&rot_mat, 1, 1));
00128         cvmSet(&cv_gl_mat, 1, 2, cvmGet(&rot_mat, 1, 2));
00129         cvmSet(&cv_gl_mat, 2, 0, cvmGet(&rot_mat, 2, 0));
00130         cvmSet(&cv_gl_mat, 2, 1, cvmGet(&rot_mat, 2, 1));
00131         cvmSet(&cv_gl_mat, 2, 2, cvmGet(&rot_mat, 2, 2));
00132 
00133         // Fill in the translation part
00134         cvSet2D(&cv_gl_mat, 0, 3, cvScalar(tra[0]));
00135         cvSet2D(&cv_gl_mat, 1, 3, cvScalar(tra[1]));
00136         cvSet2D(&cv_gl_mat, 2, 3, cvScalar(tra[2]));
00137 
00138         // Transpose into OpenGL presentation order
00139         cvTranspose(&cv_gl_mat, &cv_gl_mat);
00140 }
00141 
00142 Mutex mutex_items;
00143 vector<Drawable*> items;
00144 
00145 int cur_button;
00146 float elev = 0.0, azim = 0.0, rad = 0.0;
00147 float panx = 0.0, pany = 0.0;
00148 float jaw  = 0.0, jawx, jawy, jawz;
00149 
00150 int ar_window;
00151 int vr_window;
00152 
00153 float off_x=0, off_y=0;
00154 
00155 unsigned char* image=0;
00156 
00157 int a_argc;
00158 char **a_argv;
00159 int width;
00160 int height;
00161 
00162 Threads threads;
00163 
00164 double proj_mat[16];
00165 double modelview_mat[16] = { 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1 };
00166 
00167 static void *glut_thread(void *lpThreadParameter)
00168 {
00169         //InitializeCriticalSection(&critical_section_items);
00170 
00171         glutInit(&a_argc, a_argv);
00172         glutInitDisplayMode(GLUT_DEPTH | GLUT_RGB | GLUT_DOUBLE);
00173         glutInitWindowSize(width, height);
00174 
00175         ar_window = glutCreateWindow("AR");
00176         glutDisplayFunc(DrawAr);
00177         glutSpecialFunc(KeyCallback);
00178         glutPositionWindow(0, 0);
00179 
00180         vr_window = glutCreateWindow("VR");
00181         glutDisplayFunc(DrawVr);
00182         glutPositionWindow(0, height);
00183 
00184         glEnable(GL_CULL_FACE);
00185         glEnable(GL_DEPTH_TEST);
00186         glEnable(GL_COLOR_MATERIAL);
00187 
00188         glutMouseFunc(Mouse);
00189         glutMotionFunc(Motion);
00190 
00191         atexit(Exit);
00192 
00193         glutMainLoop();
00194         return 0;
00195 }
00196 
00197 void GlutViewer::KeyCallback(int key, int x, int y)
00198 {
00199         switch(key)
00200         {
00201         case GLUT_KEY_LEFT:
00202                 off_x-=1;
00203                 break;
00204         case GLUT_KEY_RIGHT:
00205                 off_x+=1;               
00206                 break;
00207         case GLUT_KEY_UP:
00208                 off_y-=1;
00209                 break;
00210         case GLUT_KEY_DOWN:
00211                 off_y+=1;       
00212                 break;
00213                 
00214         }
00215 }
00216 
00217 double GlutViewer::GetXOffset()
00218 {
00219         return off_x;
00220 }
00221 
00222 double GlutViewer::GetYOffset()
00223 {
00224         return off_y;
00225 }
00226 
00227 void GlutViewer::Start(int argc, char** argv, int w, int h, float r)
00228 {
00229         a_argc = argc;
00230         a_argv = argv;
00231         width = w;
00232         height = h;
00233     rad = r;
00234 
00235     threads.create(glut_thread, 0);
00236 }
00237 
00238 void GlutViewer::DrawFloor()
00239 {
00240         glColor3f(0.5, 0.5, 1.0);
00241 
00242         glBegin(GL_LINES);
00243         for(int i = -20; i <= 20; i+=1)
00244         {
00245                 glVertex3f((float)i, 0.0f,  -20);
00246                 glVertex3f((float)i, 0.0f,   20);
00247                 glVertex3f(-20, 0.0f, (float)i);
00248                 glVertex3f( 20, 0.0f, (float)i);
00249         }
00250         glEnd();
00251 }
00252 
00253 void GlutViewer::Mouse(int button, int state, int x, int y)
00254 {
00255         cur_button = button;
00256 }
00257 
00258 void GlutViewer::DrawAxis(float scale)
00259 {
00260         glColor3f(1.0, 0.0, 0.0);
00261         glBegin(GL_LINES);
00262                 glVertex3f(0.0, 0.0, 0.0);
00263                 glVertex3f(scale, 0.0, 0.0);
00264         glEnd();
00265         glColor3f(0.0, 1.0, 0.0);
00266         glBegin(GL_LINES);
00267                 glVertex3f(0.0, 0.0, 0.0);
00268                 glVertex3f(0.0, scale, 0.0);
00269         glEnd();
00270         glColor3f(0.0, 0.0, 1.0);
00271         glBegin(GL_LINES);
00272                 glVertex3f(0.0, 0.0, 0.0);
00273                 glVertex3f(0.0, 0.0, scale);
00274         glEnd();
00275 }
00276 
00277 void GlutViewer::Motion(int x, int y)
00278 {
00279         static int oldx, oldy;
00280 
00281         int dx = oldx-x;
00282         int dy = oldy-y;
00283 
00284         switch(cur_button)
00285         {
00286         case GLUT_LEFT_BUTTON :
00287                 if (abs(dx)>abs(dy))
00288                 {
00289                         if (dx>0)
00290                                 azim += 3.0;
00291                         else if (dx<0)
00292                                 azim -= 3.0;
00293                 }
00294                 else if (dy>0)
00295                         elev -= 3.0;
00296                 else if (dy<0)
00297                         elev += 3.0;
00298                 break;
00299 
00300         case GLUT_MIDDLE_BUTTON :
00301                 if (abs(dx)>abs(dy))
00302                 {
00303                         if (dx>0)
00304                                 panx += 10.5;
00305                         else if (dx<0)
00306                                 panx -= 10.5;
00307                         }
00308                         else if (dy>0)
00309                                 pany -= 10.5;
00310                         else if (dy<0)
00311                                 pany += 10.5;
00312                         break;//??
00313 
00314         case GLUT_RIGHT_BUTTON :
00315                 if (dy > 0)
00316                         rad += (10.2);
00317                 else if (dy < 0)
00318                         rad -= (10.2);
00319                 break;
00320 
00321         default:
00322                 break;
00323         }
00324 
00325         oldx = x;
00326         oldy = y;
00327 }
00328 
00329 
00330 void GlutViewer::DrawContent()
00331 {
00332         DrawAxis(100.f);
00333         Lock lock(&mutex_items);
00334         for (unsigned i = 0; i < items.size(); ++i) {
00335                 items[i]->Draw();
00336         }
00337 }
00338 
00339 
00340 void GlutViewer::DrawVr()
00341 {
00342         glutSetWindow(vr_window);
00343         glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
00344         glClearColor(0.5, 0.2, 0.2, 1.0);
00345 
00346         //glViewport(0, 0, w, h);
00347         glMatrixMode(GL_PROJECTION);
00348         glLoadIdentity();
00349         //gluPerspective(70, 1, 0.001, 5000);
00350 
00351         glMatrixMode(GL_MODELVIEW);
00352         glLoadIdentity();
00353 
00354         glTranslatef(panx, pany, -rad);
00355         glRotatef(-elev, 1.0, 0.0, 0.0);
00356         glRotatef( azim, 0.0, 1.0, 0.0);
00357         
00358         float pos[4] = {50, 0, 50};
00359         glLightfv(GL_LIGHT0, GL_POSITION, pos);
00360 
00361         DrawContent();
00362 
00363         //glFlush();
00364         glutSwapBuffers();
00365         glutPostRedisplay();
00366 }
00367 
00368 void GlutViewer::DrawAr()
00369 {
00370         glutSetWindow(ar_window);
00371         glClearColor(0.2, 0.5, 0.2, 1.0);
00372         glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
00373 
00374         DrawVideo();
00375 
00376         glMatrixMode(GL_PROJECTION);
00377         glLoadMatrixd(proj_mat);
00378 
00379         glMatrixMode(GL_MODELVIEW);
00380         glLoadMatrixd(modelview_mat);
00381 
00382         DrawContent();
00383 
00384         glutSwapBuffers();
00385         glutPostRedisplay();
00386 }
00387 
00388 void GlutViewer::SetGlProjectionMatrix(double p[16]) {
00389         memcpy(proj_mat, p, sizeof(double)*16);
00390 }
00391 
00392 void GlutViewer::SetGlModelviewMatrix(double p[16]) {
00393         memcpy(modelview_mat, p, sizeof(double)*16);
00394 }
00395 
00396 void GlutViewer::Reshape(int w, int h)
00397 {
00398         h = (h == 0 ?  1 : h);
00399         //glViewport(0, 0, w, h);
00400 
00401         //glMatrixMode(GL_PROJECTION);
00402 
00403         //glLoadIdentity();
00404         //glMultMatrixd(projmat);
00405 
00406         //glMatrixMode(GL_MODELVIEW);
00407 
00408         //glutPostRedisplay();
00409 }
00410 
00411 void GlutViewer::Exit()
00412 {
00413         //DeleteCriticalSection(&critical_section_items);
00414 }
00415 
00416 void GlutViewer::DrawableClear() {
00417         Lock lock(&mutex_items);
00418         items.clear();
00419 }
00420 
00421 void GlutViewer::DrawableAdd(Drawable* item)
00422 {
00423         Lock lock(&mutex_items);
00424         items.push_back(item);
00425 }
00426 
00427 // TODO ensure that threading doesn't cause any problems...
00428 void GlutViewer::SetVideo(const IplImage* _image)
00429 {
00430         image = (unsigned char*)_image->imageData;
00431 }
00432 
00433 void GlutViewer::DrawVideo()
00434 {
00435         if(!image) return;
00436 
00437         glDepthMask(GL_FALSE);
00438         glDisable(GL_DEPTH_TEST);
00439 
00440         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
00441         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
00442         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
00443         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
00444 
00445         glColor3f(1, 1, 1);
00446         glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 512, 512, 0, GL_BGR_EXT, GL_UNSIGNED_BYTE, image);
00447 
00448         glEnable(GL_TEXTURE_2D);
00449         
00450         glMatrixMode(GL_PROJECTION);
00451         glPushMatrix();
00452         glLoadIdentity();
00453         glOrtho(0, 1, 0, 1, 0, 1);
00454 
00455         glMatrixMode(GL_MODELVIEW);
00456         glLoadIdentity();
00457 
00458         glBegin( GL_QUADS );
00459                 glTexCoord2d(0.0,1.0); glVertex2d(0.0,0.0);
00460                 glTexCoord2d(1.0,1.0); glVertex2d(1.0,0.0);
00461                 glTexCoord2d(1.0,0.0); glVertex2d(1.0,1.0);
00462                 glTexCoord2d(0.0,0.0); glVertex2d(0.0,1.0);
00463         glEnd();
00464 
00465         glDisable(GL_TEXTURE_2D);
00466         glDepthMask(GL_TRUE);
00467         glEnable(GL_DEPTH_TEST);
00468 
00469         glMatrixMode(GL_PROJECTION);
00470         glPopMatrix();
00471 }
00472 
00473 /*
00474 void GlutViewer::Init(int argc, char** argv, int w, int h)
00475 {
00476         glutInit(&argc, argv);
00477         glutInitDisplayMode(GLUT_DEPTH | GLUT_RGB | GLUT_DOUBLE);
00478         glutInitWindowSize(w, h);
00479         
00480         ar_window = glutCreateWindow("AR");
00481         glutDisplayFunc(DrawAr);
00482         //glutReshapeFunc(Reshape);
00483 
00484         vr_window = glutCreateWindow("VR");
00485         glutDisplayFunc(DrawVr);
00486         //glutReshapeFunc(Reshape);
00487 
00488         glEnable(GL_CULL_FACE);
00489         glEnable(GL_DEPTH_TEST);
00490         glEnable(GL_COLOR_MATERIAL);
00491 
00492         glutMouseFunc(Mouse);
00493         glutMotionFunc(Motion);
00494 
00495         atexit(Exit);
00496 }
00497 */


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02