TriangleMeshShaper.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 "TriangleMeshShaper.h"
00016 #include "Triangulator.h"
00017 #include <iostream>
00018 #include <cmath>
00019 #include <vector>
00020 #include <map>
00021 #include <hrpUtil/Eigen3d.h>
00022 
00023 using namespace std;
00024 using namespace boost;
00025 using namespace hrp;
00026 
00027 namespace {
00028     const double PI = 3.14159265358979323846;
00029 }
00030 
00031 namespace hrp {
00032 
00033     class TMSImpl
00034     {
00035     public:
00036         TMSImpl(TriangleMeshShaper* self);
00037 
00038         TriangleMeshShaper* self;
00039 
00040         int divisionNumber;
00041         bool isNormalGenerationMode;
00042 
00043         typedef std::map<VrmlShapePtr, SFNode> ShapeToGeometryMap;
00044         ShapeToGeometryMap shapeToOriginalGeometryMap;
00045 
00046         // for triangulation
00047         Triangulator triangulator;
00048         std::vector<int> polygon;
00049         std::vector<int> indexPositionMap;
00050         std::vector<int> faceIndexMap;
00051 
00052         // for normal generation
00053         std::vector<Vector3> faceNormals;
00054         std::vector< std::vector<int> > vertexIndexToFaceIndicesMap;
00055         std::vector< std::vector<int> > vertexIndexToNormalIndicesMap;
00056 
00057         enum RemapType { REMAP_COLOR, REMAP_NORMAL };
00058 
00059 
00060         SFNode getOriginalGeometry(VrmlShapePtr shapeNode);
00061         bool traverseShapeNodes(VrmlNode* node, AbstractVrmlGroup* parentNode, int indexInParent);
00062         bool convertShapeNode(VrmlShape* shapeNode);
00063         bool convertIndexedFaceSet(VrmlIndexedFaceSet* faceSet);
00064 
00065         template <class TArray>
00066             bool remapDirectMapObjectsPerFaces(TArray& objects, const char* objectName);
00067         
00068         bool checkAndRemapIndices(RemapType type, unsigned int numElements, MFInt32& indices, bool perVertex,
00069                                   VrmlIndexedFaceSet* triangleMesh);
00070         void putError1(const char* valueName);
00071         
00072         bool convertBox(VrmlBox* box, VrmlIndexedFaceSetPtr& triangleMesh);
00073         bool convertCone(VrmlCone* cone, VrmlIndexedFaceSetPtr& triangleMesh);
00074         bool convertPointSet(VrmlPointSet* pointSet, VrmlIndexedFaceSetPtr& triangleMesh);
00075         bool convertCylinder(VrmlCylinder* cylinder, VrmlIndexedFaceSetPtr& triangleMesh);
00076         bool convertSphere(VrmlSphere* sphere, VrmlIndexedFaceSetPtr& triangleMesh);
00077         bool convertElevationGrid(VrmlElevationGrid* grid, VrmlIndexedFaceSetPtr& triangleMesh);
00078         bool convertExtrusion(VrmlExtrusion* extrusion, VrmlIndexedFaceSetPtr& triangleMesh);
00079         void generateNormals(VrmlIndexedFaceSetPtr& triangleMesh);
00080         void calculateFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh);
00081         void setVertexNormals(VrmlIndexedFaceSetPtr& triangleMesh);
00082         void setFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh);
00083         bool setTexCoordIndex(VrmlIndexedFaceSetPtr faseSet );
00084 
00085         void putMessage(const std::string& message);
00086     };
00087 }
00088 
00089 
00090 TriangleMeshShaper::TriangleMeshShaper()
00091 {
00092     impl = new TMSImpl(this);
00093 }
00094 
00095 
00096 TMSImpl::TMSImpl(TriangleMeshShaper* self) : self(self)
00097 {
00098     divisionNumber = 20;
00099     isNormalGenerationMode = true;
00100 }
00101 
00102 
00103 TriangleMeshShaper::~TriangleMeshShaper()
00104 {
00105     delete impl;
00106 }
00107 
00108 
00116 void TriangleMeshShaper::setDivisionNumber(int n)
00117 {
00118     impl->divisionNumber = std::max(4, n);
00119 }
00120 
00121 
00127 void TriangleMeshShaper::setNormalGenerationMode(bool on)
00128 {
00129     impl->isNormalGenerationMode = on;
00130 }
00131 
00132 
00140 SFNode TriangleMeshShaper::getOriginalGeometry(VrmlShapePtr shapeNode)
00141 {
00142     return impl->getOriginalGeometry(shapeNode);
00143 }
00144 
00145 
00146 SFNode TMSImpl::getOriginalGeometry(VrmlShapePtr shapeNode)
00147 {
00148     SFNode originalGeometryNode;
00149     ShapeToGeometryMap::iterator p = shapeToOriginalGeometryMap.find(shapeNode);
00150     if(p != shapeToOriginalGeometryMap.end()){
00151         originalGeometryNode = p->second;
00152     }
00153     return originalGeometryNode;
00154 }
00155 
00156 
00169 VrmlNodePtr TriangleMeshShaper::apply(VrmlNodePtr topNode)
00170 {
00171     bool resultOfTopNode = impl->traverseShapeNodes(topNode.get(), 0, 0);
00172     return resultOfTopNode ? topNode : VrmlNodePtr();
00173 }
00174 
00175 
00176 bool TMSImpl::traverseShapeNodes(VrmlNode* node, AbstractVrmlGroup* parentNode, int indexInParent)
00177 {
00178     bool result = true;
00179 
00180     if(node->isCategoryOf(PROTO_INSTANCE_NODE)){
00181         VrmlProtoInstance* protoInstance = static_cast<VrmlProtoInstance*>(node);
00182         if(protoInstance->actualNode){
00183             traverseShapeNodes(protoInstance->actualNode.get(), parentNode, indexInParent);
00184         }
00185 
00186     } else if(node->isCategoryOf(GROUPING_NODE)){
00187         AbstractVrmlGroup* group = static_cast<AbstractVrmlGroup*>(node);
00188         int numChildren = group->countChildren();
00189         for(int i = 0; i < numChildren; i++){
00190             traverseShapeNodes(group->getChild(i), group, i);
00191         }
00192 
00193     } else if(node->isCategoryOf(SHAPE_NODE)){
00194         VrmlShape* shapeNode = static_cast<VrmlShape*>(node);
00195         result = convertShapeNode(shapeNode);
00196         if(!result){
00197             if(parentNode){
00198                 putMessage("Node is inconvertible and removed from the scene graph");
00199                 parentNode->removeChild(indexInParent);
00200             }
00201         }
00202     }
00203 
00204     return result;
00205 }
00206 
00207 
00208 bool TMSImpl::convertShapeNode(VrmlShape* shapeNode)
00209 {
00210     bool result = false;
00211 
00212     VrmlNode *node = shapeNode->geometry.get();
00213     VrmlGeometry* geometry = dynamic_cast<VrmlGeometry *>(node);
00214     if (!geometry){
00215         VrmlProtoInstance *protoInstance = dynamic_cast<VrmlProtoInstance *>(node);
00216         if (protoInstance){
00217             geometry = dynamic_cast<VrmlGeometry *>(protoInstance->actualNode.get());
00218         }
00219     }
00220     
00221     VrmlIndexedFaceSetPtr triangleMesh;
00222 
00223     if(VrmlIndexedFaceSet* faceSet = dynamic_cast<VrmlIndexedFaceSet*>(geometry)){
00224         if(faceSet->coord){
00225             result = convertIndexedFaceSet(faceSet);
00226             triangleMesh = faceSet;
00227         }
00228 
00229     } else {
00230     
00231         triangleMesh = new VrmlIndexedFaceSet();
00232         triangleMesh->coord = new VrmlCoordinate();
00233         
00234         if(VrmlBox* box = dynamic_cast<VrmlBox*>(geometry)){
00235             result = convertBox(box, triangleMesh);
00236             
00237         } else if(VrmlCone* cone = dynamic_cast<VrmlCone*>(geometry)){
00238             result = convertCone(cone, triangleMesh);
00239             
00240         } else if(VrmlCylinder* cylinder = dynamic_cast<VrmlCylinder*>(geometry)){
00241             result = convertCylinder(cylinder, triangleMesh);
00242             
00243         } else if(VrmlSphere* sphere = dynamic_cast<VrmlSphere*>(geometry)){
00244             result = convertSphere(sphere, triangleMesh);
00245             
00246         } else if(VrmlElevationGrid* elevationGrid = dynamic_cast<VrmlElevationGrid*>(geometry)){
00247             result = convertElevationGrid(elevationGrid, triangleMesh);
00248             
00249         } else if(VrmlExtrusion* extrusion = dynamic_cast<VrmlExtrusion*>(geometry)){
00250             result = convertExtrusion(extrusion, triangleMesh);
00251         } else if(VrmlPointSet* pointSet = dynamic_cast<VrmlPointSet*>(geometry)){
00252             result = convertPointSet(pointSet, triangleMesh);
00253         }
00254         if(result){
00255             shapeToOriginalGeometryMap[shapeNode] = node;
00256             shapeNode->geometry = triangleMesh;
00257         }
00258     }
00259     
00260     if(result && !triangleMesh->normal && isNormalGenerationMode){
00261         generateNormals(triangleMesh);
00262     }
00263 
00264     return result;
00265 }
00266 
00267 
00268 bool TMSImpl::convertIndexedFaceSet(VrmlIndexedFaceSet* faceSet)
00269 {
00270     MFVec3f& vertices = faceSet->coord->point;
00271     int numVertices = vertices.size();
00272 
00273     MFInt32& indices = faceSet->coordIndex;
00274     const MFInt32 orgIndices = indices;
00275     indices.clear();
00276 
00277     const int numOrgIndices = orgIndices.size();
00278 
00279     int orgFaceIndex = 0;
00280     int polygonTopIndexPosition = 0;
00281 
00282     indexPositionMap.clear();
00283     faceIndexMap.clear();
00284     polygon.clear();
00285 
00286     int triangleOrder[3];
00287     if(faceSet->ccw){
00288         triangleOrder[0] = 0; triangleOrder[1] = 1; triangleOrder[2] = 2;
00289     } else {
00290         triangleOrder[0] = 2; triangleOrder[1] = 1; triangleOrder[2] = 0;
00291     }
00292     
00293     triangulator.setVertices(vertices);
00294     
00295     for(int i=0; i < numOrgIndices; ++i){
00296         int index = orgIndices[i];
00297         if(index >= numVertices){
00298             putMessage("The coordIndex field has an index over the size of the vertices in the coord field");
00299         } else if(index >= 0){
00300             polygon.push_back(index);
00301         } else {
00302             int numTriangles = triangulator.apply(polygon);
00303             const vector<int>& triangles = triangulator.triangles();
00304             for(int j=0; j < numTriangles; ++j){
00305                 for(int k=0; k < 3; ++k){
00306                     int localIndex = triangles[j * 3 + triangleOrder[k]];
00307                     indices.push_back(polygon[localIndex]);
00308                     indexPositionMap.push_back(polygonTopIndexPosition + localIndex);
00309                 }
00310                 indices.push_back(-1);
00311                 indexPositionMap.push_back(-1);
00312                 faceIndexMap.push_back(orgFaceIndex);
00313             }
00314             polygonTopIndexPosition = i + 1;
00315             orgFaceIndex++;
00316             polygon.clear();
00317         }
00318     }
00319 
00320     bool result = true;
00321 
00322     int numColors = faceSet->color ? faceSet->color->color.size() : 0;
00323     result &= checkAndRemapIndices
00324         (REMAP_COLOR, numColors, faceSet->colorIndex, faceSet->colorPerVertex, faceSet);
00325 
00326     int numNormals = faceSet->normal ? faceSet->normal->vector.size() : 0;
00327     result &= checkAndRemapIndices
00328         (REMAP_NORMAL, numNormals, faceSet->normalIndex, faceSet->normalPerVertex, faceSet);
00329 
00330     if(numNormals > 0 && !faceSet->ccw){
00331         // flip normal vectors
00332          MFVec3f& normals = faceSet->normal->vector;
00333          for(unsigned int i=0; i < normals.size(); ++i){
00334              SFVec3f& n = normals[i];
00335              n[0] = -n[0];
00336              n[1] = -n[1];
00337              n[2] = -n[2];
00338          }
00339     }
00340     faceSet->ccw = true;
00341 
00342     setTexCoordIndex(faceSet);
00343  
00344     return (result && !indices.empty());
00345 }
00346 
00347 
00348 template <class TArray>
00349 bool TMSImpl::remapDirectMapObjectsPerFaces(TArray& values, const char* valueName)
00350 {
00351     const TArray orgValues = values;
00352     int numOrgValues = orgValues.size();
00353     int numFaces = faceIndexMap.size();
00354     values.resize(numFaces);
00355     for(int i=0; i < numFaces; ++i){
00356         int faceIndex = faceIndexMap[i];
00357         if(faceIndex >= numOrgValues){
00358             putMessage(string("The number of ") + valueName + " is less than the number of faces.");
00359             return false;
00360         }
00361         values[i] = orgValues[faceIndex];
00362     }
00363     return true;
00364 }
00365 
00366 
00367 bool TMSImpl::checkAndRemapIndices
00368 (RemapType type, unsigned int numElements, MFInt32& indices, bool perVertex, VrmlIndexedFaceSet* triangleMesh)
00369 {
00370     const char* valueName = (type==REMAP_COLOR) ? "colors" : "normals" ;
00371     
00372     bool result = true;
00373     
00374     if(numElements == 0){
00375         if(!indices.empty()){
00376             putMessage(string("An IndexedFaceSet has no ") + valueName +
00377                        ", but it has a non-empty index field of " + valueName + ".");
00378             result = false;
00379         }
00380 
00381     } else {
00382 
00383         if(indices.empty()){
00384             if(perVertex){
00385                 if(numElements < triangleMesh->coord->point.size()){
00386                     putMessage(string("The number of ") + valueName +
00387                               " is less than the number of vertices.");
00388                     result = false;
00389                 }
00390             } else {
00391                 if(type == REMAP_COLOR){
00392                     remapDirectMapObjectsPerFaces(triangleMesh->color->color, valueName);
00393                 } else if(type == REMAP_NORMAL){
00394                     remapDirectMapObjectsPerFaces(triangleMesh->normal->vector, valueName);
00395                 }
00396             }
00397         } else {
00398             const MFInt32 orgIndices = indices;
00399 
00400             if(perVertex){
00401                 int numNewIndices = indexPositionMap.size();
00402                 indices.resize(numNewIndices);
00403                 for(int i=0; i < numNewIndices; ++i){
00404                     int orgPosition = indexPositionMap[i];
00405                     if(orgPosition < 0){
00406                         indices[i] = -1;
00407                     } else {
00408                         int index = orgIndices[orgPosition];
00409                         if(index < (int)numElements){
00410                             indices[i] = index;
00411                         } else {
00412                             putError1(valueName);
00413                             result = false;
00414                         }
00415                     }
00416                 }
00417             } else {
00418                 int numNewIndices = faceIndexMap.size();
00419                 indices.resize(numNewIndices);
00420                 for(int i=0; i < numNewIndices; ++i){
00421                     int orgFaceIndex = faceIndexMap[i];
00422                     int index = orgIndices[orgFaceIndex];
00423                     if(index < (int)numElements){
00424                         indices[i] = index;
00425                     } else {
00426                         putError1(valueName);
00427                         result = false;
00428                     }
00429                 }
00430             }
00431         }
00432     }
00433 
00434     return result;
00435 }
00436 
00437 
00438 bool TMSImpl::setTexCoordIndex(VrmlIndexedFaceSetPtr faseSet)
00439 {
00440     bool result = true;
00441     VrmlTextureCoordinatePtr texCoord = faseSet->texCoord;
00442     MFInt32& texCoordIndex = faseSet->texCoordIndex;
00443     MFInt32& coordIndex = faseSet->coordIndex;
00444 
00445         if(texCoord){
00446         if(texCoordIndex.empty()){   
00447             texCoordIndex.resize(coordIndex.size());
00448             copy( coordIndex.begin(), coordIndex.end(), texCoordIndex.begin() );
00449         } else {
00450             const MFInt32 orgIndices = texCoordIndex;
00451             int numNewIndices = indexPositionMap.size();
00452             texCoordIndex.resize(numNewIndices);
00453             for(int i=0; i < numNewIndices; ++i){
00454                 if(indexPositionMap[i] == -1){
00455                     texCoordIndex[i] = -1;
00456                 } else {
00457                     int index = orgIndices[indexPositionMap[i]];
00458                     if(index < (int)texCoord->point.size()){
00459                         texCoordIndex[i] = index;
00460                     } else {
00461                         putError1("texCoordIndex");
00462                         result = false;
00463                     } 
00464                 }
00465             }
00466         }
00467     }
00468     return result;
00469 }
00470 
00471 
00472 void TMSImpl::putError1(const char* valueName)
00473 {
00474     putMessage(string("There is an index of ") + valueName +
00475                " beyond the size of " + valueName + ".");
00476 }
00477 
00478 
00479 namespace {
00480 
00481     inline int addVertex(MFVec3f& vertices, const double x, const double y, const double z)
00482     {
00483         SFVec3f v;
00484         v[0] = x;
00485         v[1] = y;
00486         v[2] = z;
00487         vertices.push_back(v);
00488         return vertices.size() - 1;
00489     }
00490 
00491     inline void addTriangle(MFInt32& indices, int x, int y, int z)
00492     {
00493         indices.push_back(x);
00494         indices.push_back(y);
00495         indices.push_back(z);
00496         indices.push_back(-1);
00497     }
00498 }
00499 
00500 
00501 bool TMSImpl::convertBox(VrmlBox* box, VrmlIndexedFaceSetPtr& triangleMesh)
00502 {
00503     const double x = box->size[0] / 2.0;
00504     const double y = box->size[1] / 2.0;
00505     const double z = box->size[2] / 2.0;
00506     
00507     if(x < 0.0 || y < 0.0 || z < 0.0 ){
00508         putMessage("BOX : wrong value.");
00509         return false;
00510     }
00511 
00512     MFVec3f& vertices = triangleMesh->coord->point;
00513     vertices.reserve(8);
00514 
00515     static const int numTriangles = 12;
00516     
00517     static const double xsigns[] = { -1.0, -1.0, -1.0, -1.0,  1.0,  1.0,  1.0,  1.0 };
00518     static const double ysigns[] = { -1.0, -1.0,  1.0,  1.0, -1.0, -1.0,  1.0,  1.0 };
00519     static const double zsigns[] = { -1.0,  1.0,  1.0, -1.0, -1.0,  1.0,  1.0, -1.0 };
00520 
00521     static const int triangles[] = {
00522         0, 1, 2,
00523         2, 3, 0,
00524         3, 2, 6,
00525         3, 6, 7,
00526         6, 2, 1,
00527         1, 5, 6,
00528         1, 0, 5,
00529         5, 0, 4,
00530         5, 4, 6,
00531         6, 4, 7,
00532         7, 4, 0,
00533         0, 3, 7
00534     };
00535 
00536     for(int i=0; i < 8; ++i){
00537         addVertex(vertices, xsigns[i] * x, ysigns[i] * y, zsigns[i] * z);
00538     }
00539     
00540     MFInt32& indices = triangleMesh->coordIndex;
00541     indices.resize(numTriangles * 4);
00542 
00543     int di = 0;
00544     int si = 0;
00545     for(int i=0; i < numTriangles; i++){
00546         indices[di++] = triangles[si++];
00547         indices[di++] = triangles[si++];
00548         indices[di++] = triangles[si++];
00549         indices[di++] = -1;
00550     }
00551 
00552     return true;
00553 }
00554 
00555 
00556 bool TMSImpl::convertPointSet(VrmlPointSet* pointSet, VrmlIndexedFaceSetPtr& triangleMesh)
00557 {
00558     triangleMesh->coord = pointSet->coord;
00559     triangleMesh->color = pointSet->color;
00560 
00561     return true;
00562 }
00563 
00564 
00565 bool TMSImpl::convertCone(VrmlCone* cone, VrmlIndexedFaceSetPtr& triangleMesh)
00566 {
00567     const double radius = cone->bottomRadius;
00568     
00569     if(cone->height < 0.0 || radius < 0.0 ){
00570         putMessage( "CONE : wrong value." );
00571         return false;
00572     }
00573 
00574     MFVec3f& vertices = triangleMesh->coord->point;
00575     vertices.reserve(divisionNumber + 1);
00576 
00577     for(int i=0;  i < divisionNumber; ++i){
00578         const double angle = i * 2.0 * PI / divisionNumber;
00579         addVertex(vertices, radius * cos(angle), -cone->height/2.0, radius * sin(angle));
00580     }
00581 
00582     const int topIndex = addVertex(vertices, 0.0, cone->height/2.0, 0.0);
00583     const int bottomCenterIndex = addVertex(vertices, 0.0, -cone->height/2.0, 0.0);
00584 
00585     MFInt32& indices = triangleMesh->coordIndex;
00586     indices.reserve((divisionNumber * 2) * 4);
00587 
00588     for(int i=0; i < divisionNumber; ++i){
00589         // side faces
00590         if(cone->side)
00591             addTriangle(indices, topIndex, (i + 1) % divisionNumber, i);
00592         // bottom faces
00593         if(cone->bottom)       
00594             addTriangle(indices, bottomCenterIndex, i, (i + 1) % divisionNumber);
00595     }
00596 
00597     triangleMesh->creaseAngle = 3.14 / 2.0;
00598 
00599     return true;
00600 }
00601 
00602 
00603 bool TMSImpl::convertCylinder(VrmlCylinder* cylinder, VrmlIndexedFaceSetPtr& triangleMesh)
00604 {
00605     if(cylinder->height < 0.0 || cylinder->radius < 0.0){
00606         putMessage("CYLINDER : wrong value.");
00607         return false;
00608     }
00609 
00610     MFVec3f& vertices = triangleMesh->coord->point;
00611     vertices.reserve(divisionNumber * 2 + 2);
00612     vertices.resize(divisionNumber * 2);
00613 
00614     const double y = cylinder->height / 2.0;
00615 
00616     for(int i=0 ; i < divisionNumber ; i++ ){
00617         const double angle = i * 2.0 * PI / divisionNumber;
00618         SFVec3f& vtop = vertices[i];
00619         SFVec3f& vbottom = vertices[i + divisionNumber];
00620         vtop[0] = vbottom[0] = cylinder->radius * cos(angle);
00621         vtop[2] = vbottom[2] = cylinder->radius * sin(angle);
00622         vtop[1]    =  y;
00623         vbottom[1] = -y;
00624     }
00625 
00626     const int topCenterIndex    = addVertex(vertices, 0.0,  y, 0.0);
00627     const int bottomCenterIndex = addVertex(vertices, 0.0, -y, 0.0);
00628 
00629     MFInt32& indices = triangleMesh->coordIndex;
00630     indices.reserve((divisionNumber * 4) * 4);
00631 
00632     for(int i=0; i < divisionNumber; ++i){
00633         // top face
00634         if(cylinder->top)
00635             addTriangle(indices, topCenterIndex, (i+1) % divisionNumber, i);
00636         // side face (upward convex triangle)
00637         if(cylinder->side){        
00638             addTriangle(indices, i, ((i+1) % divisionNumber) + divisionNumber, i + divisionNumber);
00639             // side face (downward convex triangle)
00640             addTriangle(indices, i, (i+1) % divisionNumber, ((i + 1) % divisionNumber) + divisionNumber);
00641         }
00642         // bottom face
00643         if(cylinder->bottom)
00644             addTriangle(indices, bottomCenterIndex, i + divisionNumber, ((i+1) % divisionNumber) + divisionNumber);
00645     }
00646 
00647     triangleMesh->creaseAngle = 3.14 / 2.0;
00648 
00649     return true;
00650 }
00651 
00652 
00653 bool TMSImpl::convertSphere(VrmlSphere* sphere, VrmlIndexedFaceSetPtr& triangleMesh)
00654 {
00655     const double r = sphere->radius;
00656 
00657     if(r < 0.0) {
00658         putMessage("SPHERE : wrong value.");
00659         return false;
00660     }
00661 
00662     const int vdn = divisionNumber / 2;  // latitudinal division number
00663     const int hdn = divisionNumber;      // longitudinal division number
00664     
00665     MFVec3f& vertices = triangleMesh->coord->point;
00666     vertices.reserve((vdn - 1) * hdn + 2);
00667 
00668     for(int i=1; i < vdn; i++){ // latitudinal direction
00669         double tv = i * PI / vdn;
00670         for(int j=0; j < hdn; j++){ // longitudinal direction
00671             double th = j * 2.0 * PI / hdn;
00672             addVertex(vertices, r*sin(tv)*cos(th), r*cos(tv), r*sin(tv)*sin(th));
00673         }
00674     }
00675     
00676     const int topIndex    = addVertex(vertices, 0.0,  r, 0.0);
00677     const int bottomIndex = addVertex(vertices, 0.0, -r, 0.0);
00678 
00679     MFInt32& indices = triangleMesh->coordIndex;
00680     indices.reserve(vdn * hdn * 2 * 4);
00681 
00682     // top faces
00683     for(int i=0; i < hdn; ++i){
00684         addTriangle(indices, topIndex, (i+1) % hdn, i);
00685     }
00686 
00687     // side faces
00688     for(int i=0; i < vdn - 2; ++i){
00689         const int upper = i * hdn;
00690         const int lower = (i + 1) * hdn;
00691         for(int j=0; j < hdn; ++j) {
00692             // upward convex triangle
00693             addTriangle(indices, j + upper, ((j + 1) % hdn) + lower, j + lower);
00694             // downward convex triangle
00695             addTriangle(indices, j + upper, ((j + 1) % hdn) + upper, ((j + 1) % hdn) + lower);
00696         }
00697     }
00698     
00699     // bottom faces
00700     const int offset = (vdn - 2) * hdn;
00701     for(int i=0; i < hdn; ++i){
00702         addTriangle(indices, bottomIndex, (i % hdn) + offset, ((i+1) % hdn) + offset);
00703     }
00704 
00705     triangleMesh->creaseAngle = PI;
00706 
00707     return true;
00708 }
00709 
00710 
00714 bool TMSImpl::convertElevationGrid(VrmlElevationGrid* grid, VrmlIndexedFaceSetPtr& triangleMesh)
00715 {
00716     if(grid->xDimension * grid->zDimension != static_cast<SFInt32>(grid->height.size())){
00717         putMessage("ELEVATIONGRID : wrong value.");
00718         return false;
00719     }
00720 
00721     MFVec3f& vertices = triangleMesh->coord->point;
00722     vertices.reserve(grid->zDimension * grid->xDimension);
00723 
00724     for(int z=0; z < grid->zDimension; z++){
00725         for(int x=0; x < grid->xDimension; x++ ){
00726             addVertex(vertices, x * grid->xSpacing, grid->height[z * grid->xDimension + x], z * grid->zSpacing);
00727         }
00728     }
00729 
00730     MFInt32& indices = triangleMesh->coordIndex;
00731     indices.reserve((grid->zDimension - 1) * (grid->xDimension - 1) * 2 * 4);
00732 
00733     for(int z=0; z < grid->zDimension - 1; ++z){
00734         const int current = z * grid->xDimension;
00735         const int next = (z + 1) * grid->xDimension;
00736         for(int x=0; x < grid->xDimension - 1; ++x){
00737             if(grid->ccw){
00738                 addTriangle(indices, x + current, x + next, (x + 1) + next);
00739                 addTriangle(indices, x + current, (x + 1) + next, (x + 1) + current);
00740             }else{
00741                 addTriangle(indices, x + current, (x + 1) + next, x + next);
00742                 addTriangle(indices, x + current, (x + 1) + current, (x + 1) + next);
00743             }
00744         }
00745     }
00746 
00747     triangleMesh->creaseAngle = grid->creaseAngle;
00748     triangleMesh->ccw = true;
00749     //triangleMesh->normalPerVertex = grid->normalPerVertex;
00750     triangleMesh->solid = grid->solid;
00751 
00752     if(grid->texCoord){
00753         triangleMesh->texCoord->point.resize(grid->texCoord->point.size());
00754         copy(grid->texCoord->point.begin(), grid->texCoord->point.end(), triangleMesh->texCoord->point.begin());
00755         triangleMesh->texCoordIndex.resize(indices.size());
00756         copy(indices.begin(), indices.end(), triangleMesh->texCoordIndex.begin());
00757     }
00758 
00759     return true;
00760 }
00761 
00762 
00763 bool TMSImpl::convertExtrusion(VrmlExtrusion* extrusion, VrmlIndexedFaceSetPtr& triangleMesh)
00764 {
00765     bool isClosed = false;
00766     int numSpine = extrusion->spine.size();
00767     int numcross = extrusion->crossSection.size();
00768     if( extrusion->spine[0][0] == extrusion->spine[numSpine-1][0] &&
00769         extrusion->spine[0][1] == extrusion->spine[numSpine-1][1] &&
00770         extrusion->spine[0][2] == extrusion->spine[numSpine-1][2] )
00771         isClosed = true;
00772     bool crossSectionisClosed = false;
00773     if( extrusion->crossSection[0][0] == extrusion->crossSection[numcross-1][0] &&
00774         extrusion->crossSection[0][1] == extrusion->crossSection[numcross-1][1] )
00775         crossSectionisClosed = true;
00776 
00777     MFVec3f& vertices = triangleMesh->coord->point;
00778     vertices.reserve(numSpine*numcross);
00779 
00780     Vector3 preZaxis;
00781     int definedZaxis=-1;
00782     std::vector<Vector3> Yaxisarray;
00783     std::vector<Vector3> Zaxisarray;
00784     if(numSpine > 2){
00785         for(int i=0; i<numSpine; i++){
00786             Vector3 spine1, spine2, spine3;
00787             Vector3 Yaxis, Zaxis;
00788             if(i==0){
00789                 if(isClosed){
00790                     getVector3(spine1, extrusion->spine[numSpine-2]);
00791                     getVector3(spine2, extrusion->spine[0]);
00792                     getVector3(spine3, extrusion->spine[1]);
00793                     Yaxis = spine3-spine1;
00794                     Zaxis = (spine3-spine2).cross(spine1-spine2);
00795                 }else{
00796                     getVector3(spine1, extrusion->spine[0]);
00797                     getVector3(spine2, extrusion->spine[1]);
00798                     getVector3(spine3, extrusion->spine[2]);
00799                     Yaxis = spine2-spine1;
00800                     Zaxis = (spine3-spine2).cross(spine1-spine2);
00801                 }
00802             }else if(i==numSpine-1){
00803                 if(isClosed){
00804                     getVector3(spine1, extrusion->spine[numSpine-2]);
00805                     getVector3(spine2, extrusion->spine[0]);
00806                     getVector3(spine3, extrusion->spine[1]);
00807                     Yaxis = spine3-spine1;
00808                     Zaxis = (spine3-spine2).cross(spine1-spine2);
00809                 }else{
00810                     getVector3(spine1, extrusion->spine[numSpine-3]);
00811                     getVector3(spine2, extrusion->spine[numSpine-2]);
00812                     getVector3(spine3, extrusion->spine[numSpine-1]);
00813                     Yaxis = spine3-spine2;
00814                     Zaxis = (spine3-spine2).cross(spine1-spine2);
00815                 }
00816             }else{
00817                 getVector3(spine1, extrusion->spine[i-1]);
00818                 getVector3(spine2, extrusion->spine[i]);
00819                 getVector3(spine3, extrusion->spine[i+1]);
00820                 Yaxis = spine3-spine1;
00821                 Zaxis = (spine3-spine2).cross(spine1-spine2);
00822             }
00823             if(!Zaxis.norm()){
00824                 if(definedZaxis!=-1)
00825                     Zaxis=preZaxis;
00826             }else{
00827                 if(definedZaxis==-1)
00828                     definedZaxis=i;
00829                 preZaxis = Zaxis;
00830             }
00831             Yaxisarray.push_back(Yaxis);
00832             Zaxisarray.push_back(Zaxis);
00833         }
00834     }else{
00835         Vector3 spine1, spine2;
00836         Vector3 Yaxis;
00837         getVector3(spine1, extrusion->spine[0]);
00838         getVector3(spine2, extrusion->spine[1]);
00839         Yaxis = spine2-spine1;
00840         Yaxisarray.push_back(Yaxis);
00841         Yaxisarray.push_back(Yaxis);
00842     }
00843     for(int i=0; i<numSpine; i++){
00844         Matrix33 Scp;
00845         if(definedZaxis==-1){
00846             Vector3 y(Yaxisarray[i].normalized());
00847             SFRotation R;
00848             // R[0,1,2] = y X (0,1,0) cos(R[3]) = y.(0,1,0)
00849             R[0] = y[2]; R[1] = 0.0; R[2] = -y[0]; R[3] = acos(y[1]);
00850             Scp = rodrigues(Vector3(R[0],R[1],R[2]), R[3]);
00851         }else{
00852             if(i<definedZaxis)
00853                 Zaxisarray[i] = Zaxisarray[definedZaxis];
00854             if( i && Zaxisarray[i].dot(Zaxisarray[i-1])<0 )
00855                 Zaxisarray[i] *= -1;
00856             Vector3 y(Yaxisarray[i].normalized());
00857             Vector3 z(Zaxisarray[i].normalized());
00858             Vector3 x(y.cross(z));
00859             Scp << x, y, z;
00860         }
00861 
00862         Vector3 spine(extrusion->spine[i][0],
00863                       extrusion->spine[i][1],
00864                       extrusion->spine[i][2]);
00865                       
00866         Vector3 scale;
00867         if(extrusion->scale.size()==1)
00868             scale = Vector3(extrusion->scale[0][0], 0, extrusion->scale[0][1]);
00869         else
00870             scale = Vector3(extrusion->scale[i][0], 0, extrusion->scale[i][1]);
00871         Matrix33 orientation;
00872         if(extrusion->orientation.size()==1)
00873             orientation = rodrigues(Vector3(extrusion->orientation[0][0],extrusion->orientation[0][1],extrusion->orientation[0][2]),
00874                 extrusion->orientation[0][3]);
00875         else
00876             orientation = rodrigues(Vector3(extrusion->orientation[i][0],extrusion->orientation[i][1],extrusion->orientation[i][2]),
00877                 extrusion->orientation[i][3]);
00878 
00879         for(int j=0; j<numcross; j++){
00880             Vector3 crossSection(extrusion->crossSection[j][0], 0, extrusion->crossSection[j][1] );
00881             Vector3 v1(crossSection[0]*scale[0], 0, crossSection[2]*scale[2]);
00882             Vector3 v2(Scp*orientation*v1+spine); 
00883             addVertex(vertices,v2[0], v2[1], v2[2]);
00884         }
00885     }
00886 
00887     MFInt32& indices = triangleMesh->coordIndex;
00888     for(int i=0; i < numSpine-1 ; i++){
00889         const int upper = i * numcross;
00890         const int lower = (i + 1) * numcross;
00891         for(int j=0; j < numcross-1; ++j) {
00892             if(extrusion->ccw){
00893                 // upward convex triangle
00894                 addTriangle(indices, j + upper, (j + 1)+ lower, j + lower);
00895                 // downward convex triangle
00896                 addTriangle(indices, j + upper, (j + 1)+ upper, j + 1 + lower);
00897             }else{
00898                 addTriangle(indices, j + upper, j + lower, (j + 1) + lower);
00899                 addTriangle(indices, j + upper, (j + 1)+ lower, j + 1 + upper);
00900             }
00901         }
00902     }
00903 
00904     int j=0;
00905     if(crossSectionisClosed)
00906         j=1;
00907     if(extrusion->beginCap && !isClosed){
00908         triangulator.setVertices(vertices);
00909         polygon.clear();
00910         for(int i=0; i<numcross-j; i++)
00911             polygon.push_back(i);
00912         triangulator.apply(polygon);
00913         const vector<int>& triangles = triangulator.triangles();
00914         for(unsigned int i=0; i<triangles.size(); i+=3 )
00915             if(extrusion->ccw){
00916                 addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+2]], polygon[triangles[i+1]]);
00917             }else{
00918                 addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+1]], polygon[triangles[i+2]]);
00919             }
00920     }
00921 
00922     if(extrusion->endCap && !isClosed){
00923         triangulator.setVertices(vertices);
00924         polygon.clear();
00925         for(int i=0; i<numcross-j; i++)
00926             polygon.push_back(numcross*(numSpine-1)+i);
00927         triangulator.apply(polygon);
00928         const vector<int>& triangles = triangulator.triangles();
00929         for(unsigned int i=0; i<triangles.size(); i+=3 )
00930             if(extrusion->ccw){
00931                 addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+1]], polygon[triangles[i+2]]);
00932             }else{
00933                 addTriangle(indices, polygon[triangles[i]], polygon[triangles[i+2]], polygon[triangles[i+1]]);
00934             }
00935     }
00936 
00937     triangleMesh->creaseAngle = extrusion->creaseAngle;
00938     triangleMesh->ccw = true;
00939     triangleMesh->convex = true;
00940     triangleMesh->solid = extrusion->solid;
00941 
00942     return true;
00943 }
00944 
00945 
00946 void TMSImpl::generateNormals(VrmlIndexedFaceSetPtr& triangleMesh)
00947 {
00948     triangleMesh->normal = new VrmlNormal();
00949     triangleMesh->normalPerVertex = (triangleMesh->creaseAngle > 0.0) ? true : false;
00950 
00951     calculateFaceNormals(triangleMesh);
00952 
00953     if(triangleMesh->normalPerVertex){
00954         setVertexNormals(triangleMesh);
00955     } else {
00956         setFaceNormals(triangleMesh);
00957     }
00958 }
00959 
00960 
00961 void TMSImpl::calculateFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh)
00962 {
00963     const MFVec3f& vertices = triangleMesh->coord->point;
00964     const int numVertices = vertices.size();
00965     const MFInt32& triangles = triangleMesh->coordIndex;
00966     const int numFaces = triangles.size() / 4;
00967 
00968     faceNormals.clear();
00969 
00970     if(triangleMesh->normalPerVertex){
00971         vertexIndexToFaceIndicesMap.clear();
00972         vertexIndexToFaceIndicesMap.resize(numVertices);
00973     }
00974 
00975     for(int faceIndex=0; faceIndex < numFaces; ++faceIndex){
00976 
00977         Vector3Ref v0(getVector3Ref(vertices[triangles[faceIndex * 4 + 0]].data()));
00978         Vector3Ref v1(getVector3Ref(vertices[triangles[faceIndex * 4 + 1]].data()));
00979         Vector3Ref v2(getVector3Ref(vertices[triangles[faceIndex * 4 + 2]].data()));
00980         Vector3 normal((v1 - v0).cross(v2 - v0));
00981         if ( ! normal.isZero() ) {
00982                 normal.normalize();
00983         }
00984         faceNormals.push_back(normal);
00985 
00986         if(triangleMesh->normalPerVertex){
00987             for(int i=0; i < 3; ++i){
00988                 int vertexIndex = triangles[faceIndex * 4 + i];
00989                 vector<int>& facesOfVertex = vertexIndexToFaceIndicesMap[vertexIndex];
00990                 bool isSameNormalFaceFound = false;
00991                 for(unsigned int j=0; j < facesOfVertex.size(); ++j){
00992                     const Vector3& otherNormal = faceNormals[facesOfVertex[j]];
00993                     const Vector3 d(otherNormal - normal);
00994                     // the same face is not appended
00995                     if(d.dot(d) <= numeric_limits<double>::epsilon()){
00996                         isSameNormalFaceFound = true;
00997                         break;
00998                     }
00999                 }
01000                 if(!isSameNormalFaceFound){
01001                     facesOfVertex.push_back(faceIndex);
01002                 }
01003             }
01004         }
01005     }
01006 }
01007 
01008 
01009 void TMSImpl::setVertexNormals(VrmlIndexedFaceSetPtr& triangleMesh)
01010 {
01011     const MFVec3f& vertices = triangleMesh->coord->point;
01012     const int numVertices = vertices.size();
01013     const MFInt32& triangles = triangleMesh->coordIndex;
01014     const int numFaces = triangles.size() / 4;
01015 
01016     MFVec3f& normals = triangleMesh->normal->vector;
01017     MFInt32& normalIndices = triangleMesh->normalIndex;
01018     normalIndices.clear();
01019     normalIndices.reserve(triangles.size());
01020 
01021     vertexIndexToNormalIndicesMap.clear();
01022     vertexIndexToNormalIndicesMap.resize(numVertices);
01023 
01024     //const double cosCreaseAngle = cos(triangleMesh->creaseAngle);
01025 
01026     for(int faceIndex=0; faceIndex < numFaces; ++faceIndex){
01027 
01028         for(int i=0; i < 3; ++i){
01029 
01030             int vertexIndex = triangles[faceIndex * 4 + i];
01031             vector<int>& facesOfVertex = vertexIndexToFaceIndicesMap[vertexIndex];
01032             const Vector3& currentFaceNormal = faceNormals[faceIndex];
01033             Vector3 normal = currentFaceNormal;
01034             bool normalIsFaceNormal = true;
01035 
01036             // avarage normals of the faces whose crease angle is below the 'creaseAngle' variable
01037             for(unsigned int j=0; j < facesOfVertex.size(); ++j){
01038                 int adjoiningFaceIndex = facesOfVertex[j];
01039                 const Vector3& adjoiningFaceNormal = faceNormals[adjoiningFaceIndex];
01040                 double currentFaceNormalLen = currentFaceNormal.norm();
01041                 if (currentFaceNormalLen == 0) continue;
01042                 double adjoiningFaceNormalLen = adjoiningFaceNormal.norm();
01043                 if (adjoiningFaceNormalLen == 0) continue;
01044                 double cosAngle = currentFaceNormal.dot(adjoiningFaceNormal)
01045                   / (currentFaceNormalLen * adjoiningFaceNormalLen);
01046                 if (cosAngle >  1.0) cosAngle =  1.0;
01047                 if (cosAngle < -1.0) cosAngle = -1.0;
01048                 double angle = acos(cosAngle);
01049                 if(angle > 1.0e-6 && angle < triangleMesh->creaseAngle){
01050                     normal += adjoiningFaceNormal;
01051                     normalIsFaceNormal = false;
01052                 }
01053                 
01054             }
01055             if(!normalIsFaceNormal){
01056                 normal.normalize();
01057             }
01058 
01059             int normalIndex = -1;
01060 
01061             for(int j=0; j < 3; ++j){
01062                 int vertexIndex2 = triangles[faceIndex * 4 + j];
01063                 vector<int>& normalIndicesOfVertex = vertexIndexToNormalIndicesMap[vertexIndex2];
01064                 for(unsigned int k=0; k < normalIndicesOfVertex.size(); ++k){
01065                     int index = normalIndicesOfVertex[k];
01066                     const SFVec3f& norg = normals[index];
01067                     const Vector3 d(Vector3(norg[0], norg[1], norg[2]) - normal);
01068                     if(d.dot(d) <= numeric_limits<double>::epsilon()){
01069                         normalIndex = index;
01070                         goto normalIndexFound;
01071                     }
01072                 }
01073             }
01074             if(normalIndex < 0){
01075                 SFVec3f n;
01076                 n[0] = normal[0]; n[1] = normal[1]; n[2] = normal[2]; 
01077                 normalIndex = normals.size();
01078                 normals.push_back(n);
01079                 vertexIndexToNormalIndicesMap[vertexIndex].push_back(normalIndex);
01080             }
01081             
01082           normalIndexFound:
01083             normalIndices.push_back(normalIndex);
01084         }
01085         normalIndices.push_back(-1);
01086     }
01087 }
01088 
01089 
01090 void TMSImpl::setFaceNormals(VrmlIndexedFaceSetPtr& triangleMesh)
01091 {
01092     const MFInt32& triangles = triangleMesh->coordIndex;
01093     const int numFaces = triangles.size() / 4;
01094 
01095     MFVec3f& normals = triangleMesh->normal->vector;
01096     MFInt32& normalIndices = triangleMesh->normalIndex;
01097     normalIndices.clear();
01098     normalIndices.reserve(numFaces);
01099 
01100     const int numVertices = triangleMesh->coord->point.size();
01101     vertexIndexToNormalIndicesMap.clear();
01102     vertexIndexToNormalIndicesMap.resize(numVertices);
01103 
01104     for(int faceIndex=0; faceIndex < numFaces; ++faceIndex){
01105 
01106         const Vector3& normal = faceNormals[faceIndex];
01107         int normalIndex = -1;
01108 
01109         // find the same normal from the existing normals
01110         for(int i=0; i < 3; ++i){
01111             int vertexIndex = triangles[faceIndex * 4 + i];
01112             vector<int>& normalIndicesOfVertex = vertexIndexToNormalIndicesMap[vertexIndex];
01113             for(unsigned int j=0; j < normalIndicesOfVertex.size(); ++j){
01114                 int index = normalIndicesOfVertex[j];
01115                 const SFVec3f& norg = normals[index];
01116                 const Vector3 n(norg[0], norg[1], norg[2]);
01117                 if((n - normal).norm() <= numeric_limits<double>::epsilon()){
01118                     normalIndex = index;
01119                     goto normalIndexFound2;
01120                 }
01121             }
01122         }
01123         if(normalIndex < 0){
01124             SFVec3f n;
01125             n[0] = normal[0]; n[1] = normal[1]; n[2] = normal[2]; 
01126             normalIndex = normals.size();
01127             normals.push_back(n);
01128             for(int i=0; i < 3; ++i){
01129                 int vertexIndex = triangles[faceIndex * 4 + i];
01130                 vertexIndexToNormalIndicesMap[vertexIndex].push_back(normalIndex);
01131             }
01132         }
01133       normalIndexFound2:
01134         normalIndices.push_back(normalIndex);
01135     }
01136 }
01137     
01138 
01139 void TMSImpl::putMessage(const std::string& message)
01140 {
01141     if(!self->sigMessage.empty()){
01142         self->sigMessage(message + "\n" );
01143     }
01144 }
01145 
01146 
01147 void TriangleMeshShaper::defaultTextureMapping(VrmlShape* shapeNode)
01148 {
01149     VrmlIndexedFaceSet* triangleMesh = dynamic_cast<VrmlIndexedFaceSet*>(shapeNode->geometry.get());
01150     if(!triangleMesh)
01151         return;
01152     VrmlNode* node = getOriginalGeometry(shapeNode).get();
01153     VrmlGeometry *originalGeometry = dynamic_cast<VrmlGeometry *>(node);
01154     if (!originalGeometry){
01155         VrmlProtoInstance *protoInstance = dynamic_cast<VrmlProtoInstance *>(node);
01156         if (protoInstance){
01157             originalGeometry = dynamic_cast<VrmlGeometry *>(protoInstance->actualNode.get());
01158         }
01159     }
01160     if(originalGeometry){
01161         if(dynamic_cast<VrmlBox*>(originalGeometry)){    //Box
01162             defaultTextureMappingBox(triangleMesh);
01163         }else if(dynamic_cast<VrmlCone*>(originalGeometry)){  //cone
01164             defaultTextureMappingCone(triangleMesh);
01165         }else if(dynamic_cast<VrmlCylinder*>(originalGeometry)){   //Cylinder
01166             defaultTextureMappingCylinder(triangleMesh);
01167         }else if(VrmlSphere* sphere = dynamic_cast<VrmlSphere*>(originalGeometry)){     //sphere
01168             defaultTextureMappingSphere(triangleMesh, sphere->radius);
01169         }else if(VrmlElevationGrid* grid = dynamic_cast<VrmlElevationGrid*>(originalGeometry)){     //ElevationGrid
01170             defaultTextureMappingElevationGrid(grid, triangleMesh);
01171         }else if(VrmlExtrusion* extrusion = dynamic_cast<VrmlExtrusion*>(originalGeometry)){     //Extrusion
01172             defaultTextureMappingExtrusion(triangleMesh, extrusion);
01173         }
01174     }else{      //IndexedFaceSet
01175         defaultTextureMappingFaceSet(triangleMesh);
01176     }
01177 }
01178 
01179 
01180 void TriangleMeshShaper::defaultTextureMappingFaceSet(VrmlIndexedFaceSet* triangleMesh)
01181 {
01182     if(!triangleMesh->texCoord){
01183         float max[3]={static_cast<float> (triangleMesh->coord->point[0][0]),
01184                       static_cast<float> (triangleMesh->coord->point[0][1]),
01185                       static_cast<float> (triangleMesh->coord->point[0][2])};
01186         float min[3]={static_cast<float> (triangleMesh->coord->point[0][0]),
01187                       static_cast<float> (triangleMesh->coord->point[0][1]),
01188                       static_cast<float> (triangleMesh->coord->point[0][2])};
01189         int n = triangleMesh->coord->point.size();
01190         for(int i=1; i<n; i++){
01191             for(int j=0; j<3; j++){
01192                 float w = triangleMesh->coord->point[i][j];
01193                 max[j] = std::max( max[j], w );
01194                 min[j] = std::min( min[j], w );
01195             }
01196         }
01197         float size[3]={0,0,0};
01198         for(int j=0; j<3; j++)
01199             size[j] = max[j]-min[j];
01200         int s,t;
01201         size[0] >= size[1] ? 
01202               ( size[0] >= size[2] ? 
01203                       ( s=0 , t=size[1] >= size[2] ? 1 : 2 ) 
01204                     : ( s=2 , t=0) ) 
01205             : ( size[1] >= size[2] ? 
01206                       ( s=1 , t=size[0] >= size[2] ? 0 : 2 )
01207                     : ( s=2 , t=1) ) ;
01208         triangleMesh->texCoord = new VrmlTextureCoordinate();
01209         double ratio = size[t]/size[s];
01210         for(int i=0; i<n; i++){
01211             SFVec2f point;
01212             point[0] = (triangleMesh->coord->point[i][s]-min[s])/size[s];
01213             point[1] = (triangleMesh->coord->point[i][t]-min[t])/size[t]*ratio;
01214             triangleMesh->texCoord->point.push_back(point);
01215         }
01216         triangleMesh->texCoordIndex.resize(triangleMesh->coordIndex.size());
01217         copy( triangleMesh->coordIndex.begin(), triangleMesh->coordIndex.end(), 
01218                         triangleMesh->texCoordIndex.begin() );
01219     }   
01220 }
01221 
01222 
01223 void TriangleMeshShaper::defaultTextureMappingElevationGrid(VrmlElevationGrid* grid, VrmlIndexedFaceSet* triangleMesh)
01224 {
01225     float xmax = grid->xSpacing * (grid->xDimension-1);
01226     float zmax = grid->zSpacing * (grid->zDimension-1);
01227     triangleMesh->texCoord = new VrmlTextureCoordinate();
01228     for(unsigned int i=0; i<triangleMesh->coord->point.size(); i++){
01229        SFVec2f point;
01230        point[0] = (triangleMesh->coord->point[i][0])/xmax;
01231        point[1] = (triangleMesh->coord->point[i][2])/zmax;
01232        triangleMesh->texCoord->point.push_back(point);
01233     }
01234     triangleMesh->texCoordIndex.resize(triangleMesh->coordIndex.size());
01235     copy( triangleMesh->coordIndex.begin(), triangleMesh->coordIndex.end(), 
01236                 triangleMesh->texCoordIndex.begin() );
01237 }
01238 
01239 
01240 int TriangleMeshShaper::faceofBox(SFVec3f* point)
01241 {
01242     if(point[0][0] <= 0 && point[1][0] <= 0 && point[2][0] <= 0 ) return LEFT;
01243     if(point[0][0] > 0 && point[1][0] > 0 && point[2][0] > 0 ) return RIGHT;
01244     if(point[0][1] <= 0 && point[1][1] <= 0 && point[2][1] <= 0 ) return BOTTOM;
01245     if(point[0][1] > 0 && point[1][1] > 0 && point[2][1] > 0 ) return TOP;
01246     if(point[0][2] <= 0 && point[1][2] <= 0 && point[2][2] <= 0 ) return BACK;
01247     if(point[0][2] > 0 && point[1][2] > 0 && point[2][2] > 0 ) return FRONT;
01248     return -1;
01249 }
01250 
01251 
01252 int TriangleMeshShaper::findPoint(MFVec2f& points, SFVec2f& target)
01253 {
01254     for(unsigned int i=0; i<points.size(); i++){
01255         if((points[i][0]-target[0])*(points[i][0]-target[0]) +
01256             (points[i][1]-target[1])*(points[i][1]-target[1]) < 1.0e-9 )
01257             return i;
01258     }
01259     return -1;
01260 }
01261 
01262 
01263 double TriangleMeshShaper::calcangle(SFVec3f& point)
01264 {
01265     double angle = atan2( point[2], point[0] );
01266     if(angle>=0) angle=1.5*PI-angle;
01267     else if(-0.5*PI<angle) angle=-angle+1.5*PI;
01268     else angle=-angle-0.5*PI;
01269     return angle;
01270 }
01271 
01272 
01273 void TriangleMeshShaper::defaultTextureMappingBox(VrmlIndexedFaceSet* triangleMesh)
01274 {
01275     triangleMesh->texCoord = new VrmlTextureCoordinate();
01276     SFVec2f point ;
01277     point[0] = 0.0; point[1] = 0.0;     //index=0
01278     triangleMesh->texCoord->point.push_back(point);
01279     point[0] = 1.0; point[1] = 0.0;     //index=1
01280     triangleMesh->texCoord->point.push_back(point);
01281     point[0] = 0.0; point[1] = 1.0;     //index=2
01282     triangleMesh->texCoord->point.push_back(point);
01283     point[0] = 1.0; point[1] = 1.0;     //index=3
01284     triangleMesh->texCoord->point.push_back(point);
01285     
01286     for(int i=0; i<12; i++){
01287         SFVec3f point[3];
01288         for(int j=0; j<3; j++)
01289             point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i*4+j]];
01290         switch(faceofBox(point)){
01291             case LEFT:
01292                 for(int j=0; j<3; j++){
01293                     if(point[j][1] > 0 && point[j][2] > 0 ) triangleMesh->texCoordIndex.push_back(3);
01294                     else if(point[j][1] > 0 && point[j][2] <= 0 ) triangleMesh->texCoordIndex.push_back(2);
01295                     else if(point[j][1] <= 0 && point[j][2] > 0 ) triangleMesh->texCoordIndex.push_back(1);
01296                     else if(point[j][1] <= 0 && point[j][2] <= 0 ) triangleMesh->texCoordIndex.push_back(0);
01297                 }
01298                 break;
01299             case RIGHT:
01300                 for(int j=0; j<3; j++){
01301                     if(point[j][1] > 0 && point[j][2] > 0 ) triangleMesh->texCoordIndex.push_back(2);
01302                     else if(point[j][1] > 0 && point[j][2] <= 0 ) triangleMesh->texCoordIndex.push_back(3);
01303                     else if(point[j][1] <= 0 && point[j][2] > 0 ) triangleMesh->texCoordIndex.push_back(0);
01304                     else if(point[j][1] <= 0 && point[j][2] <= 0 ) triangleMesh->texCoordIndex.push_back(1);
01305                 }
01306                 break;
01307             case BOTTOM:
01308                 for(int j=0; j<3; j++){
01309                     if(point[j][2] > 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(3);
01310                     else if(point[j][2] > 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(2);
01311                     else if(point[j][2] <= 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(1);
01312                     else if(point[j][2] <= 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(0);
01313                 }
01314                 break;
01315             case TOP:
01316                 for(int j=0; j<3; j++){
01317                     if(point[j][2] > 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(1);
01318                     else if(point[j][2] > 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(0);
01319                     else if(point[j][2] <= 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(3);
01320                     else if(point[j][2] <= 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(2);
01321                 }
01322                 break;
01323             case BACK:
01324                 for(int j=0; j<3; j++){
01325                     if(point[j][1] > 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(2);
01326                     else if(point[j][1] > 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(3);
01327                     else if(point[j][1] <= 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(0);
01328                     else if(point[j][1] <= 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(1);
01329                 }
01330                 break;
01331             case FRONT:
01332                  for(int j=0; j<3; j++){
01333                     if(point[j][1] > 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(3);
01334                     else if(point[j][1] > 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(2);
01335                     else if(point[j][1] <= 0 && point[j][0] > 0 ) triangleMesh->texCoordIndex.push_back(1);
01336                     else if(point[j][1] <= 0 && point[j][0] <= 0 ) triangleMesh->texCoordIndex.push_back(0);
01337                 }
01338                 break;
01339             default:
01340                 break;
01341         }
01342         triangleMesh->texCoordIndex.push_back(-1);
01343     }
01344 }
01345 
01346 
01347 void TriangleMeshShaper::defaultTextureMappingCone(VrmlIndexedFaceSet* triangleMesh)
01348 {
01349     triangleMesh->texCoord = new VrmlTextureCoordinate();
01350     SFVec2f texPoint ;
01351     texPoint[0] = 0.5; texPoint[1] = 0.5;     //center of bottom index=0
01352     triangleMesh->texCoord->point.push_back(texPoint);
01353     int texIndex = 1;
01354     for(unsigned int i=0; i<triangleMesh->coordIndex.size(); i++){
01355         SFVec3f point[3];
01356         int top=-1;
01357         int center=-1;
01358         for(int j=0; j<3; j++){
01359             point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i++]];
01360             if(point[j][1] > 0) top = j;
01361             if(point[j][0] == 0.0 && point[j][2] == 0.0) center = j;
01362         }
01363         if(top>=0){         //side
01364             double s[3]={0,0,0};
01365             int pre=-1;
01366             for(int j=0; j<3; j++){
01367                 if(j!=top){
01368                     double angle = calcangle(point[j]);
01369                     s[j] = angle/2/PI;     
01370                     if(pre!=-1)
01371                         if(s[pre] > 0.5 && s[j] < 1.0e-6)
01372                             s[j] = 1.0;
01373                     pre = j;
01374 
01375                 }
01376             }
01377             for(int j=0; j<3; j++){
01378                 if(j!=top){
01379                     texPoint[0] = s[j];        
01380                     texPoint[1] = 0.0;
01381                 }else{
01382                     texPoint[0] = (s[0]+s[1]+s[2])/2.0;        
01383                     texPoint[1] = 1.0;
01384                 }
01385                 int k=findPoint(triangleMesh->texCoord->point, texPoint);
01386                 if(k!=-1){
01387                     triangleMesh->texCoordIndex.push_back(k);
01388                 }else{
01389                     triangleMesh->texCoord->point.push_back(texPoint);
01390                     triangleMesh->texCoordIndex.push_back(texIndex++);
01391                 }
01392             }
01393             triangleMesh->texCoordIndex.push_back(-1);
01394         }else{              // bottom
01395             for(int j=0; j<3; j++){
01396                 if(j!=center){
01397                     double angle = atan2( point[j][2], point[j][0] );
01398                     texPoint[0] = 0.5 + 0.5*cos(angle);        
01399                     texPoint[1] = 0.5 + 0.5*sin(angle);
01400                     int k=findPoint(triangleMesh->texCoord->point, texPoint);
01401                     if(k!=-1){
01402                          triangleMesh->texCoordIndex.push_back(k);
01403                     }else{
01404                          triangleMesh->texCoord->point.push_back(texPoint);
01405                          triangleMesh->texCoordIndex.push_back(texIndex++);
01406                     }
01407                 }else{
01408                     triangleMesh->texCoordIndex.push_back(0);
01409                 }
01410             }
01411             triangleMesh->texCoordIndex.push_back(-1);
01412         }
01413     }
01414 }
01415 
01416 
01417 void TriangleMeshShaper::defaultTextureMappingCylinder(VrmlIndexedFaceSet* triangleMesh)
01418 {
01419     triangleMesh->texCoord = new VrmlTextureCoordinate();
01420     SFVec2f texPoint ;
01421     texPoint[0] = 0.5; texPoint[1] = 0.5;     //center of top(bottom) index=0
01422     triangleMesh->texCoord->point.push_back(texPoint);
01423     int texIndex = 1;
01424     for(unsigned int i=0; i<triangleMesh->coordIndex.size(); i++){
01425         SFVec3f point[3];
01426         bool notside=true;
01427         int center=-1;
01428         for(int j=0; j<3; j++){
01429             point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i++]];
01430             if(j){
01431                 if(point[0][1] == point[j][1] ) notside &= true;
01432                 else   notside &= false;
01433             }
01434             if(point[j][0] == 0.0 && point[j][2] == 0.0) center = j;
01435         }
01436         if(!notside){         //side
01437             bool over=false;
01438             double s[3]={0,0,0};
01439             for(int j=0; j<3; j++){
01440                 double angle = calcangle(point[j]);
01441                 s[j] = angle/2/PI;
01442                 if(s[j] > 0.5)
01443                     over = true;
01444             }
01445             for(int j=0; j<3; j++){
01446                 if(over && s[j]<1.0e-6)
01447                     s[j] = 1.0;
01448                 texPoint[0] = s[j];        
01449                 if(point[j][1] > 0) texPoint[1] = 1.0;
01450                 else    texPoint[1] = 0.0;
01451                 int k=findPoint(triangleMesh->texCoord->point, texPoint);
01452                 if(k!=-1){
01453                     triangleMesh->texCoordIndex.push_back(k);
01454                 }else{
01455                     triangleMesh->texCoord->point.push_back(texPoint);
01456                     triangleMesh->texCoordIndex.push_back(texIndex++);
01457                 }
01458             }
01459             triangleMesh->texCoordIndex.push_back(-1);
01460         }else{              // top / bottom
01461             for(int j=0; j<3; j++){
01462                 if(j!=center){
01463                     double angle = atan2( point[j][2], point[j][0] );
01464                     texPoint[0] = 0.5 + 0.5*cos(angle);    
01465                     if(point[0][1] > 0)  //top
01466                         texPoint[1] = 0.5 - 0.5*sin(angle);
01467                     else                //bottom
01468                         texPoint[1] = 0.5 + 0.5*sin(angle);   
01469                     int k=findPoint(triangleMesh->texCoord->point, texPoint);
01470                     if(k!=-1){
01471                         triangleMesh->texCoordIndex.push_back(k);
01472                     }else{
01473                         triangleMesh->texCoord->point.push_back(texPoint);
01474                         triangleMesh->texCoordIndex.push_back(texIndex++);
01475                     }
01476                 }else{
01477                     triangleMesh->texCoordIndex.push_back(0);
01478                 }
01479             }
01480             triangleMesh->texCoordIndex.push_back(-1);
01481         }
01482     }
01483 }
01484 
01485 
01486 void TriangleMeshShaper::defaultTextureMappingSphere(VrmlIndexedFaceSet* triangleMesh, double radius)
01487 {
01488     triangleMesh->texCoord = new VrmlTextureCoordinate();
01489     SFVec2f texPoint ;
01490     int texIndex = 0;
01491     for(unsigned int i=0; i<triangleMesh->coordIndex.size(); i++){
01492         SFVec3f point[3];
01493         bool over=false;
01494         double s[3]={0,0,0};
01495         for(int j=0; j<3; j++){
01496             point[j] = triangleMesh->coord->point[triangleMesh->coordIndex[i++]];
01497             double angle = calcangle(point[j]);
01498             s[j] = angle/2/PI; 
01499             if(s[j] > 0.5)
01500                 over = true;
01501         }
01502         for(int j=0; j<3; j++){
01503             if(over && s[j]<1.0e-6)
01504                 s[j] = 1.0;
01505             texPoint[0] = s[j];
01506             double theta = acos(point[j][1]/radius);        
01507             texPoint[1] = 1.0-theta/PI;
01508             int k=findPoint(triangleMesh->texCoord->point, texPoint);
01509             if(k!=-1){
01510                 triangleMesh->texCoordIndex.push_back(k);
01511             }else{
01512                 triangleMesh->texCoord->point.push_back(texPoint);
01513                 triangleMesh->texCoordIndex.push_back(texIndex++);
01514             }
01515         }
01516         triangleMesh->texCoordIndex.push_back(-1);
01517     }
01518 }
01519 
01520 
01521 void TriangleMeshShaper::defaultTextureMappingExtrusion(VrmlIndexedFaceSet* triangleMesh, VrmlExtrusion* extrusion )
01522 {
01523     int numSpine = extrusion->spine.size();
01524     int numcross = extrusion->crossSection.size();
01525         
01526     triangleMesh->texCoord = new VrmlTextureCoordinate();
01527     std::vector<double> s;
01528     std::vector<double> t;
01529     double slen=0;
01530     s.push_back(0);
01531     for(unsigned int i=1; i<extrusion->crossSection.size(); i++){
01532         double x=extrusion->crossSection[i][0]-extrusion->crossSection[i-1][0];
01533         double z=extrusion->crossSection[i][1]-extrusion->crossSection[i-1][1];
01534         slen += sqrt(x*x+z*z);
01535         s.push_back(slen);
01536     }
01537     double tlen=0;
01538     t.push_back(0);
01539     for(unsigned int i=1; i<extrusion->spine.size(); i++){
01540         double x=extrusion->spine[i][0]-extrusion->spine[i-1][0];
01541         double y=extrusion->spine[i][1]-extrusion->spine[i-1][1];
01542         double z=extrusion->spine[i][2]-extrusion->spine[i-1][2];
01543         tlen += sqrt(x*x+y*y+z*z);
01544         t.push_back(tlen);
01545     }
01546     for(unsigned int i=0; i<extrusion->spine.size(); i++){
01547         SFVec2f point;
01548         point[1] = t[i]/tlen;
01549         for(unsigned int j=0; j<extrusion->crossSection.size(); j++){
01550             point[0] = s[j]/slen;
01551             triangleMesh->texCoord->point.push_back(point);
01552         }
01553     }
01554 
01555     int endofspin = (numSpine-1)*(numcross-1)*2*4;
01556     triangleMesh->texCoordIndex.resize(endofspin);
01557     copy( triangleMesh->coordIndex.begin(), triangleMesh->coordIndex.begin()+endofspin, 
01558                 triangleMesh->texCoordIndex.begin() );
01559     int endofbegincap = endofspin;
01560     int endofpoint = triangleMesh->texCoord->point.size();
01561 
01562     if(extrusion->beginCap){
01563         if(extrusion->endCap)
01564             endofbegincap += (triangleMesh->coordIndex.size()-endofspin)/2;
01565         else
01566             endofbegincap = triangleMesh->coordIndex.size();
01567         double xmin, xmax;
01568         double zmin, zmax;
01569         xmin = xmax = extrusion->crossSection[0][0];
01570         zmin = zmax = extrusion->crossSection[0][1];
01571         for(unsigned int i=1; i<extrusion->crossSection.size(); i++){
01572             xmax = std::max(xmax,extrusion->crossSection[i][0]);
01573             xmin = std::min(xmin,extrusion->crossSection[i][0]);
01574             zmax = std::max(zmax,extrusion->crossSection[i][1]);
01575             zmin = std::min(xmin,extrusion->crossSection[i][1]);
01576         }
01577         double xsize = xmax-xmin;
01578         double zsize = zmax-zmin;
01579         for(int i=0; i<numcross; i++){
01580             SFVec2f point;
01581             point[0] = (extrusion->crossSection[i][0]-xmin)/xsize;
01582             point[1] = (extrusion->crossSection[i][1]-zmin)/zsize;
01583             triangleMesh->texCoord->point.push_back(point);
01584         }
01585         for(int i=endofspin; i<endofbegincap; i++){
01586             int k=triangleMesh->coordIndex[i];
01587             if(k != -1)
01588                 triangleMesh->texCoordIndex.push_back(k+endofpoint);
01589             else
01590                 triangleMesh->texCoordIndex.push_back(-1);
01591         }
01592     }
01593 
01594     if(extrusion->endCap){
01595         double xmax,xmin;
01596         double zmax,zmin;
01597         xmin = xmax = extrusion->crossSection[0][0];
01598         zmin = zmax = extrusion->crossSection[0][1];
01599         for(unsigned int i=1; i<extrusion->crossSection.size(); i++){
01600             xmax = std::max(xmax,extrusion->crossSection[i][0]);
01601             xmin = std::min(xmin,extrusion->crossSection[i][0]);
01602             zmax = std::max(zmax,extrusion->crossSection[i][1]);
01603             zmin = std::min(xmin,extrusion->crossSection[i][1]);
01604         }
01605         double xsize = xmax-xmin;
01606         double zsize = zmax-zmin;
01607         for(unsigned int i=0; i<extrusion->crossSection.size(); i++){
01608             SFVec2f point;
01609             point[0] = (extrusion->crossSection[i][0]-xmin)/xsize;
01610             point[1] = (extrusion->crossSection[i][1]-zmin)/zsize;
01611             triangleMesh->texCoord->point.push_back(point);
01612         }
01613         for(unsigned int i=endofbegincap; i<triangleMesh->coordIndex.size(); i++){
01614             int k=triangleMesh->coordIndex[i];
01615             if(k!=-1)   
01616                 triangleMesh->texCoordIndex.push_back(triangleMesh->texCoord->point.size()+k-endofpoint);
01617             else
01618                 triangleMesh->texCoordIndex.push_back(-1);
01619         }
01620     }
01621 }
01622 
01623  bool TriangleMeshShaper::convertBox(VrmlBox* box, VrmlIndexedFaceSetPtr& triangleMesh){
01624     return impl->convertBox(box, triangleMesh);
01625 }


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