GLlink.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #ifdef __APPLE__
00003 #include <OpenGL/gl.h>
00004 #else
00005 #include <GL/gl.h>
00006 #endif
00007 #include <hrpModel/Body.h>
00008 #include <hrpModel/Sensor.h>
00009 #include <hrpModel/Light.h>
00010 #include "GLutil.h"
00011 #include "GLbody.h"
00012 #include "GLcamera.h"
00013 #include "GLlink.h"
00014 #include "GLshape.h"
00015 
00016 using namespace OpenHRP;
00017 using namespace hrp;
00018 
00019 bool GLlink::m_useAbsTransformToDraw = false;
00020 int GLlink::m_drawMode = GLlink::DM_SOLID;
00021 
00022 void GLlink::useAbsTransformToDraw()
00023 {
00024     m_useAbsTransformToDraw = true;
00025 }
00026 
00027 GLlink::GLlink() : m_showAxes(false), m_highlight(false)
00028 {
00029     Rs = hrp::Matrix33::Identity();
00030     R  = hrp::Matrix33::Identity();
00031     setQ(0);
00032 }
00033 
00034 GLlink::~GLlink()
00035 {
00036     for (size_t i=0; i<m_shapes.size(); i++){
00037         delete m_shapes[i];
00038     }
00039     for (unsigned int i=0; i<m_cameras.size(); i++){
00040         delete m_cameras[i];
00041     }
00042 }
00043         
00044 size_t GLlink::draw(){
00045     size_t ntri=0;
00046     glPushMatrix();
00047     if (m_useAbsTransformToDraw){
00048         glMultMatrixd(m_absTrans);
00049     }else{
00050         glMultMatrixd(m_trans);
00051         glMultMatrixd(m_T_j);
00052     }
00053     if (m_drawMode != DM_COLLISION){
00054         for (size_t i=0; i<m_shapes.size(); i++){
00055             ntri += m_shapes[i]->draw(m_drawMode);
00056         }
00057         for (size_t i=0; i<m_cameras.size(); i++){
00058             ntri += m_cameras[i]->draw(m_drawMode);
00059         }
00060     }else{
00061         if (coldetModel && coldetModel->getNumTriangles()){
00062             ntri = coldetModel->getNumTriangles();
00063             Eigen::Vector3f n, v[3];
00064             int vindex[3];
00065             if (m_highlight){
00066                 float red[] = {1,0,0,1};
00067                 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, red);
00068             }else{
00069                 float gray[] = {0.8,0.8,0.8,1};
00070                 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, gray);
00071             }
00072             glBegin(GL_TRIANGLES);
00073             for (int i=0; i<coldetModel->getNumTriangles(); i++){
00074                 coldetModel->getTriangle(i, vindex[0], vindex[1], vindex[2]);
00075                 for (int j=0; j<3; j++){
00076                     coldetModel->getVertex(vindex[j], v[j][0], v[j][1], v[j][2]);
00077                 }
00078                 n = (v[1]-v[0]).cross(v[2]-v[0]);
00079                 n.normalize();
00080                 glNormal3fv(n.data());
00081                 for (int j=0; j<3; j++){
00082                     glVertex3fv(v[j].data());
00083                 }
00084             }
00085             glEnd();
00086         }
00087     }
00088     for (size_t i=0; i<sensors.size(); i++){
00089         Sensor *s = sensors[i];
00090         double T[16];
00091         for (int i=0; i<3; i++){
00092             for (int j=0; j<3; j++){
00093                 T[i*4+j] = s->localR(j,i);
00094             }
00095         }
00096         T[12] = s->localPos[0]; T[13] = s->localPos[1]; T[14] = s->localPos[2];
00097         T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
00098         glPushMatrix();
00099         glMultMatrixd(T);
00100         GLbody *glb = dynamic_cast<GLbody *>(body);
00101         glb->drawSensor(s);
00102         glPopMatrix();
00103     }
00104     for (size_t i=0; i<lights.size(); i++){
00105         Light *l = lights[i];
00106         int lid = GL_LIGHT0+l->id; 
00107         if (!l->on){
00108             glDisable(lid);
00109             continue;
00110         }
00111         double T[16];
00112         for (int j=0; j<3; j++){
00113             for (int k=0; k<3; k++){
00114                 T[j*4+k] = l->localR(k,j);
00115             }
00116         }
00117         T[12] = l->localPos[0]; T[13] = l->localPos[1]; T[14] = l->localPos[2];
00118         T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
00119         glPushMatrix();
00120         glMultMatrixd(T);
00121         glEnable(GL_LIGHTING);
00122         glEnable(lid);
00123         GLfloat pos[] = {0,0,0,1};
00124         glLightfv(lid, GL_POSITION, pos);
00125         GLfloat color[] = {l->color[0], l->color[1], l->color[2], 1};
00126         glLightfv(lid, GL_DIFFUSE,  color);
00127         if (l->type == POINT){
00128             glLightf(lid, GL_CONSTANT_ATTENUATION,  l->attenuation[0]);
00129             glLightf(lid, GL_LINEAR_ATTENUATION,    l->attenuation[1]);
00130             glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
00131         }else if(l->type == SPOT){
00132             glLightf(lid, GL_CONSTANT_ATTENUATION,  l->attenuation[0]);
00133             glLightf(lid, GL_LINEAR_ATTENUATION,    l->attenuation[1]);
00134             glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
00135             glLightf(lid, GL_SPOT_EXPONENT, 20);
00136             glLightf(lid, GL_SPOT_CUTOFF, l->cutOffAngle*180/M_PI);
00137             GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
00138             glLightfv(lid, GL_SPOT_DIRECTION, dir);
00139         }else if(l->type == DIRECTIONAL){
00140             GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
00141             glLightfv(lid, GL_SPOT_DIRECTION, dir);
00142         }
00143         glPopMatrix();
00144     }
00145 
00146     if (m_showAxes){
00147         glDisable(GL_LIGHTING);
00148         glBegin(GL_LINES);
00149         glColor3f(1,0,0);
00150         glVertex3f(0,0,0); glVertex3f(0.5, 0, 0);
00151         glColor3f(0,1,0);
00152         glVertex3f(0,0,0); glVertex3f(0, 0.5, 0);
00153         glColor3f(0,0,1);
00154         glVertex3f(0,0,0); glVertex3f(0, 0, 0.5);
00155         glEnd();
00156         glEnable(GL_LIGHTING);
00157     }
00158     if (!m_useAbsTransformToDraw){
00159         hrp::Link *l = child;
00160         while (l){
00161             ntri += ((GLlink *)l)->draw();
00162             l = l->sibling;
00163         }
00164     }
00165     glPopMatrix();
00166     return ntri;
00167 }
00168 
00169 void GLlink::setQ(double i_q){
00170     switch(jointType){
00171     case ROTATIONAL_JOINT:
00172         {
00173             Matrix33 R;
00174             Vector3 aLocal(Rs.transpose()*a);
00175             hrp::calcRodrigues(R, aLocal, i_q);
00176             m_T_j[ 0]=R(0,0);m_T_j[ 1]=R(1,0);m_T_j[ 2]=R(2,0);m_T_j[3]=0; 
00177             m_T_j[ 4]=R(0,1);m_T_j[ 5]=R(1,1);m_T_j[ 6]=R(2,1);m_T_j[7]=0; 
00178             m_T_j[ 8]=R(0,2);m_T_j[ 9]=R(1,2);m_T_j[10]=R(2,2);m_T_j[11]=0;
00179             m_T_j[12]=0;     m_T_j[13]=0;     m_T_j[14]=0;     m_T_j[15]=1;    
00180         }
00181         break;
00182     case SLIDE_JOINT:
00183         {
00184             Vector3 aLocal(Rs.transpose()*d);
00185             m_T_j[ 0]=1;m_T_j[ 1]=0;m_T_j[ 2]=0;m_T_j[ 3]=0; 
00186             m_T_j[ 4]=0;m_T_j[ 5]=1;m_T_j[ 6]=0;m_T_j[ 7]=0; 
00187             m_T_j[ 8]=0;m_T_j[ 9]=0;m_T_j[10]=1;m_T_j[11]=0;
00188             m_T_j[12]=aLocal[0]*i_q;
00189             m_T_j[13]=aLocal[1]*i_q;
00190             m_T_j[14]=aLocal[2]*i_q;
00191             m_T_j[15]=1;    
00192         }
00193         break;
00194     default:
00195         m_T_j[ 0]=1;m_T_j[ 1]=0;m_T_j[ 2]=0;m_T_j[ 3]=0; 
00196         m_T_j[ 4]=0;m_T_j[ 5]=1;m_T_j[ 6]=0;m_T_j[ 7]=0; 
00197         m_T_j[ 8]=0;m_T_j[ 9]=0;m_T_j[10]=1;m_T_j[11]=0;
00198         m_T_j[12]=0;m_T_j[13]=0;m_T_j[14]=0;m_T_j[15]=1;    
00199         break;
00200     }
00201     //printf("m_T_j:\n");
00202     //printMatrix(m_T_j);
00203 }
00204 
00205 void GLlink::setAbsTransform(double i_trans[16]){
00206     memcpy(m_absTrans, i_trans, sizeof(double)*16);
00207 }
00208 
00209 GLcamera *GLlink::findCamera(const char *i_name){
00210     std::string name(i_name);
00211     for (unsigned int i=0; i<m_cameras.size(); i++){
00212         if (m_cameras[i]->name() == name) return m_cameras[i];
00213     }
00214     return NULL;
00215 }
00216 
00217 void GLlink::computeAbsTransform(){
00218     computeAbsTransform(m_absTrans);
00219 }
00220 
00221 void GLlink::computeAbsTransform(double o_trans[16]){
00222     if (m_useAbsTransformToDraw){
00223         memcpy(o_trans, m_absTrans, sizeof(double)*16);
00224         return;
00225     }
00226     if (parent){
00227         double trans1[16], trans2[16];
00228         mulTrans(m_T_j, m_trans, trans1);
00229         ((GLlink *)parent)->computeAbsTransform(trans2);
00230         mulTrans(trans1, trans2, o_trans);
00231     }else{
00232         memcpy(o_trans, m_trans, sizeof(double)*16);
00233     }
00234 }
00235 
00236 void GLlink::addShape(GLshape *shape)
00237 {
00238     m_shapes.push_back(shape);
00239 }
00240 
00241 void GLlink::addCamera(GLcamera *camera)
00242 {
00243     m_cameras.push_back(camera);
00244 }
00245 
00246 void GLlink::showAxes(bool flag)
00247 {
00248     m_showAxes = flag;
00249 }
00250 
00251 hrp::Link *GLlinkFactory()
00252 {
00253     return new GLlink();
00254 }
00255 
00256 const std::vector<GLcamera *>& GLlink::cameras()
00257 {
00258     return m_cameras;
00259 }
00260 
00261 int GLlink::drawMode()
00262 {
00263     return m_drawMode;
00264 }
00265 
00266 void GLlink::drawMode(int i_mode)
00267 {
00268     m_drawMode = i_mode;
00269 }
00270 
00271 void GLlink::highlight(bool flag)
00272 {
00273     m_highlight = flag;
00274     for (size_t i=0; i<m_shapes.size(); i++){
00275         m_shapes[i]->highlight(flag);
00276     }
00277     for (size_t i=0; i<m_cameras.size(); i++){
00278         m_cameras[i]->highlight(flag);
00279     }
00280 }
00281 
00282 void GLlink::divideLargeTriangles(double maxEdgeLen)
00283 {
00284     for (size_t i=0; i<m_shapes.size(); i++){
00285         m_shapes[i]->divideLargeTriangles(maxEdgeLen);
00286     }
00287 }
00288 
00289 void GLlink::computeAABB(hrp::Vector3& o_min, hrp::Vector3& o_max)
00290 {
00291     if (m_useAbsTransformToDraw){
00292         hrp::Vector3 mi, ma, p(m_absTrans[12], m_absTrans[13], m_absTrans[14]);
00293         hrp::Matrix33 R;
00294         for (int i=0; i<3; i++){
00295             for (int j=0; j<3; j++){
00296                 R(j,i) = m_absTrans[i*4+j];
00297             }
00298         }
00299         for (size_t i=0; i<m_shapes.size(); i++){
00300             m_shapes[i]->computeAABB(p, R, mi, ma);
00301             if (i==0){
00302                 o_min = mi; o_max = ma;
00303             }else{
00304                 for (int j=0; j<3; j++){
00305                     if (o_min[j] > mi[j]) o_min[j] = mi[j];
00306                     if (o_max[j] < ma[j]) o_max[j] = ma[j];
00307                 }
00308             }
00309         }
00310     }else{
00311         // TODO: implement
00312         //glMultMatrixd(m_trans);
00313         //glMultMatrixd(m_T_j);
00314     }
00315 }


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