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 <OpenGL/gl.h>
00007 #else
00008 #include <GL/gl.h>
00009 #endif
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 "GLscene.h"
00015 
00016 using namespace OpenHRP;
00017 using namespace hrp;
00018 
00019 void GLscene::updateScene()
00020 { 
00021     if (m_log->index()<0) return;
00022 
00023     LogManager<OpenHRP::WorldState> *lm 
00024         = (LogManager<OpenHRP::WorldState> *)m_log;
00025     OpenHRP::WorldState &state = lm->state();
00026     for (unsigned int i=0; i<state.characterPositions.length(); i++){
00027         const CharacterPosition& cpos = state.characterPositions[i];
00028         std::string cname(cpos.characterName);
00029         GLbody *glbody = dynamic_cast<GLbody *>(body(cname).get());
00030         if (!glbody) {
00031             //std::cerr << "can't find a body named " << cname << std::endl;
00032             continue;
00033         }
00034         for (unsigned int j=0; j<cpos.linkPositions.length(); j++){
00035             const LinkPosition &lp = cpos.linkPositions[j];
00036             double T[] = {lp.R[0], lp.R[3], lp.R[6],0,
00037                           lp.R[1], lp.R[4], lp.R[7],0,
00038                           lp.R[2], lp.R[5], lp.R[8],0,
00039                           lp.p[0], lp.p[1], lp.p[2],1};
00040 #if 0
00041             for (int i=0; i<4; i++){
00042                 for (int j=0; j<4; j++){
00043                     printf("%6.3f ", T[i*4+j]);
00044                 }
00045                 printf("\n");
00046             }
00047             printf("\n");
00048 #endif
00049             ((GLlink *)glbody->link(j))->setAbsTransform(T);
00050         }
00051     }
00052 }
00053 
00054 void GLscene::drawAdditionalLines()
00055 {
00056     LogManager<OpenHRP::WorldState> *lm 
00057         = (LogManager<OpenHRP::WorldState> *)m_log;
00058     OpenHRP::WorldState state;
00059     if (!lm->state(state)) return;
00060 
00061     glBegin(GL_LINES);
00062     glColor3f(1,0,0);
00063     double e[3];
00064     const CollisionSequence &cs = state.collisions;
00065     for (unsigned int i=0; i<cs.length(); i++){
00066         const CollisionPointSequence& cps = cs[i].points; 
00067         for (unsigned int j=0; j<cps.length(); j++){
00068             glVertex3dv(cps[j].position);
00069             for (int k=0; k<3; k++){
00070                 e[k] = cps[j].position[k] + cps[j].normal[k]*(cps[j].idepth*10+0.1);
00071             }
00072             glVertex3dv(e);
00073         }
00074     }
00075     glEnd();
00076 }
00077 
00078 GLscene::GLscene(LogManagerBase *i_log) 
00079   : GLsceneBase(i_log)
00080 {
00081 }
00082 
00083 GLscene::~GLscene()
00084 {
00085 }


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