GLutil.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <math.h>
00003 #ifdef __APPLE__
00004 #include <OpenGL/gl.h>
00005 #include <OpenGL/glu.h>
00006 #else
00007 #include <GL/gl.h>
00008 #include <GL/glu.h>
00009 #endif
00010 #include <hrpUtil/Eigen3d.h>
00011 #include "GLshape.h"
00012 #include "GLbody.h"
00013 #include "GLlink.h"
00014 #include "GLcamera.h"
00015 #include "GLtexture.h"
00016 #include "GLutil.h"
00017 
00018 using namespace OpenHRP;
00019 using namespace hrp;
00020 
00021 class shapeLoader{
00022 public:
00023     void setShapeSetInfo(ShapeSetInfo_ptr i_ssinfo);
00024     void loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo, GLshape *(*shapeFactory)());
00025     void loadShapeFromLinkInfo(GLlink *link, const LinkInfo &i_li, GLshape *(*shapeFactory)());
00026     void loadShape(GLshape *shape, 
00027                    const OpenHRP::TransformedShapeIndex &i_tsi);
00028     void loadCamera(GLcamera *camera, const OpenHRP::SensorInfo &i_si); 
00029     ShapeInfoSequence_var sis;
00030     AppearanceInfoSequence_var ais;
00031     MaterialInfoSequence_var mis;
00032     TextureInfoSequence_var txs;
00033 };
00034 
00035 void shapeLoader::setShapeSetInfo(ShapeSetInfo_ptr i_ssinfo)
00036 {
00037     sis = i_ssinfo->shapes();
00038     ais = i_ssinfo->appearances();
00039     mis = i_ssinfo->materials(); 
00040     txs = i_ssinfo->textures();
00041 }
00042 
00043 void shapeLoader::loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo,
00044                                         GLshape *(*shapeFactory)())
00045 {
00046     LinkInfoSequence_var lis = i_binfo->links();
00047     for (unsigned int i=0; i<lis->length(); i++){
00048         hrp::Link *l = body->link(std::string(lis[i].name));
00049         if (l){
00050             loadShapeFromLinkInfo((GLlink *)l, lis[i], shapeFactory);
00051         }else{
00052             std::cout << "can't find a link named " << lis[i].name 
00053                       << std::endl;
00054         }
00055     }
00056 }
00057 
00058 void mulTrans(const double i_m1[16], const double i_m2[16], double o_m[16])
00059 {
00060     for (int i=0; i<4; i++){
00061         for (int j=0; j<4;j++){
00062             double v = 0;
00063             for (int k=0; k<4; k++){
00064                 v += i_m1[i*4+k]*i_m2[j+k*4];
00065             }
00066             o_m[i*4+j] = v;
00067         }
00068     }
00069 }
00070 
00071 bool loadTextureFromTextureInfo(GLtexture *texture, TextureInfo &ti)
00072 {
00073     if (ti.image.length() == 0){
00074         std::cerr << "texture image is not loaded(" << ti.url << ")" << std::endl;
00075         return false;
00076     }else if (ti.numComponents != 3 && ti.numComponents != 4){
00077         std::cerr << "texture image which has "
00078                   << ti.numComponents 
00079                   << " components is not supported(" << ti.url << ")"
00080                   << std::endl;
00081         return false;
00082     }
00083     texture->repeatS = ti.repeatS;
00084     texture->repeatT = ti.repeatT;
00085     texture->numComponents = ti.numComponents;
00086     texture->url = ti.url;
00087     texture->width = ti.width;
00088     texture->height = ti.height;
00089     texture->image.resize(ti.image.length());
00090     memcpy(&texture->image[0], ti.image.get_buffer(), ti.image.length());
00091     return true;
00092 }
00093 
00094 void shapeLoader::loadShape(GLshape *shape, 
00095                             const OpenHRP::TransformedShapeIndex &i_tsi)
00096 {
00097     shape->setTransform(i_tsi.transformMatrix);
00098     ShapeInfo& si = sis[i_tsi.shapeIndex];
00099     shape->setVertices(si.vertices.length()/3, si.vertices.get_buffer());
00100     shape->setTriangles(si.triangles.length()/3, 
00101                         (int *)si.triangles.get_buffer());
00102     const AppearanceInfo& ai = ais[si.appearanceIndex];
00103     shape->setNormals(ai.normals.length()/3, ai.normals.get_buffer());
00104     shape->setNormalIndices(ai.normalIndices.length(), 
00105                             (int *)ai.normalIndices.get_buffer()); 
00106     shape->setTextureCoordinates(ai.textureCoordinate.length()/2, 
00107                                  ai.textureCoordinate.get_buffer());
00108     shape->setTextureCoordIndices(ai.textureCoordIndices.length(),
00109                                   (int *)ai.textureCoordIndices.get_buffer());
00110     shape->setColors(ai.colors.length()/3, ai.colors.get_buffer());
00111     if (ai.textureIndex >=0){
00112         if (txs->length() <= (unsigned int)ai.textureIndex){
00113             std::cerr << "invalid texture index(" << ai.textureIndex << ")"
00114                       << std::endl;
00115         }else{
00116             TextureInfo &ti = txs[ai.textureIndex];
00117             GLtexture *texture = new GLtexture();
00118             if (loadTextureFromTextureInfo(texture, ti)){
00119                 shape->setTexture(texture);
00120             }else{
00121                 delete texture;
00122             }
00123         }
00124     }
00125     if (ai.colors.length()){
00126         shape->setDiffuseColor(ai.colors[0], ai.colors[1], ai.colors[2], 1.0);
00127     }else if (ai.materialIndex >= 0){ 
00128         const MaterialInfo& mi = mis[ai.materialIndex];
00129         shape->setDiffuseColor(mi.diffuseColor[0], mi.diffuseColor[1], mi.diffuseColor[2], 1.0-mi.transparency);
00130         shape->setShininess(mi.shininess);
00131         shape->setSpecularColor(mi.specularColor[0], mi.specularColor[1], mi.specularColor[2]);
00132     }else{
00133         //std::cout << "no material" << std::endl;
00134     }
00135     shape->normalPerVertex(ai.normalPerVertex);
00136     shape->solid(ai.solid);
00137     shape->compile();
00138 }
00139 
00140 
00141 void loadCube(GLshape *shape, double x, double y, double z)
00142 {
00143     double hx = x/2, hy = y/2, hz = z/2;
00144     float vertices[] = {hx,hy,hz,
00145                         -hx,hy,hz,
00146                         -hx,-hy,hz,
00147                         hx,-hy,hz,
00148                         hx,hy,-hz,
00149                         -hx,hy,-hz,
00150                         -hx,-hy,-hz,
00151                         hx,-hy,-hz};
00152     int triangles[] = {0,2,3,//+z
00153                        0,1,2,//+z
00154                        4,3,7,//+x
00155                        4,0,3,//+x
00156                        0,4,5,//+y
00157                        5,1,0,//+y
00158                        1,5,6,//-x
00159                        1,6,2,//-x
00160                        2,6,7,//-y
00161                        2,7,3,//-y
00162                        7,6,4,//-z
00163                        5,4,6};//-z
00164     float normals[] = {1,0,0,
00165                        0,1,0,
00166                        0,0,1,
00167                        -1,0,0,
00168                        0,-1,0,
00169                        0,0,-1};
00170     int normalIndices[] = {2,2,0,0,1,1,3,3,4,4,5,5};
00171     shape->setVertices(8, vertices);
00172     shape->setTriangles(12, triangles);
00173     shape->setNormals(6, normals);
00174     shape->setNormalIndices(12, normalIndices);
00175     shape->setDiffuseColor(0.8, 0.8, 0.8, 1.0);
00176     shape->normalPerVertex(false);
00177     shape->solid(true);
00178     shape->compile();
00179 }
00180 
00181 void loadShapeFromBodyInfo(GLbody *body, BodyInfo_var i_binfo,
00182                            GLshape *(*shapeFactory)())
00183 {
00184     shapeLoader loader;
00185     loader.setShapeSetInfo(i_binfo);
00186     loader.loadShapeFromBodyInfo(body, i_binfo, shapeFactory);
00187 }
00188 
00189 void loadShapeFromSceneInfo(GLlink *link, SceneInfo_var i_sinfo, 
00190                             GLshape *(*shapeFactory)())
00191 {
00192     shapeLoader loader;
00193     loader.setShapeSetInfo(i_sinfo);
00194     TransformedShapeIndexSequence_var tsis = i_sinfo->shapeIndices();
00195     for (size_t i = 0; i<tsis->length(); i++){
00196         GLshape *shape = shapeFactory ? shapeFactory() : new GLshape();
00197         loader.loadShape(shape, tsis[i]);
00198         link->addShape(shape);
00199     }
00200 }
00201 
00202 void loadShapeFromLinkInfo(GLlink *link, 
00203                            const OpenHRP::LinkInfo &i_li, 
00204                            OpenHRP::ShapeSetInfo_ptr i_ssinfo,
00205                            GLshape *(*shapeFactory)())
00206 {
00207     shapeLoader loader;
00208     loader.setShapeSetInfo(i_ssinfo);
00209     loader.loadShapeFromLinkInfo(link, i_li, shapeFactory);
00210 }
00211 
00212 void shapeLoader::loadShapeFromLinkInfo(GLlink *link, const LinkInfo &i_li, GLshape *(*shapeFactory)()){
00213     Vector3 axis;
00214     Matrix33 R;
00215     
00216     for (int i=0; i<3; i++) axis[i] = i_li.rotation[i];
00217     hrp::calcRodrigues(R, axis, i_li.rotation[3]);
00218 
00219     double trans[16];
00220     trans[ 0]=R(0,0);trans[ 1]=R(0,1);trans[ 2]=R(0,2);trans[3]=i_li.translation[0]; 
00221     trans[ 4]=R(1,0);trans[ 5]=R(1,1);trans[ 6]=R(1,2);trans[7]=i_li.translation[1]; 
00222     trans[ 8]=R(2,0);trans[ 9]=R(2,1);trans[10]=R(2,2);trans[11]=i_li.translation[2]; 
00223     link->setTransform(trans);
00224     link->setQ(0);
00225     link->computeAbsTransform();
00226 
00227     for (size_t i = 0; i<i_li.shapeIndices.length(); i++){
00228         GLshape *shape = shapeFactory ? shapeFactory() : new GLshape();
00229         loadShape(shape, i_li.shapeIndices[i]);
00230         link->addShape(shape);
00231     }
00232 
00233     const SensorInfoSequence& sensors = i_li.sensors;
00234     for (unsigned int i=0; i<sensors.length(); i++){
00235         const SensorInfo& si = sensors[i];
00236         std::string type(si.type);
00237         if (type == "Vision"){
00238             //std::cout << si.name << std::endl;
00239             GLcamera *camera = new GLcamera(si.specValues[4], si.specValues[5],
00240                                             si.specValues[0], si.specValues[1],
00241                                             si.specValues[2],
00242                                             link, si.id);
00243             loadCamera(camera, si);
00244             link->addCamera(camera);
00245         }else{
00246             Vector3 p;
00247             p[0] = si.translation[0];
00248             p[1] = si.translation[1];
00249             p[2] = si.translation[2];
00250             Matrix33 R;
00251             Vector3 axis;
00252             axis[0] = si.rotation[0];
00253             axis[1] = si.rotation[1];
00254             axis[2] = si.rotation[2];
00255             hrp::calcRodrigues(R, axis, si.rotation[3]);
00256     
00257             for (size_t i=0; i<si.shapeIndices.length(); i++){
00258                 GLshape *shape = shapeFactory ? shapeFactory() : new GLshape();
00259                 loadShape(shape, si.shapeIndices[i]);
00260                 Vector3 newp = R*shape->getPosition()+p;
00261                 shape->setPosition(newp[0], newp[1], newp[2]);
00262                 Matrix33 newR = R*shape->getRotation();
00263                 shape->setRotation(newR);
00264                 link->addShape(shape);
00265             }
00266         }
00267     }
00268 }
00269 
00270 void shapeLoader::loadCamera(GLcamera *i_camera, const SensorInfo &i_si)
00271 {
00272     i_camera->name(std::string(i_si.name));
00273     i_camera->setPosition(i_si.translation[0], i_si.translation[1],
00274                           i_si.translation[2]);
00275     i_camera->setRotation(i_si.rotation[0], i_si.rotation[1],
00276                           i_si.rotation[2], i_si.rotation[3]);
00277 
00278     for (size_t i=0; i<i_si.shapeIndices.length(); i++){
00279         GLshape *shape = new GLshape();
00280         loadShape(shape, i_si.shapeIndices[i]);
00281         i_camera->addShape(shape);
00282     }
00283 }


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