IrrModel.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include "IrrModel.h"
00003 
00004 using namespace irr;
00005 
00006 using namespace core;
00007 using namespace scene;
00008 using namespace video;
00009 using namespace io;
00010 
00011 using namespace OpenHRP;
00012 using namespace hrp;
00013 
00014 class MyEventReceiver : public IEventReceiver
00015 {
00016 public:
00017     MyEventReceiver(ICameraSceneNode *i_camera, double i_radius=3.0) : 
00018         m_camera(i_camera),
00019         m_radius(i_radius),
00020         m_pan(-M_PI/6),
00021         m_tilt(0){
00022         updateEye();
00023     }
00024 
00025     virtual bool OnEvent(const SEvent& event)
00026         {
00027             if(event.EventType == EET_KEY_INPUT_EVENT)
00028                 {
00029                     if(event.KeyInput.PressedDown)
00030                         {
00031                             switch(event.KeyInput.Key)
00032                                 {
00033                                 case KEY_LEFT:
00034                                     return true;
00035                                 case KEY_RIGHT:
00036                                     return true;
00037                                 case KEY_UP:
00038                                     return true;
00039                                 case KEY_DOWN:
00040                                     return true;
00041                                 default:
00042                                     return false;
00043                                 }
00044                         }
00045                     return true;
00046                 }
00047             if(event.EventType == EET_MOUSE_INPUT_EVENT)
00048                 {
00049                     switch(event.MouseInput.Event)
00050                         {
00051                         case EMIE_LMOUSE_PRESSED_DOWN:
00052                             m_mouse.X = event.MouseInput.X;
00053                             m_mouse.Y = event.MouseInput.Y;
00054                             return true;
00055                         case EMIE_MOUSE_WHEEL:
00056                             //上回転
00057                             if(event.MouseInput.Wheel == 1)
00058                                 {
00059                                     if (m_radius > 0.001){
00060                                         m_radius *= 0.9;
00061                                         updateEye();
00062                                     }
00063                                 }
00064                             //下回転
00065                             else if(event.MouseInput.Wheel == -1)
00066                                 {
00067                                     m_radius *= 1.1;
00068                                     updateEye();
00069                                 }
00070                             return true;
00071                         case EMIE_MOUSE_MOVED:
00072                             if (event.MouseInput.isLeftPressed()){
00073                                 s32 dx = event.MouseInput.X - m_mouse.X;
00074                                 s32 dy = event.MouseInput.Y - m_mouse.Y;
00075                                 m_pan  += dx*0.01;
00076                                 m_tilt += dy*0.01;
00077                                 if (m_tilt < -M_PI/2) m_tilt = -M_PI/2;
00078                                 if (m_tilt >  M_PI/2) m_tilt =  M_PI/2;
00079                                 updateEye();
00080                                 m_mouse.X = event.MouseInput.X;
00081                                 m_mouse.Y = event.MouseInput.Y;
00082                             }
00083                             return true;
00084 
00085                         default:
00086                             return true;
00087                         }
00088                 }
00089             return false;
00090         }
00091 private:
00092     void updateEye(){
00093         vector3df target = m_camera->getTarget();
00094         m_eye.X = target.X + m_radius*cos(m_tilt)*cos(m_pan);
00095         m_eye.Y = target.Y + m_radius*cos(m_tilt)*sin(m_pan);
00096         m_eye.Z = target.Z + m_radius*sin(m_tilt);
00097         m_camera->setPosition(m_eye);
00098     }
00099     ICameraSceneNode *m_camera;
00100     position2d<s32> m_mouse;
00101     vector3df m_eye;
00102     float m_radius, m_pan, m_tilt;
00103 };
00104 
00105 class GLlink : public ISceneNode
00106 {
00107 public:
00108     GLlink(ISceneNode *i_parent, ISceneManager *i_mgr, s32 i_id,
00109                         const LinkInfo &i_li, BodyInfo_var i_binfo) :
00110         ISceneNode(i_parent, i_mgr, i_id),
00111         m_jointId(i_li.jointId){
00112         setAutomaticCulling(scene::EAC_OFF);
00113 
00114         
00115         setPosition(vector3df( i_li.translation[0], 
00116                               -i_li.translation[1], 
00117                                i_li.translation[2]));
00118         Vector3 axis(i_li.rotation[0],
00119                      i_li.rotation[1],
00120                      i_li.rotation[2]);
00121         Matrix33 R;
00122         hrp::calcRodrigues(R, axis, i_li.rotation[3]);
00123         Vector3 rpy(rpyFromRot(R));
00124         //std::cout << "rpy:" << rpy << std::endl;
00125         setRotation(vector3df(-180/M_PI*rpy[0],
00126                                180/M_PI*rpy[1],
00127                               -180/M_PI*rpy[2]));
00128         
00129         m_axis << i_li.jointAxis[0], i_li.jointAxis[1], i_li.jointAxis[2];
00130 
00131         ShapeInfoSequence_var sis = i_binfo->shapes();
00132         AppearanceInfoSequence_var ais = i_binfo->appearances();
00133         MaterialInfoSequence_var mis = i_binfo->materials();
00134         TextureInfoSequence_var txs = i_binfo->textures();
00135         const TransformedShapeIndexSequence& tsis = i_li.shapeIndices;
00136         
00137         
00138         core::vector3df vertex;
00139         core::vector3df normal;
00140         
00141         for (unsigned int l=0; l<tsis.length(); l++){
00142             SMesh* mesh = new SMesh();
00143             SMeshBuffer* meshBuffer = new SMeshBuffer();
00144             mesh->addMeshBuffer(meshBuffer);
00145             meshBuffer->drop();
00146             
00147             const TransformedShapeIndex &tsi = tsis[l];
00148             short index = tsi.shapeIndex;
00149             ShapeInfo& si = sis[index];
00150             const float *vertices = si.vertices.get_buffer();
00151             const LongSequence& triangles = si.triangles;
00152             const AppearanceInfo& ai = ais[si.appearanceIndex];
00153             const float *normals = ai.normals.get_buffer();
00154             //std::cout << "length of normals = " << ai.normals.length() << std::endl;
00155             const LongSequence& normalIndices = ai.normalIndices;
00156             //std::cout << "length of normalIndices = " << normalIndices.length() << std::endl;
00157             const int numTriangles = triangles.length() / 3;
00158             //std::cout << "numTriangles = " << numTriangles << std::endl;
00159             
00160             video::SColor color(0xffffffff);
00161             if (ai.colors.length()){
00162                 color.set(0xff, 
00163                           0xff*ai.colors[0], 
00164                           0xff*ai.colors[1], 
00165                           0xff*ai.colors[2]);
00166             }else if (ai.materialIndex >= 0){ 
00167                 const MaterialInfo& mi = mis[ai.materialIndex];
00168                 color.set(0xff, 
00169                           0xff*mi.diffuseColor[0], 
00170                           0xff*mi.diffuseColor[1], 
00171                           0xff*mi.diffuseColor[2]);
00172             }else{
00173                 std::cout << "no material" << std::endl;
00174             }
00175             
00176             
00177             SMeshBuffer* mb = reinterpret_cast<SMeshBuffer*>(mesh->getMeshBuffer(mesh->getMeshBufferCount()-1));
00178             u32 vCount = mb->getVertexCount();
00179 
00180             const DblArray12& tfm = tsi.transformMatrix;
00181             CMatrix4<f32> cmat;
00182             for (int i=0; i<3; i++){
00183                 for (int j=0; j<4; j++){
00184                     cmat[j*4+i] = tfm[i*4+j];
00185                 }
00186             }
00187             cmat[3] = cmat[7] = cmat[11] = 0.0; cmat[15] = 1.0;
00188             vector3df pos = cmat.getTranslation();
00189             pos.Y *= -1;
00190             vector3df rpy = cmat.getRotationDegrees();
00191             rpy.X *= -1;
00192             rpy.Z *= -1;
00193             vector3df scale = cmat.getScale();
00194 
00195             const float *textureCoordinate = NULL;
00196             if (ai.textureIndex >= 0){
00197                 textureCoordinate = ai.textureCoordinate.get_buffer();
00198                 //std::cout << "length of textureCoordinate:" << ai.textureCoordinate.length() << std::endl;
00199                 //std::cout << "length of vertices:" << si.vertices.length() << std::endl;
00200                 
00201             }
00202             
00203             for(int j=0; j < numTriangles; ++j){
00204                 if (!ai.normalPerVertex){
00205                     int p;
00206                     if (normalIndices.length() == 0){
00207                         p = j*3;
00208                     }else{
00209                         p = normalIndices[j]*3;
00210                     }
00211                     if ( normals != NULL ) {
00212                         normal.X =  normals[p];
00213                         normal.Y = -normals[p+1]; //left-handed->right-handed
00214                         normal.Z =  normals[p+2];
00215                     } else {
00216                         normal.X = 0;
00217                         normal.Y = 0;
00218                         normal.Z = 1;
00219                     }
00220                 }
00221                 for(int k=0; k < 3; ++k){
00222                     long orgVertexIndex = si.triangles[j * 3 + k];
00223                     if (ai.normalPerVertex){
00224                         int p;
00225                         if (normalIndices.length()){
00226                             p = normalIndices[j*3+k]*3;
00227                         }else{
00228                             p = orgVertexIndex*3;
00229                         }
00230                         normal.X =  normals[p];
00231                         normal.Y = -normals[p+1]; //left-handed -> right-handed
00232                         normal.Z =  normals[p+2];
00233                     }
00234                     int p = orgVertexIndex * 3;
00235                     vertex.X =  scale.X*vertices[p];
00236                     vertex.Y = -scale.Y*vertices[p+1]; // left-handed -> right-handed
00237                     vertex.Z =  scale.Z*vertices[p+2];
00238                     //std::cout << vertices[p] <<"," << vertices[p+1] << "," << vertices[p+2] << std::endl;
00239                     vector2df texc;
00240                     if (textureCoordinate){
00241                         
00242                         texc.X = textureCoordinate[ai.textureCoordIndices[j*3+k]*2];
00243                         texc.Y = textureCoordinate[ai.textureCoordIndices[j*3+k]*2+1];
00244                     }
00245                     // redundant vertices
00246                     mb->Vertices.push_back(video::S3DVertex(vertex,normal,color, texc));
00247                 }
00248                 mb->Indices.push_back(vCount);
00249                 mb->Indices.push_back(vCount+2);
00250                 mb->Indices.push_back(vCount+1);
00251                 vCount += 3;
00252             }
00253             mesh->getMeshBuffer(0)->recalculateBoundingBox();
00254             
00255             // Create the Animated mesh if there's anything in the mesh
00256             SAnimatedMesh* pAM = 0;
00257             if ( 0 != mesh->getMeshBufferCount() )
00258                 {
00259                     mesh->recalculateBoundingBox();
00260                     pAM = new SAnimatedMesh();
00261                     pAM->Type = EAMT_OBJ;
00262                     pAM->addMesh(mesh);
00263                     pAM->recalculateBoundingBox();
00264                 }
00265             
00266             mesh->drop();
00267 
00268             vector3df noscale(1,1,1);
00269 
00270             IMeshSceneNode *node 
00271                 = i_mgr->addMeshSceneNode(mesh, this, -1,
00272                                           pos, 
00273                                           rpy,
00274                                           noscale);
00275             
00276             if (ai.textureIndex >= 0){
00277                 const TextureInfo& ti = txs[ai.textureIndex];
00278                 //std::cout << "url:" << ti.url << std::endl;
00279                 video::IVideoDriver* driver = i_mgr->getVideoDriver();
00280                 const char *path = ti.url;
00281                 SMaterial& mat = node->getMaterial(0);
00282                 ITexture *texture = driver->getTexture(path);
00283                 mat.setTexture( 0, texture);
00284             }
00285             
00286         }
00287 
00288         const SensorInfoSequence& sensors = i_li.sensors;
00289         for (unsigned int i=0; i<sensors.length(); i++){
00290             const SensorInfo& si = sensors[i];
00291             std::string type(si.type);
00292             if (type == "Vision"){
00293                 //std::cout << si.name << std::endl;
00294                 ISceneNode *camera = i_mgr->addEmptySceneNode(this);
00295                 camera->setName(si.name);
00296                 camera->setPosition(vector3df( si.translation[0],
00297                                               -si.translation[1],
00298                                                si.translation[2]));
00299                 Vector3 axis(si.rotation[0],
00300                              si.rotation[1],
00301                              si.rotation[2]);
00302                 Matrix33 R;
00303                 hrp::calcRodrigues(R, axis, si.rotation[3]);
00304                 Vector3 rpy(rpyFromRot(R));
00305                 camera->setRotation(vector3df(-180/M_PI*rpy[0],
00306                                                180/M_PI*rpy[1],
00307                                               -180/M_PI*rpy[2]));
00308                 m_cameraInfos.push_back(new GLcamera(si, camera));
00309             }
00310         }
00311     }
00312     virtual void render() {}
00313     virtual const aabbox3d<f32>& getBoundingBox() const { return m_box; }
00314     void setQ(double i_q){
00315         Matrix33 R;
00316         hrp::calcRodrigues(R, m_axis, i_q);
00317         Vector3 rpy(rpyFromRot(R));
00318         rpy *= 180/M_PI;
00319         vector3df euler(-rpy[0], rpy[1], -rpy[2]);
00320         setRotation(euler);
00321     }
00322     int jointId() const { return m_jointId; }
00323     GLcamera *findCamera(const char *i_name){
00324         for (unsigned int i=0; i<m_cameraInfos.size(); i++){
00325             if (strcmp(i_name, m_cameraInfos[i]->name())==0) return m_cameraInfos[i];
00326         }
00327         return NULL;
00328     }
00329 private:
00330     aabbox3d<f32> m_box;
00331     Vector3 m_axis;
00332     int m_jointId;
00333     std::vector<GLcamera *> m_cameraInfos;
00334 };
00335 
00336 GLbody::GLbody(ISceneNode *i_parent, ISceneManager *i_mgr, s32 i_id,
00337                                  BodyInfo_var i_binfo) :
00338     ISceneNode(i_parent, i_mgr, i_id){
00339     setAutomaticCulling(scene::EAC_OFF);
00340     
00341     LinkInfoSequence_var lis = i_binfo->links();
00342     //std::cout << "creating links" << std::endl;
00343     for (unsigned int i=0; i<lis->length(); i++){
00344         m_links.push_back(
00345             new GLlink(i_mgr->getRootSceneNode(), i_mgr, 
00346                                     -1, lis[i], i_binfo));
00347     }
00348     //std::cout << "creating a tree" << std::endl;
00349     // setup tree
00350     for (unsigned int i=0; i<m_links.size(); i++){
00351         const LinkInfo &li = lis[i];
00352         if (li.parentIndex < 0) {
00353             m_root = m_links[i];
00354             addChild(m_links[i]);
00355         }
00356         for (unsigned int j=0; j<li.childIndices.length(); j++){
00357             m_links[i]->addChild(m_links[li.childIndices[j]]);
00358         }
00359     }
00360     //std::cout << "done" << std::endl;
00361 }
00362 
00363 void GLbody::setPosture(double *i_angles, double *i_pos, double *i_rpy){
00364     m_root->setPosition(vector3df(i_pos[0], -i_pos[1], i_pos[2])); 
00365     m_root->setRotation(vector3df(-180/M_PI*i_rpy[0],
00366                                    180/M_PI*i_rpy[1],
00367                                   -180/M_PI*i_rpy[2]));
00368     for (unsigned int i=0; i<m_links.size(); i++){
00369         int id = m_links[i]->jointId();
00370         if (id >= 0){
00371             m_links[i]->setQ(i_angles[id]);
00372         }
00373     }
00374 }
00375 
00376 GLcamera *GLbody::findCamera(const char *i_name)
00377 {
00378     for (unsigned int i=0; i<m_links.size(); i++){
00379         GLcamera *ci = m_links[i]->findCamera(i_name);
00380         if (ci) return ci;
00381     }
00382     return NULL;
00383 }
00384 
00385 GLcamera::GLcamera(const OpenHRP::SensorInfo& i_info, ISceneNode *i_node) :
00386     m_node(i_node)
00387 {
00388     m_near = i_info.specValues[0];
00389     m_far  = i_info.specValues[1];
00390     m_fovy  = i_info.specValues[2];
00391     m_width  = i_info.specValues[4];
00392     m_height = i_info.specValues[5];
00393 }
00394 
00395 
00396 void GLcamera::setCameraParameters(ICameraSceneNode *i_camera)
00397 {
00398 #if 0 // This doesn't work
00399     i_camera->setAspectRatio(((float)m_width)/m_height);
00400     i_camera->setNearValue(m_near);
00401     i_camera->setFarValue(m_far);
00402     i_camera->setFOV(m_fov);
00403 #else
00404     matrix4 m;
00405     double t = m_near*tan(m_fovy/2), r = (t*m_width)/m_height;
00406     m[0] = m_near/r;
00407     m[1] = 0;
00408     m[2] = 0;
00409     m[3] = 0;
00410 
00411     m[4] = 0;
00412     m[5] = m_near/t;
00413     m[6] = 0;
00414     m[7] = 0;
00415 
00416     m[8] = 0;
00417     m[9] = 0;
00418     m[10] = (m_far+m_near)/(m_far-m_near);
00419     m[11] = 1;
00420 
00421     m[12] = 0;
00422     m[13] = 0;
00423     m[14] = -2*m_far*m_near/(m_far-m_near);
00424     m[15] = 0;
00425     i_camera->setProjectionMatrix(m);
00426 #endif
00427 }
00428 
00429 void updateAbsoluteTransformation(ISceneNode *i_node)
00430 {
00431     ISceneNode *parent = i_node->getParent();
00432     if (parent){
00433         updateAbsoluteTransformation(parent);
00434     }
00435     i_node->updateAbsolutePosition();
00436 }
00437 
00438 void GLcamera::updateCameraTransform(ICameraSceneNode *i_camera)
00439 {
00440     updateAbsoluteTransformation(m_node);
00441     matrix4 mat = m_node->getAbsoluteTransformation();
00442     vector3df pos = mat.getTranslation();
00443     i_camera->setPosition(pos);
00444     vector3df view(pos.X-mat[8],pos.Y-mat[9],pos.Z-mat[10]); // -Z
00445     vector3df up(-mat[4], -mat[5], -mat[6]); // -Y axis
00446     i_camera->setTarget(view);
00447     i_camera->setUpVector(up);
00448 }
00449 
00450 const char *GLcamera::name()
00451 {
00452     return m_node->getName();
00453 }
00454 
00455 int GLcamera::width()
00456 {
00457     return m_width;
00458 }
00459 
00460 int GLcamera::height()
00461 {
00462     return m_height;
00463 }
00464 
00465 void GLcamera::getAbsTransform(double *o_T)
00466 {
00467     matrix4 mat = m_node->getAbsoluteTransformation();
00468     vector3df pos = mat.getTranslation();
00469     vector3df rpy = mat.getRotationDegrees();
00470     Matrix33 R(rotFromRpy(-rpy.X*M_PI/180, rpy.Y*M_PI/180, -rpy.Z*M_PI/180));
00471     o_T[ 0] = R(0,0);o_T[ 4] = R(0,1);o_T[ 8] = R(0,2);o_T[12] =  pos.X;
00472     o_T[ 1] = R(1,0);o_T[ 5] = R(1,1);o_T[ 9] = R(1,2);o_T[13] = -pos.Y;
00473     o_T[ 2] = R(2,0);o_T[ 6] = R(2,1);o_T[10] = R(2,2);o_T[14] =  pos.Z;
00474     o_T[ 3] = 0;     o_T[ 7] = 0;     o_T[11] = 0;    ;o_T[15] =  1.0;
00475 }
00476 
00477 float GLcamera::near()
00478 {
00479     return m_near;
00480 }
00481 
00482 float GLcamera::far()
00483 {
00484     return m_far;
00485 }
00486 
00487 float GLcamera::fovy()
00488 {
00489     return m_fovy;
00490 }
00491 
00492 GLscene::GLscene() : m_device(NULL), m_camera(NULL), m_cnode(NULL)
00493 {
00494 }
00495 
00496 void GLscene::draw()
00497 {
00498     m_device->run();
00499     if (m_camera != m_defaultCamera) m_camera->updateCameraTransform(m_cnode);
00500     m_device->getVideoDriver()->beginScene(true, true, SColor(255,100,101,140));
00501     m_device->getSceneManager()->drawAll();
00502     m_device->getVideoDriver()->endScene();
00503     
00504     int fps = m_device->getVideoDriver()->getFPS();
00505     int prims = m_device->getVideoDriver()->getPrimitiveCountDrawn();
00506     wchar_t tmp[1024];
00507     swprintf(tmp, 1024, L"Irrlicht (fps:%d) Triangles:%d", fps, prims);
00508     m_device->setWindowCaption(tmp);
00509 }
00510 
00511 GLscene::~GLscene()
00512 {
00513     if (m_defaultCamera) delete m_defaultCamera;
00514 }
00515 
00516 GLscene *GLscene::m_scene = NULL;
00517 
00518 GLscene *GLscene::getInstance()
00519 {
00520     if (!m_scene) m_scene = new GLscene;
00521     return m_scene;
00522 }
00523 
00524 bool GLscene::init(int w, int h)
00525 {
00526     m_device =
00527         createDevice( video::EDT_OPENGL, dimension2d<u32>(w, h), 32,
00528                       false, false, false, 0);
00529     
00530     if (!m_device) return false;
00531     
00532     m_device->setWindowCaption(L"Irrlicht");
00533 
00534     ISceneManager* smgr = m_device->getSceneManager();
00535     smgr->addLightSceneNode(0, vector3df(18,-12,6), SColorf(1.0, 1.0, 1.0), 30.0f);
00536     smgr->addLightSceneNode(0, vector3df(-18,12,6), SColorf(1.0, 1.0, 1.0), 30.0f);
00537     m_cnode = smgr->addCameraSceneNode();
00538 #if 1
00539     m_cnode->setTarget(vector3df(0,0,0.7));
00540     m_cnode->setUpVector(vector3df(0,0,1));
00541 #endif
00542     m_receiver = new MyEventReceiver(m_cnode, 3);
00543     m_device->setEventReceiver(m_receiver);
00544     m_defaultCamera = new GLcamera(m_cnode);
00545     setCamera(m_defaultCamera);
00546 
00547     return true;
00548 }
00549 
00550 GLbody *GLscene::addBody(OpenHRP::BodyInfo_var i_binfo)
00551 {
00552     ISceneManager* smgr = m_device->getSceneManager();
00553     return new GLbody(smgr->getRootSceneNode(), smgr, -1, i_binfo);
00554 }
00555 
00556 void GLscene::setCamera(GLcamera *i_camera)
00557 {
00558     m_camera = i_camera;
00559     m_camera->setCameraParameters(m_cnode);
00560 }
00561 
00562 
00563 GLcamera *GLscene::getCamera()
00564 {
00565     return m_camera;
00566 }
00567 
00568 GLcamera::GLcamera(ISceneNode *i_node) : m_node(i_node), m_near(0.1), m_far(100.0), m_fovy(M_PI/4), m_width(640), m_height(480)
00569 {
00570 }
00571 
00572 ISceneManager *GLscene::getSceneManager()
00573 {
00574     return m_device->getSceneManager();
00575 }
00576 
00577 IVideoDriver *GLscene::getVideoDriver()
00578 {
00579     return m_device->getVideoDriver();
00580 }


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