shape_operations.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #include "geometric_shapes/shape_operations.h"
00038 
00039 #include <cstdio>
00040 #include <cmath>
00041 #include <algorithm>
00042 #include <set>
00043 
00044 #include <ros/console.h>
00045 #include <resource_retriever/retriever.h>
00046 
00047 #if defined(IS_ASSIMP3)
00048 #include <assimp/scene.h>
00049 #include <assimp/Importer.hpp>
00050 #include <assimp/postprocess.h>
00051 #else
00052 #include <assimp/aiScene.h>
00053 #include <assimp/assimp.hpp>
00054 #include <assimp/aiPostProcess.h>
00055 #endif
00056 
00057 namespace shapes
00058 {
00059   
00060 Shape* cloneShape(const Shape *shape)
00061 {
00062   Shape *result = NULL;
00063   if (shape) {
00064     switch (shape->type)
00065     {
00066     case SPHERE:
00067       result = new Sphere(static_cast<const Sphere*>(shape)->radius);
00068       break;
00069     case CYLINDER:
00070       result = new Cylinder(static_cast<const Cylinder*>(shape)->radius, static_cast<const Cylinder*>(shape)->length);
00071       break;
00072     case BOX:
00073       result = new Box(static_cast<const Box*>(shape)->size[0], static_cast<const Box*>(shape)->size[1], static_cast<const Box*>(shape)->size[2]);
00074       break;
00075     case MESH:
00076       {
00077         const Mesh *src = static_cast<const Mesh*>(shape);
00078         Mesh *dest = new Mesh(src->vertexCount, src->triangleCount);
00079         unsigned int n = 3 * src->vertexCount;
00080         for (unsigned int i = 0 ; i < n ; ++i)
00081           dest->vertices[i] = src->vertices[i];
00082         n = 3 * src->triangleCount;
00083         for (unsigned int i = 0 ; i < n ; ++i)
00084         {
00085           dest->triangles[i] = src->triangles[i];
00086           dest->normals[i] = src->normals[i];
00087         }
00088         result = dest;
00089       }
00090       break;
00091     default:
00092       break;
00093     }
00094   }
00095   return result;
00096 }
00097 
00098 std::vector<Shape*> cloneShapeVector(const std::vector<Shape*>& shapes)
00099 {
00100   std::vector<Shape*> ret;
00101   for(unsigned int i = 0; i < shapes.size(); i++) {
00102     ret.push_back(cloneShape(shapes[i]));
00103   }
00104   return ret;
00105 }
00106 
00107 void deleteShapeVector(std::vector<Shape*>& shapes) {
00108   for(unsigned int i = 0; i < shapes.size(); i++) {
00109     delete shapes[i];
00110   }
00111   shapes.clear();
00112 }
00113     
00114 StaticShape* cloneShape(const StaticShape *shape)
00115 {
00116   StaticShape *result = NULL;
00117   if (shape) {
00118     switch (shape->type)
00119     {
00120     case PLANE:
00121       result = new Plane(static_cast<const Plane*>(shape)->a, static_cast<const Plane*>(shape)->b, 
00122                          static_cast<const Plane*>(shape)->c, static_cast<const Plane*>(shape)->d);
00123       break;
00124     default:
00125       break;
00126     }
00127   }
00128         
00129   return result;
00130 }
00131     
00132     
00133 namespace detail
00134 {
00135 struct myVertex
00136 {
00137   tf::Vector3    point;
00138   unsigned int index;
00139 };
00140         
00141 struct ltVertexValue
00142 {
00143   bool operator()(const myVertex &p1, const myVertex &p2) const
00144   {
00145     const tf::Vector3 &v1 = p1.point;
00146     const tf::Vector3 &v2 = p2.point;
00147     if (v1.getX() < v2.getX())
00148       return true;
00149     if (v1.getX() > v2.getX())
00150       return false;
00151     if (v1.getY() < v2.getY())
00152       return true;
00153     if (v1.getY() > v2.getY())
00154       return false;
00155     if (v1.getZ() < v2.getZ())
00156       return true;
00157     return false;
00158   }
00159 };
00160         
00161 struct ltVertexIndex
00162 {
00163   bool operator()(const myVertex &p1, const myVertex &p2) const
00164   {
00165     return p1.index < p2.index;
00166   }
00167 };
00168 
00169 }
00170     
00171 shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &vertices, const std::vector<unsigned int> &triangles)
00172 {
00173   unsigned int nt = triangles.size() / 3;
00174   shapes::Mesh *mesh = new shapes::Mesh(vertices.size(), nt);
00175   for (unsigned int i = 0 ; i < vertices.size() ; ++i)
00176   {
00177     mesh->vertices[3 * i    ] = vertices[i].getX();
00178     mesh->vertices[3 * i + 1] = vertices[i].getY();
00179     mesh->vertices[3 * i + 2] = vertices[i].getZ();
00180   }
00181         
00182   std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00183         
00184   // compute normals 
00185   for (unsigned int i = 0 ; i < nt ; ++i)
00186   {
00187     tf::Vector3 s1 = vertices[triangles[i * 3    ]] - vertices[triangles[i * 3 + 1]];
00188     tf::Vector3 s2 = vertices[triangles[i * 3 + 1]] - vertices[triangles[i * 3 + 2]];
00189     tf::Vector3 normal = s1.cross(s2);
00190     normal.normalize();
00191     mesh->normals[3 * i    ] = normal.getX();
00192     mesh->normals[3 * i + 1] = normal.getY();
00193     mesh->normals[3 * i + 2] = normal.getZ();
00194   }
00195   return mesh;
00196 }
00197     
00198 shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &source)
00199 {
00200   if (source.size() < 3)
00201     return NULL;
00202         
00203   std::set<detail::myVertex, detail::ltVertexValue> vertices;
00204   std::vector<unsigned int>                         triangles;
00205         
00206   for (unsigned int i = 0 ; i < source.size() / 3 ; ++i)
00207   {
00208     // check if we have new vertices
00209     detail::myVertex vt;
00210             
00211     vt.point = source[3 * i];
00212     std::set<detail::myVertex, detail::ltVertexValue>::iterator p1 = vertices.find(vt);
00213     if (p1 == vertices.end())
00214     {
00215       vt.index = vertices.size();
00216       vertices.insert(vt);                  
00217     }
00218     else
00219       vt.index = p1->index;
00220     triangles.push_back(vt.index);              
00221             
00222     vt.point = source[3 * i + 1];
00223     std::set<detail::myVertex, detail::ltVertexValue>::iterator p2 = vertices.find(vt);
00224     if (p2 == vertices.end())
00225     {
00226       vt.index = vertices.size();
00227       vertices.insert(vt);                  
00228     }
00229     else
00230       vt.index = p2->index;
00231     triangles.push_back(vt.index);              
00232             
00233     vt.point = source[3 * i + 2];
00234     std::set<detail::myVertex, detail::ltVertexValue>::iterator p3 = vertices.find(vt);
00235     if (p3 == vertices.end())
00236     {
00237       vt.index = vertices.size();
00238       vertices.insert(vt);                  
00239     }
00240     else
00241       vt.index = p3->index;
00242 
00243     triangles.push_back(vt.index);
00244   }
00245         
00246   // sort our vertices
00247   std::vector<detail::myVertex> vt;
00248   vt.insert(vt.begin(), vertices.begin(), vertices.end());
00249   std::sort(vt.begin(), vt.end(), detail::ltVertexIndex());
00250         
00251   // copy the data to a mesh structure 
00252   unsigned int nt = triangles.size() / 3;
00253         
00254   shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
00255   for (unsigned int i = 0 ; i < vt.size() ; ++i)
00256   {
00257     mesh->vertices[3 * i    ] = vt[i].point.getX();
00258     mesh->vertices[3 * i + 1] = vt[i].point.getY();
00259     mesh->vertices[3 * i + 2] = vt[i].point.getZ();
00260   }
00261         
00262   std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00263         
00264   // compute normals 
00265   for (unsigned int i = 0 ; i < nt ; ++i)
00266   {
00267     tf::Vector3 s1 = vt[triangles[i * 3    ]].point - vt[triangles[i * 3 + 1]].point;
00268     tf::Vector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
00269     tf::Vector3 normal = s1.cross(s2);
00270     normal.normalize();
00271     mesh->normals[3 * i    ] = normal.getX();
00272     mesh->normals[3 * i + 1] = normal.getY();
00273     mesh->normals[3 * i + 2] = normal.getZ();
00274   }
00275         
00276   return mesh;
00277 }
00278 
00279 shapes::Mesh* createMeshFromFilename(const std::string& filename, const tf::Vector3* scale) {
00280   resource_retriever::Retriever retriever;
00281   resource_retriever::MemoryResource res;
00282   try {
00283     res = retriever.get(filename);
00284   } catch (resource_retriever::Exception& e) {
00285     ROS_ERROR("%s", e.what());
00286     return NULL;
00287   }
00288         
00289   if (res.size == 0) {
00290     ROS_WARN("Retrieved empty mesh for resource '%s'", filename.c_str());
00291     return NULL;
00292   } 
00293   
00294   // Create an instance of the Importer class
00295   Assimp::Importer importer;
00296 
00297   // try to get a file extension
00298   std::string hint;
00299   std::size_t pos = filename.find_last_of(".");
00300   if (pos != std::string::npos)
00301   {
00302     hint = filename.substr(pos + 1);
00303     
00304     // temp hack until everything is stl not stlb
00305     if (hint.find("stl") != std::string::npos)
00306       hint = "stl";
00307   }
00308     
00309   // And have it read the given file with some postprocessing
00310   const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<void*>(res.data.get()), res.size,
00311                                                      aiProcess_Triangulate            |
00312                                                      aiProcess_JoinIdenticalVertices  |
00313                                                      aiProcess_SortByPType, hint.c_str());
00314   if(!scene) {
00315     ROS_WARN_STREAM("Assimp reports no scene in " << filename);
00316     return NULL;
00317   }
00318   if(!scene->HasMeshes()) {
00319     ROS_WARN_STREAM("Assimp reports scene in " << filename << " has no meshes");
00320     return NULL;
00321   }
00322   if(scene->mNumMeshes > 1) {
00323     ROS_WARN_STREAM("Mesh loaded from " << filename << " has " << scene->mNumMeshes << " but only the first one will be used");
00324   }
00325         
00326   aiNode *node = scene->mRootNode;
00327 
00328   bool found = false;
00329   if(node->mNumMeshes > 0 && node->mMeshes != NULL) {
00330     ROS_DEBUG_STREAM("Root node has meshes " << node->mMeshes);
00331     found = true;
00332   } else {
00333     for (uint32_t i=0; i < node->mNumChildren; ++i) {
00334       if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) {
00335         ROS_DEBUG_STREAM("Child " << i << " has meshes");
00336         node = node->mChildren[i];
00337         found = true;
00338         break;
00339       }
00340     }
00341   }
00342   if(found == false) {
00343     ROS_WARN_STREAM("Can't find meshes in file");
00344     return NULL;
00345   }
00346   aiMatrix4x4 transform = node->mTransformation;
00347   tf::Vector3 ts(1.0, 1.0, 1.0);
00348   if(scale != NULL) {
00349     ts = (*scale);
00350   }
00351   return(shapes::createMeshFromAsset(scene->mMeshes[node->mMeshes[0]], transform, ts));
00352 }
00353 
00354 shapes::Mesh* createMeshFromAsset(const aiMesh* a, const aiMatrix4x4& transform, const tf::Vector3& scale)
00355 {
00356   if (!a->HasFaces())
00357   {
00358     ROS_ERROR("Mesh asset has no faces");
00359     return NULL;
00360   }
00361         
00362   if (!a->HasPositions())
00363   {
00364     ROS_ERROR("Mesh asset has no positions");
00365     return NULL;
00366   }
00367         
00368   for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
00369     if (a->mFaces[i].mNumIndices != 3)
00370     {
00371       ROS_ERROR("Asset is not a triangle mesh");
00372       return NULL;
00373     }
00374         
00375         
00376   shapes::Mesh *mesh = new shapes::Mesh(a->mNumVertices, a->mNumFaces);
00377 
00378   // copy vertices
00379   for (unsigned int i = 0 ; i < a->mNumVertices ; ++i)
00380   {
00381     aiVector3D p;
00382     p.x = a->mVertices[i].x;
00383     p.y = a->mVertices[i].y;
00384     p.z = a->mVertices[i].z;
00385     p *= transform;
00386     mesh->vertices[3 * i    ] = p.x*scale.x();
00387     mesh->vertices[3 * i + 1] = p.y*scale.y();
00388     mesh->vertices[3 * i + 2] = p.z*scale.z();
00389   }
00390         
00391   // copy triangles
00392   for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
00393   {
00394     mesh->triangles[3 * i    ] = a->mFaces[i].mIndices[0];
00395     mesh->triangles[3 * i + 1] = a->mFaces[i].mIndices[1];
00396     mesh->triangles[3 * i + 2] = a->mFaces[i].mIndices[2];
00397   }
00398         
00399   // compute normals
00400   for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
00401   {
00402     aiVector3D f1 = a->mVertices[a->mFaces[i].mIndices[0]];
00403     f1.x *= scale.x();
00404     f1.y *= scale.y();
00405     f1.z *= scale.z();
00406     aiVector3D f2 = a->mVertices[a->mFaces[i].mIndices[1]];
00407     f2.x *= scale.x();
00408     f2.y *= scale.y();
00409     f2.z *= scale.z();          
00410     aiVector3D f3 = a->mVertices[a->mFaces[i].mIndices[2]];
00411     f3.x *= scale.x();
00412     f3.y *= scale.y();
00413     f3.z *= scale.z();          
00414     aiVector3D as1 = f1-f2;
00415     aiVector3D as2 = f2-f3;
00416     tf::Vector3   s1(as1.x, as1.y, as1.z);
00417     tf::Vector3   s2(as2.x, as2.y, as2.z);
00418     tf::Vector3 normal = s1.cross(s2);
00419     normal.normalize();
00420     mesh->normals[3 * i    ] = normal.getX();
00421     mesh->normals[3 * i + 1] = normal.getY();
00422     mesh->normals[3 * i + 2] = normal.getZ();
00423   }
00424         
00425   return mesh;
00426 }
00427 
00428 }


geometric_shapes
Author(s): Ioan Sucan
autogenerated on Thu Dec 12 2013 11:08:55