GLscene.cpp
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <iostream>
00003 #include <fstream>
00004 #include <sys/time.h>
00005 #ifdef __APPLE__
00006 #include <GLUT/glut.h>
00007 #else
00008 #include <GL/glut.h>
00009 #endif
00010 #include <boost/bind.hpp>
00011 #include <hrpModel/Sensor.h>
00012 #include "hrpsys/util/GLcamera.h"
00013 #include "hrpsys/util/GLlink.h"
00014 #include "hrpsys/util/GLbody.h"
00015 #include "hrpsys/util/LogManager.h"
00016 #include "SceneState.h"
00017 #include "GLscene.h"
00018 
00019 using namespace OpenHRP;
00020 using namespace hrp;
00021 
00022 GLscene::GLscene(LogManagerBase *i_log) : 
00023     GLsceneBase(i_log), 
00024     m_showSensors(false),
00025     m_showCollision(true)
00026 {
00027 }
00028 
00029 void GLscene::updateScene()
00030 { 
00031     if (m_log->index()<0) return;
00032 
00033     LogManager<SceneState> *lm 
00034         = (LogManager<SceneState> *)m_log;
00035     SceneState &state = lm->state();
00036     
00037     for (unsigned int i=0; i<state.bodyStates.size(); i++){
00038         const BodyState& bstate = state.bodyStates[i];
00039         GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
00040         glbody->setPosture(bstate.q, bstate.p, bstate.R);
00041         if (m_showSensors){
00042             glbody->setSensorDrawCallback(
00043                 boost::bind(&GLscene::drawSensorOutput, this, _1, _2));
00044         }else{
00045             glbody->setSensorDrawCallback(NULL);
00046         }
00047     }
00048 }
00049 
00050 static void drawString2(const char *str)
00051 {
00052     for (unsigned int i=0; i<strlen(str); i++){
00053         glutBitmapCharacter(GLUT_BITMAP_8_BY_13, str[i]);
00054     }
00055 }
00056 
00057 void GLscene::drawAdditionalLines()
00058 {
00059     if (!m_showCollision || m_log->index()<0) return;
00060 
00061     LogManager<SceneState> *lm 
00062         = (LogManager<SceneState> *)m_log;
00063     SceneState &state = lm->state();
00064 
00065     glBegin(GL_LINES);
00066     glColor3f(1,0,0);
00067     double e[3];
00068     const std::vector<CollisionInfo> &cs = state.collisions;
00069     for (unsigned int i=0; i<cs.size(); i++){
00070         for (int k=0; k<3; k++){
00071             e[k] = cs[i].position[k] + cs[i].normal[k]*(cs[i].idepth*10+0.1);
00072         }
00073         glVertex3dv(cs[i].position);
00074         glVertex3dv(e);
00075     }
00076     glEnd();
00077 }
00078 
00079 void GLscene::showStatus()
00080 {
00081     if (m_log->index()<0) return;
00082 
00083     LogManager<SceneState> *lm 
00084         = (LogManager<SceneState> *)m_log;
00085     SceneState &state = lm->state();
00086 
00087     if (m_showingStatus){
00088         GLbody *glbody = NULL;
00089         BodyState *bstate = NULL;
00090         for (unsigned int i=0; i<numBodies(); i++){
00091             if (body(i)->numJoints()){
00092                 glbody = dynamic_cast<GLbody *>(body(i).get());
00093                 bstate = &state.bodyStates[i];
00094                 break;
00095             }
00096         }
00097         if (!glbody) return;
00098 #define HEIGHT_STEP 12
00099         int width = m_width - 350;
00100         int height = m_height-HEIGHT_STEP;
00101         char buf[256];
00102         double q[glbody->numJoints()];
00103         for (unsigned int i=0; i<glbody->numLinks(); i++){
00104             Link* l = glbody->link(i);
00105             if (l->jointId >= 0) q[l->jointId] = bstate->q[i];
00106         }
00107         for (unsigned int i=0; i<glbody->numJoints(); i++){
00108             GLlink *l = (GLlink *)glbody->joint(i);
00109             if (l){
00110                 sprintf(buf, "%2d %15s %8.3f", i, l->name.c_str(),
00111                         q[i]*180/M_PI);
00112                 glRasterPos2f(width, height);
00113                 height -= HEIGHT_STEP;
00114                 drawString2(buf);
00115             }
00116         }
00117         if (bstate->acc.size()){
00118             glRasterPos2f(width, height);
00119             height -= HEIGHT_STEP;
00120             drawString2("acc:");
00121             for (unsigned int i=0; i<bstate->acc.size(); i++){
00122                 sprintf(buf, "  %8.4f %8.4f %8.4f",
00123                         bstate->acc[i][0], bstate->acc[i][1], bstate->acc[i][2]);
00124                 glRasterPos2f(width, height);
00125                 height -= HEIGHT_STEP;
00126                 drawString2(buf);
00127             }
00128         }
00129         if (bstate->rate.size()){
00130             glRasterPos2f(width, height);
00131             height -= HEIGHT_STEP;
00132             drawString2("rate:");
00133             for (unsigned int i=0; i<bstate->rate.size(); i++){
00134                 sprintf(buf, "  %8.4f %8.4f %8.4f",
00135                         bstate->rate[i][0], bstate->rate[i][1], bstate->rate[i][2]);
00136                 glRasterPos2f(width, height);
00137                 height -= HEIGHT_STEP;
00138                 drawString2(buf);
00139             }
00140         }
00141         if (bstate->force.size()){
00142             glRasterPos2f(width, height);
00143             height -= HEIGHT_STEP;
00144             drawString2("force/torque:");
00145             for (unsigned int i=0; i<bstate->force.size(); i++){
00146                 sprintf(buf, "  %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
00147                         bstate->force[i][0], 
00148                         bstate->force[i][1], 
00149                         bstate->force[i][2],
00150                         bstate->force[i][3], 
00151                         bstate->force[i][4], 
00152                         bstate->force[i][5]);
00153                 glRasterPos2f(width, height);
00154                 height -= HEIGHT_STEP;
00155                 drawString2(buf);
00156             }
00157         }
00158         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00159         glEnable(GL_BLEND);
00160         glColor4f(0.0,0.0,0.0, 0.5);
00161         if (m_showSlider){
00162             glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
00163         }else{
00164             glRectf(width,0,m_width,m_height);
00165         }
00166         glDisable(GL_BLEND);
00167     }
00168 }
00169 
00170 
00171 void GLscene::drawSensorOutput(Body *body, Sensor *sensor)
00172 {
00173     if (m_log->index()<0) return;
00174 
00175     LogManager<SceneState> *lm 
00176         = (LogManager<SceneState> *)m_log;
00177     SceneState &sstate = lm->state();
00178     if (bodyIndex(body->name())<0){
00179         std::cerr << "invalid bodyIndex(" << bodyIndex(body->name()) 
00180                   << ") for " << body->name() << std::endl;
00181         return;
00182     } 
00183     const BodyState &state = sstate.bodyStates[bodyIndex(body->name())];
00184 
00185     if (sensor->type == Sensor::RANGE){
00186         RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
00187         const std::vector<double> distances = state.range[sensor->id];
00188         if (distances.empty()) return;
00189         int scan_half = (int)(range->scanAngle/2/range->scanStep);
00190         double th;
00191         Vector3 v;
00192         v[1] = 0.0;
00193         glDisable(GL_LIGHTING);
00194         glBegin(GL_LINES);
00195         glColor3f(1,0,0);
00196         for (int i = -scan_half,j=0; i<= scan_half; i++,j++){
00197             th = i*range->scanStep;
00198             double d = distances[j] ? distances[j] : range->maxDistance;
00199             v[0] = -d*sin(th); 
00200             v[2] = -d*cos(th); 
00201             glVertex3f(0,0,0); glVertex3f(v[0], v[1], v[2]);
00202         }
00203         glEnd();
00204         glEnable(GL_LIGHTING);
00205     }else if(sensor->type == Sensor::VISION){
00206         VisionSensor *v = dynamic_cast<VisionSensor *>(sensor);
00207         double far = v->far, near = v->near;
00208         glDisable(GL_LIGHTING);
00209         glColor3f(1,1,1);
00210         glBegin(GL_LINES);
00211         double t = tan(v->fovy/2);
00212         double xf = t*far*v->width/v->height, yf = t*far;
00213         glVertex3f( xf,  yf, -far); glVertex3f(-xf,  yf, -far);
00214         glVertex3f(-xf,  yf, -far); glVertex3f(-xf, -yf, -far);
00215         glVertex3f(-xf, -yf, -far); glVertex3f( xf, -yf, -far);
00216         glVertex3f( xf, -yf, -far); glVertex3f( xf,  yf, -far); 
00217         double xn = t*near*v->width/v->height, yn = t*near;
00218         glVertex3f( xn,  yn, -near); glVertex3f(-xn,  yn, -near);
00219         glVertex3f(-xn,  yn, -near); glVertex3f(-xn, -yn, -near);
00220         glVertex3f(-xn, -yn, -near); glVertex3f( xn, -yn, -near);
00221         glVertex3f( xn, -yn, -near); glVertex3f( xn,  yn, -near);
00222         glVertex3f( xn,  yn, -near); glVertex3f( xf,  yf, -far);
00223         glVertex3f(-xn,  yn, -near); glVertex3f(-xf,  yf, -far);
00224         glVertex3f(-xn, -yn, -near); glVertex3f(-xf, -yf, -far);
00225         glVertex3f( xn, -yn, -near); glVertex3f( xf, -yf, -far);
00226         glEnd();
00227         if (v->imageType == VisionSensor::DEPTH 
00228             || v->imageType == VisionSensor::COLOR_DEPTH 
00229             || v->imageType == VisionSensor::MONO_DEPTH){
00230             bool colored = v->imageType == VisionSensor::COLOR_DEPTH;
00231             glBegin(GL_POINTS);
00232             float *ptr = (float *)&v->depth[0];
00233             for (unsigned int i=0; i<v->depth.size()/16; i++){
00234                 glVertex3f(ptr[0], ptr[1], ptr[2]);
00235                 if (colored){
00236                     ptr += 3;
00237                     unsigned char *rgb = (unsigned char *)ptr;
00238                     glColor3f(rgb[0]/255.0, rgb[1]/255.0, rgb[2]/255.0);
00239                     ptr++;
00240                 }else{
00241                     ptr += 4;
00242                 }
00243             }
00244             glEnd();
00245         } 
00246 
00247         glEnable(GL_LIGHTING);
00248     }
00249 }
00250 
00251 void GLscene::showSensors(bool flag)
00252 {
00253     m_showSensors = flag;
00254 }
00255 
00256 bool GLscene::showSensors()
00257 {
00258     return m_showSensors;
00259 }
00260 
00261 void GLscene::showCollision(bool flag)
00262 {
00263     m_showCollision = flag;
00264 }
00265 
00266 bool GLscene::showCollision()
00267 {
00268     return m_showCollision;
00269 }


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