GLmodel.cpp
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <fstream>
00003 #include <GL/glfw.h>
00004 #include <sys/time.h>
00005 #include "GLmodel.h"
00006 
00007 using namespace OpenHRP;
00008 using namespace hrp;
00009 
00010 #define DEFAULT_W 640
00011 #define DEFAULT_H 480
00012 
00013 GLcamera::GLcamera(const SensorInfo &i_si, GLlink *i_link) : m_name(i_si.name), m_link(i_link) {
00014     
00015     Matrix33 R;
00016     Vector3 axis;
00017     axis[0] = i_si.rotation[0];
00018     axis[1] = i_si.rotation[1];
00019     axis[2] = i_si.rotation[2];
00020     
00021     hrp::calcRodrigues(R, axis, i_si.rotation[3]);
00022     
00023     m_trans[ 0]=R(0,0);m_trans[ 1]=R(1,0);m_trans[ 2]=R(2,0);m_trans[3]=0; 
00024     m_trans[ 4]=R(0,1);m_trans[ 5]=R(1,1);m_trans[ 6]=R(2,1);m_trans[7]=0; 
00025     m_trans[ 8]=R(0,2);m_trans[ 9]=R(1,2);m_trans[10]=R(2,2);m_trans[11]=0; 
00026     m_trans[12]=i_si.translation[0];m_trans[13]=i_si.translation[1];
00027     m_trans[14]=i_si.translation[2];m_trans[15]=1; 
00028 
00029     m_near = i_si.specValues[0];
00030     m_far  = i_si.specValues[1];
00031     m_fovy = i_si.specValues[2];
00032     m_width  = i_si.specValues[4];
00033     m_height = i_si.specValues[5];
00034 }
00035 
00036 GLcamera::GLcamera(int i_width, int i_height, double i_near, double i_far, double i_fovy) : m_near(i_near), m_far(i_far), m_fovy(i_fovy), m_width(i_width), m_height(i_height)
00037 {
00038 }
00039 
00040 const std::string& GLcamera::name() const {
00041     return m_name;
00042 }
00043 
00044 void GLcamera::computeAbsTransform(double o_trans[16]){
00045     if (m_link){
00046         double trans[16];
00047         m_link->computeAbsTransform(trans);
00048         mulTrans(m_trans, trans, o_trans);
00049     }else{
00050         memcpy(o_trans, m_trans, sizeof(double)*16);
00051     }
00052 }
00053 
00054 void GLcamera::setView()
00055 {
00056     computeAbsTransform(m_absTrans);
00057     
00058     glMatrixMode(GL_PROJECTION);
00059     glLoadIdentity();
00060     gluPerspective(fovy()*180/M_PI, 
00061                    (double)width() / (double)height(), 
00062                    near(), far());
00063     gluLookAt(m_absTrans[12], m_absTrans[13], m_absTrans[14], 
00064               m_absTrans[12]-m_absTrans[8], 
00065               m_absTrans[13]-m_absTrans[9], 
00066               m_absTrans[14]-m_absTrans[10],
00067               m_absTrans[4], m_absTrans[5], m_absTrans[6]);
00068 }
00069 
00070 void GLcamera::setTransform(double i_trans[16]){
00071     memcpy(m_trans, i_trans, sizeof(double)*16);
00072 }
00073 
00074 void GLcamera::getAbsTransform(double o_trans[16]){
00075     memcpy(o_trans, m_absTrans, sizeof(double)*16);
00076 }
00077 
00078 void GLcamera::getDepthOfLine(int i_row, float *o_depth)
00079 {
00080     glReadPixels(0, i_row, width(), 1, GL_DEPTH_COMPONENT, GL_FLOAT, o_depth);
00081 }
00082 
00083 
00084 GLlink::GLlink(const LinkInfo &i_li, BodyInfo_var i_binfo) : m_parent(NULL), m_jointId(i_li.jointId){
00085     Vector3 axis;
00086     Matrix33 R;
00087     
00088     for (int i=0; i<3; i++){
00089         m_axis[i] = i_li.jointAxis[i];
00090         axis[i] = i_li.rotation[i];
00091     }
00092     setQ(0);
00093     
00094     hrp::calcRodrigues(R, axis, i_li.rotation[3]);
00095     
00096     m_trans[ 0]=R(0,0);m_trans[ 1]=R(1,0);m_trans[ 2]=R(2,0);m_trans[3]=0; 
00097     m_trans[ 4]=R(0,1);m_trans[ 5]=R(1,1);m_trans[ 6]=R(2,1);m_trans[7]=0; 
00098     m_trans[ 8]=R(0,2);m_trans[ 9]=R(1,2);m_trans[10]=R(2,2);m_trans[11]=0; 
00099     m_trans[12]=i_li.translation[0];m_trans[13]=i_li.translation[1];
00100     m_trans[14]=i_li.translation[2];m_trans[15]=1; 
00101     
00102     
00103     m_list = glGenLists(1);
00104     //std::cout << i_li.name << std::endl;
00105     //printMatrix(m_trans);
00106     
00107     glNewList(m_list, GL_COMPILE);
00108     
00109     ShapeInfoSequence_var sis = i_binfo->shapes();
00110     AppearanceInfoSequence_var ais = i_binfo->appearances();
00111     MaterialInfoSequence_var mis = i_binfo->materials();
00112     const TransformedShapeIndexSequence& tsis = i_li.shapeIndices;
00113     for (unsigned int l=0; l<tsis.length(); l++){
00114         const TransformedShapeIndex &tsi = tsis[l];
00115         double tform[16];
00116         for (int i=0; i<3; i++){
00117             for (int j=0; j<4; j++){
00118                 tform[j*4+i] = tsi.transformMatrix[i*4+j];
00119             }
00120         }
00121         tform[3] = tform[7] = tform[11] = 0.0; tform[15] = 1.0;
00122         
00123         glPushMatrix();
00124         glMultMatrixd(tform);
00125         //printMatrix(tform);
00126         
00127         glBegin(GL_TRIANGLES);
00128         short index = tsi.shapeIndex;
00129         ShapeInfo& si = sis[index];
00130         const float *vertices = si.vertices.get_buffer();
00131         const LongSequence& triangles = si.triangles;
00132         const AppearanceInfo& ai = ais[si.appearanceIndex];
00133         const float *normals = ai.normals.get_buffer();
00134         //std::cout << "length of normals = " << ai.normals.length() << std::endl;
00135         const LongSequence& normalIndices = ai.normalIndices;
00136         //std::cout << "length of normalIndices = " << normalIndices.length() << std::endl;
00137         const int numTriangles = triangles.length() / 3;
00138         //std::cout << "numTriangles = " << numTriangles << std::endl;
00139         if (ai.materialIndex >= 0){ 
00140             const MaterialInfo& mi = mis[ai.materialIndex];
00141             glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, 
00142                          mi.diffuseColor);
00143         }else{
00144             std::cout << "no material" << std::endl;
00145         }
00146         for(int j=0; j < numTriangles; ++j){
00147             if (!ai.normalPerVertex){
00148                 int p;
00149                 if (normalIndices.length() == 0){
00150                     p = j*3;
00151                 }else{
00152                     p = normalIndices[j]*3;
00153                 }
00154                 glNormal3fv(normals+p);
00155             }
00156             for(int k=0; k < 3; ++k){
00157                 if (ai.normalPerVertex){
00158                     int p = normalIndices[j*3+k]*3;
00159                     glNormal3fv(normals+p);
00160                 }
00161                 long orgVertexIndex = si.triangles[j * 3 + k];
00162                 int p = orgVertexIndex * 3;
00163                 glVertex3fv(vertices+p);
00164             }
00165         }
00166         glEnd();
00167         glPopMatrix();
00168     }
00169     
00170     glEndList();
00171     
00172     const SensorInfoSequence& sensors = i_li.sensors;
00173     for (unsigned int i=0; i<sensors.length(); i++){
00174         const SensorInfo& si = sensors[i];
00175         std::string type(si.type);
00176         if (type == "Vision"){
00177             //std::cout << si.name << std::endl;
00178             m_cameras.push_back(new GLcamera(si, this));
00179         }
00180     }
00181         
00182 }
00183         
00184 void GLlink::draw(){
00185     glPushMatrix();
00186     glMultMatrixd(m_trans);
00187     glMultMatrixd(m_T_j);
00188     glCallList(m_list);
00189     for (unsigned int i=0; i<m_children.size(); i++){
00190         m_children[i]->draw();
00191     }
00192     glPopMatrix();
00193 }
00194 
00195 void GLlink::setParent(GLlink *i_parent){
00196     m_parent = i_parent;
00197 }
00198 
00199 void GLlink::addChild(GLlink *i_child){
00200     i_child->setParent(this);
00201     m_children.push_back(i_child);
00202 }
00203 
00204 void GLlink::setQ(double i_q){
00205     Matrix33 R;
00206     hrp::calcRodrigues(R, m_axis, i_q);
00207     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; 
00208     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; 
00209     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;
00210     m_T_j[12]=0;     m_T_j[13]=0;     m_T_j[14]=0;     m_T_j[15]=1;    
00211     //printf("m_T_j:\n");
00212     //printMatrix(m_T_j);
00213 }
00214 
00215 void GLlink::setTransform(double i_trans[16]){
00216     memcpy(m_trans, i_trans, sizeof(double)*16);
00217 }
00218 
00219 int GLlink::jointId(){
00220     return m_jointId;
00221 }
00222 
00223 GLcamera *GLlink::findCamera(const char *i_name){
00224     std::string name(i_name);
00225     for (unsigned int i=0; i<m_cameras.size(); i++){
00226         if (m_cameras[i]->name() == name) return m_cameras[i];
00227     }
00228     return NULL;
00229 }
00230 
00231 void GLlink::computeAbsTransform(double o_trans[16]){
00232     if (m_parent){
00233         double trans1[16], trans2[16];
00234         mulTrans(m_T_j, m_trans, trans1);
00235         m_parent->computeAbsTransform(trans2);
00236         mulTrans(trans1, trans2, o_trans);
00237     }else{
00238         memcpy(o_trans, m_trans, sizeof(double)*16);
00239     }
00240 }
00241 
00242 GLbody::GLbody(BodyInfo_var i_binfo){
00243     LinkInfoSequence_var lis = i_binfo->links();
00244     
00245     for (unsigned int i=0; i<lis->length(); i++){
00246         m_links.push_back(new GLlink(lis[i], i_binfo));
00247     }
00248     // setup tree
00249     for (unsigned int i=0; i<m_links.size(); i++){
00250         const LinkInfo &li = lis[i];
00251         if (li.parentIndex < 0) m_root = m_links[i];
00252         for (unsigned int j=0; j<li.childIndices.length(); j++){
00253             m_links[i]->addChild(m_links[li.childIndices[j]]);
00254         }
00255     }
00256     
00257 }
00258 
00259 GLbody::~GLbody(){
00260     for (unsigned int i=0; i<m_links.size(); i++){
00261         delete m_links[i];
00262     }
00263 }
00264 
00265 void GLbody::setPosture(double *i_angles, double *i_pos, double *i_rpy){
00266     double tform[16];
00267     Matrix33 R = rotFromRpy(i_rpy[0], i_rpy[1], i_rpy[2]);
00268     tform[ 0]=R(0,0);tform[ 1]=R(1,0);tform[ 2]=R(2,0);tform[ 3]=0;
00269     tform[ 4]=R(0,1);tform[ 5]=R(1,1);tform[ 6]=R(2,1);tform[ 7]=0;
00270     tform[ 8]=R(0,2);tform[ 9]=R(1,2);tform[10]=R(2,2);tform[11]=0;
00271     tform[12]=i_pos[0];tform[13]=i_pos[1];tform[14]=i_pos[2];tform[15]=1;
00272     m_root->setTransform(tform);
00273     for (unsigned int i=0; i<m_links.size(); i++){
00274         int id = m_links[i]->jointId();
00275         if (id >= 0){
00276             m_links[i]->setQ(i_angles[id]);
00277         }
00278     }
00279 }
00280 
00281 void GLbody::draw(){
00282     m_root->draw();
00283 }
00284 
00285 GLcamera *GLbody::findCamera(const char *i_name){
00286     for (unsigned int i=0; i<m_links.size(); i++){
00287         GLcamera *camera = m_links[i]->findCamera(i_name);
00288         if (camera) return camera;
00289     }
00290     return NULL;
00291 }
00292 
00293 void GLscene::addBody(GLbody *i_body){
00294     m_bodies.push_back(i_body);
00295 }
00296 
00297 void GLscene::draw(bool swap){
00298     m_camera->setView();
00299 #if 0
00300     struct timeval tv1, tv2;
00301     gettimeofday(&tv1, NULL);
00302 #endif
00303     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00304 
00305     glMatrixMode(GL_MODELVIEW);
00306 
00307     for (unsigned int i=0; i<m_bodies.size(); i++){
00308         m_bodies[i]->draw();
00309     }
00310 
00311     if (swap) glfwSwapBuffers();
00312 #if 0
00313     gettimeofday(&tv2, NULL);
00314     //std::cout << "t = " << ((tv2.tv_sec - tv1.tv_sec)*1e3 + (tv2.tv_usec - tv1.tv_usec)*1e-3) << "[ms]" << std::endl;
00315     //std::cout << (int)(1.0/((tv2.tv_sec - tv1.tv_sec) + (tv2.tv_usec - tv1.tv_usec)*1e-6)) << "[FPS]" << std::endl;
00316 #endif
00317 }
00318 
00319 void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
00320 {
00321     for (int i=0; i<4; i++){
00322         for (int j=0; j<4;j++){
00323             double v = 0;
00324             for (int k=0; k<4; k++){
00325                 v += i_m1[i*4+k]*i_m2[j+k*4];
00326             }
00327             o_m[i*4+j] = v;
00328         }
00329     }
00330 }
00331 
00332 GLscene *GLscene::getInstance()
00333 {
00334     return m_scene;
00335 }
00336 
00337 void printMatrix(double mat[16])
00338 {
00339     for (int i=0; i<4; i++){
00340         for (int j=0; j<4; j++){
00341             printf("%6.3f ", mat[j*4+i]);
00342         }
00343         printf("\n");
00344     }
00345 }
00346 
00347 GLscene::GLscene() : m_camera(m_default_camera)
00348 {
00349     m_default_camera = new GLcamera(DEFAULT_W, DEFAULT_H, 1.0, 100.0, 40*M_PI/180);
00350     double T[] = {0,1,0,0,
00351                   0,0,1,0,
00352                   1,0,0,0,
00353                   4,0,0.8,1};
00354     m_default_camera->setTransform(T);
00355 }
00356 
00357 GLscene::~GLscene()
00358 {
00359     delete m_default_camera;
00360 }
00361 
00362 void GLscene::init()
00363 {
00364     glfwInit();
00365 
00366     glfwOpenWindow(DEFAULT_W,DEFAULT_H,0,0,0,0,24,0, GLFW_WINDOW);
00367 
00368     setCamera(m_default_camera);
00369 
00370     GLfloat light0pos[] = { 0.0, 4.0, 6.0, 1.0 };
00371     GLfloat light1pos[] = { 6.0, 4.0, 0.0, 1.0 };
00372     GLfloat white[] = { 0.6, 0.6, 0.6, 1.0 };
00373 
00374     glClearColor(0, 0, 0, 1.0);
00375     glEnable(GL_DEPTH_TEST);
00376 
00377     glEnable(GL_CULL_FACE);
00378     glCullFace(GL_BACK);
00379 
00380     glEnable(GL_LIGHTING);
00381     glEnable(GL_LIGHT0);
00382     glEnable(GL_LIGHT1);
00383     glLightfv(GL_LIGHT0, GL_DIFFUSE, white);
00384     glLightfv(GL_LIGHT0, GL_SPECULAR, white);
00385     glLightfv(GL_LIGHT1, GL_DIFFUSE, white);
00386     glLightfv(GL_LIGHT1, GL_SPECULAR, white);
00387     glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
00388     glLightfv(GL_LIGHT1, GL_POSITION, light1pos);
00389 }
00390 
00391 void GLscene::save(const char *i_fname)
00392 {
00393     int w, h;
00394     glfwGetWindowSize(&w,&h);
00395     unsigned char *buffer = new unsigned char[w*h*3];
00396 
00397     capture(buffer);
00398     std::ofstream ofs(i_fname, std::ios::out | std::ios::trunc | std::ios::binary );
00399     char buf[10];
00400     sprintf(buf, "%d %d", w, h);
00401     ofs << "P6" << std::endl << buf << std::endl << "255" << std::endl;
00402     for (int i=0; i<h; i++){
00403         ofs.write((char *)(buffer+i*w*3), w*3);
00404     }
00405     delete [] buffer;
00406 }
00407 
00408 void GLscene::capture(unsigned char *o_buffer)
00409 {
00410     int w, h;
00411     glfwGetWindowSize(&w,&h);
00412     glReadBuffer(GL_BACK);
00413     glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
00414     for (int i=0; i<h; i++){
00415         glReadPixels(0,(h-1-i),w,1,GL_RGB,GL_UNSIGNED_BYTE,
00416                      o_buffer + i*3*w);
00417     }
00418 }
00419 
00420 void GLscene::setCamera(GLcamera *i_camera)
00421 {
00422     if (!i_camera) return;
00423 
00424     m_camera = i_camera;
00425     glfwSetWindowSize(m_camera->width(), m_camera->height());
00426     glViewport(0, 0, m_camera->width(), m_camera->height());
00427 }
00428 
00429 GLcamera *GLscene::getCamera()
00430 {
00431     return m_camera;
00432 }
00433 
00434 GLscene* GLscene::m_scene = new GLscene();
00435 
00436 
00437 unsigned int GLscene::numBodies() const
00438 {
00439     return m_bodies.size();
00440 }
00441 
00442 GLbody *GLscene::body(unsigned int i_rank)
00443 {
00444     if (i_rank >= numBodies()) return NULL;
00445     return m_bodies[i_rank];
00446 }


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