GLscene.cpp
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <iostream>
00003 #include <fstream>
00004 #ifdef __APPLE__
00005 #include <GLUT/glut.h>
00006 #else
00007 #include <GL/glut.h>
00008 #endif
00009 #include <sys/time.h>
00010 #include "hrpsys/util/GLcamera.h"
00011 #include "hrpsys/util/GLlink.h"
00012 #include "hrpsys/util/GLbody.h"
00013 #include "hrpsys/util/LogManager.h"
00014 #include "TimedPosture.h"
00015 #include "GLscene.h"
00016 #include "hrpsys/idl/CollisionDetectorService.hh"
00017 
00018 
00019 using namespace OpenHRP;
00020 using namespace hrp;
00021 using namespace CollisionDetectorComponent;
00022 
00023 static void drawString(const char *str)
00024 {
00025     for (unsigned int i=0; i<strlen(str); i++){
00026         glutBitmapCharacter(GLUT_BITMAP_9_BY_15, str[i]);
00027     }
00028 }
00029 
00030 void GLscene::updateScene()
00031 {
00032     if (m_log->index()<0) return;
00033 
00034 #ifdef USE_COLLISION_STATE
00035     LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
00036         = (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
00037     GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
00038     OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
00039     if (co.angle.length() == glbody->numJoints()){
00040         for (unsigned int i=0; i<glbody->numJoints(); i++){
00041             GLlink *j = (GLlink *)glbody->joint(i);
00042             if (j){
00043                 j->setQ(co.angle[i]);
00044             }
00045         }
00046     }
00047 #else
00048     LogManager<TimedPosture> *lm 
00049         = (LogManager<TimedPosture> *)m_log;
00050     GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
00051     TimedPosture &ts = lm->state();
00052     if (ts.posture.size() == glbody->numJoints()){
00053         for (unsigned int i=0; i<glbody->numJoints(); i++){
00054             GLlink *j = (GLlink *)glbody->joint(i);
00055             if (j){
00056                 j->setQ(ts.posture[i]);
00057             }
00058         }
00059     }
00060 #endif
00061 }
00062 
00063 void GLscene::drawAdditionalLines()
00064 {
00065     if (m_log->index()<0) return;
00066 
00067 #ifdef USE_COLLISION_STATE
00068     LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
00069         = (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
00070     OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
00071 
00072     glBegin(GL_LINES);
00073     glColor3f(1,0,0);
00074     for (unsigned int i=0; i<co.lines.length(); i++){
00075         GLdouble p0[3], p1[3];
00076         p0[0] = co.lines[i][0][0];
00077         p0[1] = co.lines[i][0][1];
00078         p0[2] = co.lines[i][0][2];
00079         p1[0] = co.lines[i][1][0];
00080         p1[1] = co.lines[i][1][1];
00081         p1[2] = co.lines[i][1][2];
00082         glVertex3dv(p0);
00083         glVertex3dv(p1);
00084     }
00085     glEnd();
00086 
00087     glPointSize(4.0);
00088     glBegin(GL_POINTS);
00089     glColor3f(1,0,0);
00090     for (unsigned int i=0; i<co.lines.length(); i++){
00091         GLdouble p0[3], p1[3];
00092         p0[0] = co.lines[i][0][0];
00093         p0[1] = co.lines[i][0][1];
00094         p0[2] = co.lines[i][0][2];
00095         p1[0] = co.lines[i][1][0];
00096         p1[1] = co.lines[i][1][1];
00097         p1[2] = co.lines[i][1][2];
00098         glVertex3dv(p0);
00099         glVertex3dv(p1);
00100     }
00101     glEnd();
00102 #else
00103     LogManager<TimedPosture> *lm 
00104         = (LogManager<TimedPosture> *)m_log;
00105     TimedPosture &tp = lm->state();
00106 
00107     glBegin(GL_LINES);
00108     glColor3f(1,0,0);
00109     for (unsigned int i=0; i<tp.lines.size(); i++){
00110         const std::pair<hrp::Vector3, hrp::Vector3>& line = tp.lines[i];
00111         glVertex3dv(line.first.data());
00112         glVertex3dv(line.second.data());
00113     }
00114     glEnd();
00115 
00116     glPointSize(4.0);
00117     glBegin(GL_POINTS);
00118     glColor3f(1,0,0);
00119     for (unsigned int i=0; i<tp.lines.size(); i++){
00120         const std::pair<hrp::Vector3, hrp::Vector3>& line = tp.lines[i];
00121         glVertex3dv(line.first.data());
00122         glVertex3dv(line.second.data());
00123     }
00124     glEnd();
00125 #endif
00126 }
00127 
00128 void GLscene::showStatus()
00129 {
00130     char buf[256];
00131 
00132     GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
00133     int width = m_width - 220;
00134 #define HEIGHT_STEP 12
00135     int height = m_height-HEIGHT_STEP;
00136     int x = width;
00137 
00138     for (unsigned int i=0; i<glbody->numLinks(); i++){
00139         hrp::Link *l = glbody->link(i);
00140         if (l){
00141             sprintf(buf, "%13s %4d tris",
00142                     l->name.c_str(),
00143                     l->coldetModel->getNumTriangles());
00144             glRasterPos2f(x, height);
00145             drawString(buf);
00146             height -= HEIGHT_STEP;
00147         }
00148     }
00149 
00150     if (m_log->index()<0) return;
00151 
00152     LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
00153         = (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
00154     OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
00155 
00156     height -= HEIGHT_STEP;
00157 
00158     x = width - 34;
00159     sprintf(buf, "Number of pair     %8d",  co.lines.length());
00160     glRasterPos2f(x, height);
00161     drawString(buf);
00162     height -= HEIGHT_STEP;
00163 
00164     sprintf(buf, "Calc Time [msec]   %8.3f",  co.computation_time);
00165     glRasterPos2f(x, height);
00166     drawString(buf);
00167     height -= HEIGHT_STEP;
00168 
00169     sprintf(buf, "Recover Time[msec] %8.3f",  co.recover_time);
00170     glRasterPos2f(x, height);
00171     drawString(buf);
00172     height -= HEIGHT_STEP;
00173 
00174     sprintf(buf, "Safe Posture       %8s",  co.safe_posture?"true":"false");
00175     glRasterPos2f(x, height);
00176     drawString(buf);
00177     height -= HEIGHT_STEP;
00178 
00179     sprintf(buf, "Loop for check     %8d",  co.loop_for_check);
00180     glRasterPos2f(x, height);
00181     drawString(buf);
00182     height -= HEIGHT_STEP;
00183 
00184 }
00185 
00186 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55