ShapeSetInfo_impl.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00015 #include "ShapeSetInfo_impl.h"
00016 
00017 #include <map>
00018 #include <vector>
00019 #include <iostream>
00020 #include <boost/bind.hpp>
00021 #include <sys/stat.h>
00022 
00023 #include <hrpCorba/ViewSimulator.hh>
00024 #include <hrpUtil/VrmlNodes.h>
00025 #include <hrpUtil/ImageConverter.h>
00026 
00027 #include "VrmlUtil.h"
00028 
00029 
00030 
00031 using namespace std;
00032 using namespace boost;
00033 
00034     
00035 
00036 ShapeSetInfo_impl::ShapeSetInfo_impl(PortableServer::POA_ptr poa) :
00037     poa(PortableServer::POA::_duplicate(poa))
00038 {
00039     triangleMeshShaper.setNormalGenerationMode(true);
00040     triangleMeshShaper.sigMessage.connect(boost::bind(&putMessage, _1));
00041 }
00042 
00043 
00044 ShapeSetInfo_impl::~ShapeSetInfo_impl()
00045 {
00046     
00047 }
00048 
00049 
00050 PortableServer::POA_ptr ShapeSetInfo_impl::_default_POA()
00051 {
00052     return PortableServer::POA::_duplicate(poa);
00053 }
00054 
00055 
00056 void ShapeSetInfo_impl::applyTriangleMeshShaper(VrmlNodePtr node)
00057 {
00058     triangleMeshShaper.apply(node);
00059 }
00060 
00061 
00062 void ShapeSetInfo_impl::putMessage(const std::string& message)
00063 {
00064     cout << message;
00065 }
00066 
00067 
00074 string& ShapeSetInfo_impl::replace(string& str, const string& sb, const string& sa)
00075 {
00076     string::size_type n, nb = 0;
00077     
00078     while ((n = str.find(sb,nb)) != string::npos){
00079         str.replace(n,sb.size(),sa);
00080         nb = n + sa.size();
00081     }
00082     
00083     return str;
00084 }
00085 
00086 
00087 ShapeInfoSequence* ShapeSetInfo_impl::shapes()
00088 {
00089     return new ShapeInfoSequence(shapes_);
00090 }
00091 
00092 
00093 AppearanceInfoSequence* ShapeSetInfo_impl::appearances()
00094 {
00095     return new AppearanceInfoSequence(appearances_);
00096 }
00097 
00098 
00099 MaterialInfoSequence* ShapeSetInfo_impl::materials()
00100 {
00101     return new MaterialInfoSequence(materials_);
00102 }
00103 
00104 
00105 TextureInfoSequence* ShapeSetInfo_impl::textures()
00106 {
00107     return new TextureInfoSequence(textures_);
00108 }
00109 
00110 
00123 void ShapeSetInfo_impl::traverseShapeNodes
00124 (VrmlNode* node, const Matrix44& T, TransformedShapeIndexSequence& io_shapeIndices, DblArray12Sequence& inlinedShapeM, const SFString* url)
00125 {
00126     static int inline_count=0;   
00127     SFString url_ = *url;
00128     if(node->isCategoryOf(PROTO_INSTANCE_NODE)){
00129         VrmlProtoInstance* protoInstance = static_cast<VrmlProtoInstance*>(node);
00130         if(protoInstance->actualNode){
00131             traverseShapeNodes(protoInstance->actualNode.get(), T, io_shapeIndices, inlinedShapeM, url);
00132         }
00133 
00134     } else if(node->isCategoryOf(GROUPING_NODE)) {
00135         VrmlGroup* groupNode = static_cast<VrmlGroup*>(node);
00136         VrmlTransform* transformNode = dynamic_cast<VrmlTransform*>(groupNode);
00137         VrmlInline* inlineNode = NULL;
00138         
00139         inlineNode = dynamic_cast<VrmlInline*>(groupNode);
00140         const Matrix44* pT;
00141         if(inlineNode){
00142             if(!inline_count){
00143                 int inlinedShapeMIndex = inlinedShapeM.length();
00144                 inlinedShapeM.length(inlinedShapeMIndex+1);
00145                 int p = 0;
00146                 for(int row=0; row < 3; ++row){
00147                     for(int col=0; col < 4; ++col){
00148                         inlinedShapeM[inlinedShapeMIndex][p++] = T(row, col);
00149                     }
00150                 }  
00151                 url_ = inlineNode->urls[0];
00152             }
00153             inline_count++;
00154             for( MFString::iterator ite = inlineNode->urls.begin(); ite != inlineNode->urls.end(); ++ite ){
00155                 string filename(deleteURLScheme(*ite));
00156                 struct stat statbuff;
00157                 time_t mtime = 0;
00158                 if( stat( filename.c_str(), &statbuff ) == 0 ){
00159                     mtime = statbuff.st_mtime;
00160                 }
00161                 fileTimeMap.insert(make_pair(filename, mtime));
00162             }
00163         }
00164         Matrix44 T2;
00165         if( transformNode ){
00166             Matrix44 Tlocal;
00167             calcTransformMatrix(transformNode, Tlocal);
00168             T2 = T * Tlocal;
00169             pT = &T2;
00170         } else {
00171             pT = &T;
00172         }
00173 
00174         VrmlSwitch* switchNode = dynamic_cast<VrmlSwitch*>(node);
00175         if(switchNode){
00176             int whichChoice = switchNode->whichChoice;
00177             if( whichChoice >= 0 && whichChoice < switchNode->countChildren() )
00178                 traverseShapeNodes(switchNode->getChild(whichChoice), *pT, io_shapeIndices, inlinedShapeM, &url_);
00179         }else{
00180             for(int i=0; i < groupNode->countChildren(); ++i){
00181                 traverseShapeNodes(groupNode->getChild(i), *pT, io_shapeIndices, inlinedShapeM, &url_);
00182             }
00183         }
00184         if(inlineNode)
00185             inline_count--;
00186 
00187         
00188     } else if(node->isCategoryOf(SHAPE_NODE)) {
00189 
00190         VrmlShape* shapeNode = static_cast<VrmlShape*>(node);
00191         short shapeInfoIndex;
00192         
00193         ShapeNodeToShapeInfoIndexMap::iterator p = shapeInfoIndexMap.find(shapeNode);
00194         if(p != shapeInfoIndexMap.end()){
00195             shapeInfoIndex = p->second;
00196         } else {
00197             shapeInfoIndex = createShapeInfo(shapeNode, url);
00198         }
00199         
00200         if(shapeInfoIndex >= 0){
00201             long length = io_shapeIndices.length();
00202             io_shapeIndices.length(length + 1);
00203             TransformedShapeIndex& tsi = io_shapeIndices[length];
00204             tsi.shapeIndex = shapeInfoIndex;
00205             int p = 0;
00206             for(int row=0; row < 3; ++row){
00207                 for(int col=0; col < 4; ++col){
00208                     tsi.transformMatrix[p++] = T(row, col);
00209                 }
00210             }
00211             if(inline_count)
00212                 tsi.inlinedShapeTransformMatrixIndex=inlinedShapeM.length()-1;
00213             else
00214                 tsi.inlinedShapeTransformMatrixIndex=-1;
00215         }
00216     }
00217 }
00218 
00222 int ShapeSetInfo_impl::createShapeInfo(VrmlShape* shapeNode, const SFString* url)
00223 {
00224     int shapeInfoIndex = -1;
00225 
00226     VrmlIndexedFaceSet* triangleMesh = dynamic_node_cast<VrmlIndexedFaceSet>(shapeNode->geometry).get();
00227 
00228     if(triangleMesh){
00229 
00230         shapeInfoIndex = shapes_.length();
00231         shapes_.length(shapeInfoIndex + 1);
00232         ShapeInfo& shapeInfo = shapes_[shapeInfoIndex];
00233 
00234         if ( url ) shapeInfo.url = CORBA::string_dup( url->c_str() );
00235         setTriangleMesh(shapeInfo, triangleMesh);
00236         setPrimitiveProperties(shapeInfo, shapeNode);
00237         shapeInfo.appearanceIndex = createAppearanceInfo(shapeInfo, shapeNode, triangleMesh, url);
00238         shapeInfoIndexMap.insert(make_pair(shapeNode, shapeInfoIndex));
00239     }
00240 
00241     return shapeInfoIndex;
00242 }
00243 
00244 
00245 void ShapeSetInfo_impl::setTriangleMesh(ShapeInfo& shapeInfo, VrmlIndexedFaceSet* triangleMesh)
00246 {
00247     const MFVec3f& vertices = triangleMesh->coord->point;
00248     size_t numVertices = vertices.size();
00249     shapeInfo.vertices.length(numVertices * 3);
00250 
00251     size_t pos = 0;
00252     for(size_t i=0; i < numVertices; ++i){
00253         const SFVec3f& v = vertices[i];
00254         shapeInfo.vertices[pos++] = v[0];
00255         shapeInfo.vertices[pos++] = v[1];
00256         shapeInfo.vertices[pos++] = v[2];
00257     }
00258 
00259     const MFInt32& indices = triangleMesh->coordIndex;
00260     const size_t numTriangles = indices.size() / 4;
00261     shapeInfo.triangles.length(numTriangles * 3);
00262         
00263     int dpos = 0;
00264     int spos = 0;
00265     for(size_t i=0; i < numTriangles; ++i){
00266         shapeInfo.triangles[dpos++] = indices[spos++];
00267         shapeInfo.triangles[dpos++] = indices[spos++];
00268         shapeInfo.triangles[dpos++] = indices[spos++];
00269         spos++; // skip a terminater '-1'
00270     }
00271 }
00272 
00273 
00274 void ShapeSetInfo_impl::setPrimitiveProperties(ShapeInfo& shapeInfo, VrmlShape* shapeNode)
00275 {
00276     shapeInfo.primitiveType = SP_MESH;
00277     FloatSequence& param = shapeInfo.primitiveParameters;
00278     
00279     VrmlNode *node = triangleMeshShaper.getOriginalGeometry(shapeNode).get();
00280     VrmlGeometry* originalGeometry = dynamic_cast<VrmlGeometry *>(node);
00281 
00282     if(originalGeometry){
00283 
00284         VrmlIndexedFaceSet* faceSet = dynamic_cast<VrmlIndexedFaceSet*>(originalGeometry);
00285 
00286         if(!faceSet){
00287             
00288             if(VrmlBox* box = dynamic_cast<VrmlBox*>(originalGeometry)){
00289                 shapeInfo.primitiveType = SP_BOX;
00290                 param.length(3);
00291                 for(int i=0; i < 3; ++i){
00292                     param[i] = box->size[i];
00293                 }
00294 
00295             } else if(VrmlCone* cone = dynamic_cast<VrmlCone*>(originalGeometry)){
00296                 shapeInfo.primitiveType = SP_CONE;
00297                 param.length(4);
00298                 param[0] = cone->bottomRadius;
00299                 param[1] = cone->height;
00300                 param[2] = cone->bottom ? 1.0 : 0.0;
00301                 param[3] = cone->side ? 1.0 : 0.0;
00302                 
00303             } else if(VrmlCylinder* cylinder = dynamic_cast<VrmlCylinder*>(originalGeometry)){
00304                 shapeInfo.primitiveType = SP_CYLINDER;
00305                 param.length(5);
00306                 param[0] = cylinder->radius;
00307                 param[1] = cylinder->height;
00308                 param[2] = cylinder->top    ? 1.0 : 0.0;
00309                 param[3] = cylinder->bottom ? 1.0 : 0.0;
00310                 param[4] = cylinder->side   ? 1.0 : 0.0;
00311                 
00312             
00313             } else if(VrmlSphere* sphere = dynamic_cast<VrmlSphere*>(originalGeometry)){
00314                 shapeInfo.primitiveType = SP_SPHERE;
00315                 param.length(1);
00316                 param[0] = sphere->radius;
00317             }
00318         }
00319     }else{
00320     
00321         VrmlProtoInstance *protoInstance = dynamic_cast<VrmlProtoInstance *>(node);
00322         if (protoInstance && protoInstance->proto->protoName == "Plane"){
00323             VrmlBox *box = dynamic_cast<VrmlBox *>(protoInstance->actualNode.get());
00324             shapeInfo.primitiveType = SP_PLANE;
00325             param.length(3);
00326             for (int i=0; i<3; i++){
00327                 param[i] = box->size[i];
00328             }
00329         }
00330     }
00331 }
00332 
00333 
00337 int ShapeSetInfo_impl::createAppearanceInfo
00338 (ShapeInfo& shapeInfo, VrmlShape* shapeNode, VrmlIndexedFaceSet* faceSet,
00339  const SFString *url)
00340 {
00341     int appearanceIndex = appearances_.length();
00342     appearances_.length(appearanceIndex + 1);
00343     AppearanceInfo& appInfo = appearances_[appearanceIndex];
00344 
00345     appInfo.normalPerVertex = faceSet->normalPerVertex;
00346     appInfo.colorPerVertex = faceSet->colorPerVertex;
00347     appInfo.solid = faceSet->solid;
00348     appInfo.creaseAngle = faceSet->creaseAngle;
00349     appInfo.materialIndex = -1;
00350     appInfo.textureIndex = -1;
00351 
00352     if(faceSet->color){
00353         setColors(appInfo, faceSet);
00354     }
00355 
00356     if(faceSet->normal){
00357         setNormals(appInfo, faceSet);
00358     } 
00359     
00360     VrmlAppearancePtr& appNode = shapeNode->appearance;
00361 
00362     if(appNode) {
00363         appInfo.materialIndex = createMaterialInfo(appNode->material);
00364         appInfo.textureIndex  = createTextureInfo (appNode->texture, url);
00365                 if(appInfo.textureIndex != -1 ){
00366                         createTextureTransformMatrix(appInfo, appNode->textureTransform);
00367             if(!faceSet->texCoord){   // default Texture Mapping
00368                 triangleMeshShaper.defaultTextureMapping(shapeNode);
00369             }
00370                         setTexCoords(appInfo, faceSet);
00371                 }
00372     }
00373 
00374     return appearanceIndex;
00375 }
00376 
00377 
00378 void ShapeSetInfo_impl::setColors(AppearanceInfo& appInfo, VrmlIndexedFaceSet* triangleMesh)
00379 {
00380     const MFColor& colors = triangleMesh->color->color;
00381     int numColors = colors.size();
00382     appInfo.colors.length(numColors * 3);
00383 
00384     int pos = 0;
00385     for(int i=0; i < numColors; ++i){
00386         const SFColor& color = colors[i];
00387         for(int j=0; j < 3; ++j){
00388             appInfo.colors[pos++] = color[j];
00389         }
00390     }
00391 
00392     const MFInt32& orgIndices = triangleMesh->colorIndex;
00393     const int numOrgIndices = orgIndices.size();
00394     if(numOrgIndices > 0){
00395         if(triangleMesh->colorPerVertex){
00396             const int numTriangles = numOrgIndices / 4; // considering delimiter element '-1'
00397             appInfo.colorIndices.length(numTriangles * 3);
00398             int dpos = 0;
00399             int spos = 0;
00400             for(int i=0; i < numTriangles; ++i){
00401                 appInfo.colorIndices[dpos++] = orgIndices[spos++];
00402                 appInfo.colorIndices[dpos++] = orgIndices[spos++];
00403                 appInfo.colorIndices[dpos++] = orgIndices[spos++];
00404                 spos++; // skip delimiter '-1'
00405             }
00406         } else { // color per face
00407             appInfo.colorIndices.length(numOrgIndices);
00408             for(int i=0; i < numOrgIndices; ++i){
00409                 appInfo.colorIndices[i] = orgIndices[i];
00410             }
00411         }
00412     }
00413 }
00414 
00415 
00416 void ShapeSetInfo_impl::setNormals(AppearanceInfo& appInfo, VrmlIndexedFaceSet* triangleMesh)
00417 {
00418     const MFVec3f& normals = triangleMesh->normal->vector;
00419     int numNormals = normals.size();
00420     appInfo.normals.length(numNormals * 3);
00421 
00422     int pos = 0;
00423     for(int i=0; i < numNormals; ++i){
00424         const SFVec3f& n = normals[i];
00425         for(int j=0; j < 3; ++j){
00426             appInfo.normals[pos++] = n[j];
00427         }
00428     }
00429 
00430     const MFInt32& orgIndices = triangleMesh->normalIndex;
00431     const int numOrgIndices = orgIndices.size();
00432     if(numOrgIndices > 0){
00433         if(triangleMesh->normalPerVertex){
00434             const int numTriangles = numOrgIndices / 4; // considering delimiter element '-1'
00435             appInfo.normalIndices.length(numTriangles * 3);
00436             int dpos = 0;
00437             int spos = 0;
00438             for(int i=0; i < numTriangles; ++i){
00439                 appInfo.normalIndices[dpos++] = orgIndices[spos++];
00440                 appInfo.normalIndices[dpos++] = orgIndices[spos++];
00441                 appInfo.normalIndices[dpos++] = orgIndices[spos++];
00442                 spos++; // skip delimiter '-1'
00443             }
00444         } else { // normal per face
00445             appInfo.normalIndices.length(numOrgIndices);
00446             for(int i=0; i < numOrgIndices; ++i){
00447                 appInfo.normalIndices[i] = orgIndices[i];
00448             }
00449         }
00450     }
00451 }
00452 
00453 void ShapeSetInfo_impl::setTexCoords(AppearanceInfo& appInfo, VrmlIndexedFaceSet* triangleMesh )
00454 {
00455         int numCoords = triangleMesh->texCoord->point.size();
00456     appInfo.textureCoordinate.length(numCoords * 2);
00457 
00458     Matrix33 m;
00459     for(int i=0,k=0; i<3; i++)
00460         for(int j=0; j<3; j++)
00461              m(i,j) = appInfo.textransformMatrix[k++];
00462     
00463     for(int i=0, pos=0; i < numCoords; i++ ){
00464         Vector3 point(triangleMesh->texCoord->point[i][0], triangleMesh->texCoord->point[i][1], 1);
00465         Vector3 texCoordinate(m * point);
00466         appInfo.textureCoordinate[pos++] = texCoordinate(0);
00467         appInfo.textureCoordinate[pos++] = texCoordinate(1);
00468     }
00469 
00470     int numIndex = triangleMesh->texCoordIndex.size();
00471     if(numIndex > 0){
00472         appInfo.textureCoordIndices.length(numIndex * 3 / 4);
00473          for(int i=0, j=0; i < numIndex; i++){
00474             if(triangleMesh->texCoordIndex[i] != -1)
00475                 appInfo.textureCoordIndices[j++] = triangleMesh->texCoordIndex[i];
00476         }
00477     }
00478 }
00479 
00480 void ShapeSetInfo_impl::createTextureTransformMatrix(AppearanceInfo& appInfo, VrmlTextureTransformPtr& textureTransform ){
00481 
00482     Matrix33 m;
00483     if(textureTransform){
00484         Matrix33 matCT;
00485         matCT << 1, 0, textureTransform->translation[0]+textureTransform->center[0],
00486                 0, 1, textureTransform->translation[1]+textureTransform->center[1],
00487                 0,0,1 ;
00488         Matrix33 matR;
00489         matR << cos(textureTransform->rotation), -sin(textureTransform->rotation), 0,
00490                sin(textureTransform->rotation), cos(textureTransform->rotation), 0,
00491                0,0,1;
00492         Matrix33 matCS;
00493         matCS << textureTransform->scale[0], 0, -textureTransform->center[0],
00494                 0, textureTransform->scale[1], -textureTransform->center[1],
00495                 0,0,1 ;
00496         
00497         m = matCS * matR * matCT;
00498     }else{
00499         m = Matrix33::Identity();
00500     }
00501  
00502     for(int i=0,k=0; i<3; i++)
00503         for(int j=0; j<3; j++)
00504             appInfo.textransformMatrix[k++] = m(i,j);
00505 }
00506 
00515 int ShapeSetInfo_impl::createMaterialInfo(VrmlMaterialPtr& materialNode)
00516 {
00517     int materialInfoIndex = -1;
00518 
00519     if(materialNode){
00520         MaterialInfo_var material(new MaterialInfo());
00521 
00522         material->ambientIntensity = materialNode->ambientIntensity;
00523         material->shininess = materialNode->shininess;
00524         material->transparency = materialNode->transparency;
00525 
00526         for(int j = 0 ; j < 3 ; j++){
00527             material->diffuseColor[j]  = materialNode->diffuseColor[j];
00528             material->emissiveColor[j] = materialNode->emissiveColor[j];
00529             material->specularColor[j] = materialNode->specularColor[j];
00530         }
00531 
00532         // materials_に追加する //
00533         materialInfoIndex = materials_.length();
00534         materials_.length(materialInfoIndex + 1 );
00535         materials_[materialInfoIndex] = material;
00536 
00537     }
00538 
00539     return materialInfoIndex;
00540 }
00541 
00542 
00552 int ShapeSetInfo_impl::createTextureInfo(VrmlTexturePtr& textureNode, const SFString *currentUrl)
00553 {
00554     int textureInfoIndex = -1;
00555 
00556     if(textureNode){
00557 
00558         TextureInfo_var texture(new TextureInfo());
00559        
00560         VrmlPixelTexturePtr pixelTextureNode = dynamic_pointer_cast<VrmlPixelTexture>(textureNode);
00561         
00562         if(!pixelTextureNode){
00563             VrmlImageTexturePtr imageTextureNode = dynamic_pointer_cast<VrmlImageTexture>(textureNode);
00564             if(imageTextureNode){
00565                 string url = setTexturefileUrl(getModelFileDirPath(*currentUrl), imageTextureNode->url);
00566                 texture->url = CORBA::string_dup(url.c_str());
00567                 texture->repeatS = imageTextureNode->repeatS;
00568                 texture->repeatT = imageTextureNode->repeatT;
00569                 if(readImage && !url.empty()){
00570                     ImageConverter  converter;
00571                     SFImage* image = converter.convert(url);          
00572                     texture->height = image->height;
00573                     texture->width = image->width;
00574                     texture->numComponents = image->numComponents;
00575                     unsigned long pixelsLength = image->pixels.size();
00576                     texture->image.length( pixelsLength );
00577                     for(unsigned long j = 0 ; j < pixelsLength ; j++ ){
00578                         texture->image[j] = image->pixels[j];
00579                     }
00580                 }else{
00581                     texture->height = 0;
00582                     texture->width = 0;
00583                     texture->numComponents = 0;
00584                 }
00585             }
00586         }else if(pixelTextureNode){
00587             texture->height = pixelTextureNode->image.height;
00588             texture->width = pixelTextureNode->image.width;
00589             texture->numComponents = pixelTextureNode->image.numComponents;
00590                 
00591             size_t pixelsLength =  pixelTextureNode->image.pixels.size();
00592             texture->image.length( pixelsLength );
00593             for(size_t j = 0 ; j < pixelsLength ; j++ ){
00594                 texture->image[j] = pixelTextureNode->image.pixels[j];
00595             }
00596             texture->repeatS = pixelTextureNode->repeatS;
00597             texture->repeatT = pixelTextureNode->repeatT;
00598         }
00599         
00600         textureInfoIndex = textures_.length();
00601         textures_.length(textureInfoIndex + 1);
00602         textures_[textureInfoIndex] = texture;
00603     }
00604 
00605     return textureInfoIndex;
00606 }
00607 
00608 
00616 std::string ShapeSetInfo_impl::getModelFileDirPath(const std::string& url)
00617 {
00618     //  BodyInfo::url_ から URLスキームを削除する //
00619     string filepath = deleteURLScheme(url);
00620 
00621     //  '/' または '\' の最後の位置を取得する //
00622     size_t pos = filepath.find_last_of("/\\");
00623 
00624     string dirPath = "";
00625 
00626     //  存在すれば, //
00627     if(pos != string::npos){
00628         //  ディレクトリパス文字列 //
00629         dirPath = filepath;
00630         dirPath.resize(pos + 1);
00631     }
00632 
00633     return dirPath;
00634 }
00635 
00636 void ShapeSetInfo_impl::setColdetModel(ColdetModelPtr& coldetModel, TransformedShapeIndexSequence shapeIndices, const Matrix44& Tparent, int& vertexIndex, int& triangleIndex){
00637     for(unsigned int i=0; i < shapeIndices.length(); i++){
00638         setColdetModelTriangles(coldetModel, shapeIndices[i], Tparent, vertexIndex, triangleIndex);
00639     }
00640 }
00641 
00642 void ShapeSetInfo_impl::setColdetModelTriangles
00643 (ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, int& vertexIndex, int& triangleIndex)
00644 {
00645     short shapeIndex = tsi.shapeIndex;
00646     const DblArray12& M = tsi.transformMatrix;;
00647     Matrix44 T, Tlocal;
00648     Tlocal << M[0], M[1], M[2],  M[3],
00649              M[4], M[5], M[6],  M[7],
00650              M[8], M[9], M[10], M[11],
00651              0.0,  0.0,  0.0,   1.0;
00652     T = Tparent * Tlocal;
00653     
00654     const ShapeInfo& shapeInfo = shapes_[shapeIndex];
00655     int vertexIndexBase = coldetModel->getNumVertices();
00656     const FloatSequence& vertices = shapeInfo.vertices;
00657     const int numVertices = vertices.length() / 3;
00658     coldetModel->setNumVertices(coldetModel->getNumVertices()+numVertices);
00659     for(int j=0; j < numVertices; ++j){
00660         Vector4 v(T * Vector4(vertices[j*3], vertices[j*3+1], vertices[j*3+2], 1.0));
00661         coldetModel->setVertex(vertexIndex++, v[0], v[1], v[2]);
00662     }
00663     const LongSequence& triangles = shapeInfo.triangles;
00664     const int numTriangles = triangles.length() / 3;
00665     coldetModel->setNumTriangles(coldetModel->getNumTriangles()+numTriangles);
00666     for(int j=0; j < numTriangles; ++j){
00667         int t0 = triangles[j*3] + vertexIndexBase;
00668         int t1 = triangles[j*3+1] + vertexIndexBase;
00669         int t2 = triangles[j*3+2] + vertexIndexBase;
00670         coldetModel->setTriangle( triangleIndex++, t0, t1, t2);
00671     }
00672 
00673 }
00674 
00675 void ShapeSetInfo_impl::saveOriginalData(){
00676     originShapes_ = shapes_;
00677     originAppearances_ = appearances_;
00678     originMaterials_ = materials_;
00679 }
00680 
00681 void ShapeSetInfo_impl::setBoundingBoxData(const Vector3& boxSize, int shapeIndex){
00682     VrmlBox* box = new VrmlBox();
00683     VrmlIndexedFaceSetPtr triangleMesh;
00684     box->size[0] = boxSize[0]*2;
00685     box->size[1] = boxSize[1]*2;
00686     box->size[2] = boxSize[2]*2;
00687     triangleMesh = new VrmlIndexedFaceSet();
00688     triangleMesh->coord = new VrmlCoordinate();
00689     triangleMeshShaper.convertBox(box, triangleMesh);
00690        
00691     shapes_.length(shapeIndex+1);
00692     ShapeInfo& shapeInfo = shapes_[shapeIndex];
00693     VrmlIndexedFaceSet* faceSet = triangleMesh.get();
00694     setTriangleMesh(shapeInfo, faceSet);
00695     shapeInfo.primitiveType = SP_BOX;
00696     FloatSequence& param = shapeInfo.primitiveParameters;
00697     param.length(3);
00698     for(int i=0; i < 3; ++i)
00699         param[i] = box->size[i];
00700     shapeInfo.appearanceIndex = 0;
00701     
00702     delete box;
00703 }
00704 
00705 void ShapeSetInfo_impl::createAppearanceInfo(){
00706     appearances_.length(1);
00707     AppearanceInfo& appInfo = appearances_[0];
00708     appInfo.normalPerVertex = false;
00709     appInfo.colorPerVertex = false;
00710     appInfo.solid = false;
00711     appInfo.creaseAngle = 0.0;
00712     appInfo.textureIndex = -1;
00713     appInfo.normals.length(0);
00714     appInfo.normalIndices.length(0);
00715     appInfo.colors.length(0);
00716     appInfo.colorIndices.length(0);
00717     appInfo.materialIndex = 0;
00718     appInfo.textureIndex = -1;
00719 
00720     materials_.length(1);
00721     MaterialInfo& material = materials_[0];
00722     material.ambientIntensity = 0.2;
00723     material.shininess = 0.2;
00724     material.transparency = 0.0;
00725     for(int j = 0 ; j < 3 ; j++){
00726         material.diffuseColor[j]  = 0.8;
00727         material.emissiveColor[j] = 0.0;
00728         material.specularColor[j] = 0.0;
00729     }
00730 }
00731 
00732 void ShapeSetInfo_impl::restoreOriginalData(){
00733     shapes_ = originShapes_;
00734     appearances_ = originAppearances_;
00735     materials_ = originMaterials_;
00736 }
00737 
00738 bool ShapeSetInfo_impl::checkFileUpdateTime(){
00739     bool ret=true;
00740     for( map<std::string, time_t>::iterator it=fileTimeMap.begin(); it != fileTimeMap.end(); ++it){
00741         struct stat statbuff;
00742         time_t mtime = 0;
00743         if( stat( it->first.c_str(), &statbuff ) == 0 ){
00744             mtime = statbuff.st_mtime;
00745         }  
00746         if(it->second!=mtime){
00747             ret=false;
00748             break;
00749         }
00750     }
00751     return ret;
00752 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:57