mesh_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 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "geometric_shapes/mesh_operations.h"
00038 #include "geometric_shapes/shape_operations.h"
00039 
00040 #include <cstdio>
00041 #include <cmath>
00042 #include <algorithm>
00043 #include <set>
00044 #include <float.h>
00045 
00046 #include <console_bridge/console.h>
00047 #include <resource_retriever/retriever.h>
00048 
00049 #include <assimp/scene.h>
00050 #include <assimp/Importer.hpp>
00051 #include <assimp/postprocess.h>
00052 
00053 #include <Eigen/Geometry>
00054 
00055 #include <boost/math/constants/constants.hpp>
00056 
00057 namespace shapes
00058 {
00059 
00060 namespace detail
00061 {
00062 
00063 namespace
00064 {
00065 
00067 struct LocalVertexType
00068 {
00069   LocalVertexType() : x(0.0), y(0.0), z(0.0)
00070   {
00071   }
00072 
00073   LocalVertexType(const Eigen::Vector3d &v) : x(v.x()), y(v.y()), z(v.z())
00074   {
00075   }
00076 
00077   double x,y,z;
00078   unsigned int index;
00079 };
00080 
00082 struct ltLocalVertexValue
00083 {
00084   bool operator()(const LocalVertexType &p1, const LocalVertexType &p2) const
00085   {
00086     if (p1.x < p2.x)
00087       return true;
00088     if (p1.x > p2.x)
00089       return false;
00090     if (p1.y < p2.y)
00091       return true;
00092     if (p1.y > p2.y)
00093       return false;
00094     if (p1.z < p2.z)
00095       return true;
00096     return false;
00097   }
00098 };
00099 
00101 struct ltLocalVertexIndex
00102 {
00103   bool operator()(const LocalVertexType &p1, const LocalVertexType &p2) const
00104   {
00105     return p1.index < p2.index;
00106   }
00107 };
00108 
00109 }
00110 }
00111 
00112 Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d &vertices, const std::vector<unsigned int> &triangles)
00113 {
00114   unsigned int nt = triangles.size() / 3;
00115   Mesh *mesh = new Mesh(vertices.size(), nt);
00116   for (unsigned int i = 0 ; i < vertices.size() ; ++i)
00117   {
00118     mesh->vertices[3 * i    ] = vertices[i].x();
00119     mesh->vertices[3 * i + 1] = vertices[i].y();
00120     mesh->vertices[3 * i + 2] = vertices[i].z();
00121   }
00122 
00123   std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00124   mesh->computeTriangleNormals();
00125   mesh->computeVertexNormals();
00126 
00127   return mesh;
00128 }
00129 
00130 Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d &source)
00131 {
00132   if (source.size() < 3)
00133     return NULL;
00134 
00135   if (source.size() % 3 != 0)
00136     logError("The number of vertices to construct a mesh from is not divisible by 3. Probably constructed triangles will not make sense.");
00137 
00138   std::set<detail::LocalVertexType, detail::ltLocalVertexValue> vertices;
00139   std::vector<unsigned int> triangles;
00140 
00141   unsigned int n = source.size() / 3;
00142   for (unsigned int i = 0 ; i < n ; ++i)
00143   {
00144     // check if we have new vertices
00145     unsigned int i3 = i * 3;
00146     detail::LocalVertexType vt1(source[i3]);
00147     std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p1 = vertices.find(vt1);
00148     if (p1 == vertices.end())
00149     {
00150       vt1.index = vertices.size();
00151       vertices.insert(vt1);
00152     }
00153     else
00154       vt1.index = p1->index;
00155     triangles.push_back(vt1.index);
00156 
00157     detail::LocalVertexType vt2(source[++i3]);
00158     std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p2 = vertices.find(vt2);
00159     if (p2 == vertices.end())
00160     {
00161       vt2.index = vertices.size();
00162       vertices.insert(vt2);
00163     }
00164     else
00165       vt2.index = p2->index;
00166     triangles.push_back(vt2.index);
00167 
00168     detail::LocalVertexType vt3(source[++i3]);
00169     std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p3 = vertices.find(vt3);
00170     if (p3 == vertices.end())
00171     {
00172       vt3.index = vertices.size();
00173       vertices.insert(vt3);
00174     }
00175     else
00176       vt3.index = p3->index;
00177 
00178     triangles.push_back(vt3.index);
00179   }
00180 
00181   // sort our vertices
00182   std::vector<detail::LocalVertexType> vt;
00183   vt.insert(vt.end(), vertices.begin(), vertices.end());
00184   std::sort(vt.begin(), vt.end(), detail::ltLocalVertexIndex());
00185 
00186   // copy the data to a mesh structure
00187   unsigned int nt = triangles.size() / 3;
00188 
00189   Mesh *mesh = new Mesh(vt.size(), nt);
00190   for (unsigned int i = 0 ; i < vt.size() ; ++i)
00191   {
00192     unsigned int i3 = i * 3;
00193     mesh->vertices[i3    ] = vt[i].x;
00194     mesh->vertices[i3 + 1] = vt[i].y;
00195     mesh->vertices[i3 + 2] = vt[i].z;
00196   }
00197 
00198   std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00199   mesh->computeTriangleNormals();
00200   mesh->computeVertexNormals();
00201 
00202   return mesh;
00203 }
00204 
00205 Mesh* createMeshFromResource(const std::string& resource)
00206 {
00207   static const Eigen::Vector3d one(1.0, 1.0, 1.0);
00208   return createMeshFromResource(resource, one);
00209 }
00210 
00211 Mesh* createMeshFromBinary(const char* buffer, std::size_t size,
00212                            const std::string &assimp_hint)
00213 {
00214   static const Eigen::Vector3d one(1.0, 1.0, 1.0);
00215   return createMeshFromBinary(buffer, size, one, assimp_hint);
00216 }
00217 
00218 Mesh* createMeshFromBinary(const char *buffer, std::size_t size, const Eigen::Vector3d &scale,
00219                            const std::string &assimp_hint)
00220 {
00221   if (!buffer || size < 1)
00222   {
00223     logWarn("Cannot construct mesh from empty binary buffer");
00224     return NULL;
00225   }
00226 
00227   // try to get a file extension
00228   std::string hint;
00229   std::size_t pos = assimp_hint.find_last_of(".");
00230   if (pos != std::string::npos)
00231   {
00232     hint = assimp_hint.substr(pos + 1);
00233     std::transform(hint.begin(), hint.end(), hint.begin(), ::tolower);
00234   }
00235   if (hint.empty())
00236     hint = assimp_hint; // send entire file name as hint if no extension was found
00237 
00238   // Create an instance of the Importer class
00239   Assimp::Importer importer;
00240 
00241   // Issue #38 fix: as part of the post-processing, we remove all other components in file but
00242   // the meshes, as anyway the resulting shapes:Mesh object just receives vertices and triangles.
00243   importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS,
00244           aiComponent_NORMALS                  |
00245           aiComponent_TANGENTS_AND_BITANGENTS  |
00246           aiComponent_COLORS                   |
00247           aiComponent_TEXCOORDS                |
00248           aiComponent_BONEWEIGHTS              |
00249           aiComponent_ANIMATIONS               |
00250           aiComponent_TEXTURES                 |
00251           aiComponent_LIGHTS                   |
00252           aiComponent_CAMERAS                  |
00253           aiComponent_MATERIALS);
00254 
00255   // And have it read the given file with some post-processing
00256   const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<const void*>(buffer), size,
00257                                                      aiProcess_Triangulate            |
00258                                                      aiProcess_JoinIdenticalVertices  |
00259                                                      aiProcess_SortByPType            |
00260                                                      aiProcess_RemoveComponent, hint.c_str());
00261   if (!scene)
00262     return NULL;
00263 
00264   // Assimp enforces Y_UP convention by rotating models with different conventions.
00265   // However, that behaviour is confusing and doesn't match the ROS convention
00266   // where the Z axis is pointing up.
00267   // Hopefully this doesn't undo legit use of the root node transformation...
00268   // Note that this is also what RViz does internally.
00269   scene->mRootNode->mTransformation = aiMatrix4x4();
00270 
00271   // These post processing steps flatten the root node transformation into child nodes,
00272   // so they must be delayed until after clearing the root node transform above.
00273   importer.ApplyPostProcessing(aiProcess_OptimizeMeshes | aiProcess_OptimizeGraph);
00274 
00275   return createMeshFromAsset(scene, scale, hint);
00276 }
00277 
00278 Mesh* createMeshFromResource(const std::string& resource, const Eigen::Vector3d &scale)
00279 {
00280   resource_retriever::Retriever retriever;
00281   resource_retriever::MemoryResource res;
00282   try
00283   {
00284     res = retriever.get(resource);
00285   }
00286   catch (resource_retriever::Exception& e)
00287   {
00288     logError("%s", e.what());
00289     return NULL;
00290   }
00291 
00292   if (res.size == 0)
00293   {
00294     logWarn("Retrieved empty mesh for resource '%s'", resource.c_str());
00295     return NULL;
00296   }
00297 
00298   Mesh *m = createMeshFromBinary(reinterpret_cast<const char*>(res.data.get()), res.size, scale, resource);
00299   if (!m)
00300   {
00301     logWarn("Assimp reports no scene in %s.", resource.c_str());
00302     logWarn("This could be because of a resource filename that is too long for the Assimp library, a known bug. As a workaround shorten the filename/path.");
00303   }
00304   return m;
00305 }
00306 
00307 namespace
00308 {
00309 void extractMeshData(const aiScene *scene, const aiNode *node, const aiMatrix4x4 &parent_transform, const Eigen::Vector3d &scale,
00310                      EigenSTL::vector_Vector3d &vertices, std::vector<unsigned int> &triangles)
00311 {
00312   aiMatrix4x4 transform = parent_transform;
00313   transform *= node->mTransformation;
00314   for (unsigned int j = 0 ; j < node->mNumMeshes; ++j)
00315   {
00316     const aiMesh* a = scene->mMeshes[node->mMeshes[j]];
00317     unsigned int offset = vertices.size();
00318     for (unsigned int i = 0 ; i < a->mNumVertices ; ++i)
00319     {
00320       aiVector3D v = transform * a->mVertices[i];
00321       vertices.push_back(Eigen::Vector3d(v.x * scale.x(), v.y * scale.y(), v.z * scale.z()));
00322     }
00323     for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
00324       if (a->mFaces[i].mNumIndices == 3)
00325       {
00326         triangles.push_back(offset + a->mFaces[i].mIndices[0]);
00327         triangles.push_back(offset + a->mFaces[i].mIndices[1]);
00328         triangles.push_back(offset + a->mFaces[i].mIndices[2]);
00329       }
00330   }
00331 
00332   for (unsigned int n = 0; n < node->mNumChildren; ++n)
00333     extractMeshData(scene, node->mChildren[n], transform, scale, vertices, triangles);
00334 }
00335 }
00336 
00337 Mesh* createMeshFromAsset(const aiScene* scene, const std::string &resource_name)
00338 {
00339   static const Eigen::Vector3d one(1.0, 1.0, 1.0);
00340   return createMeshFromAsset(scene, one, resource_name);
00341 }
00342 
00343 Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d &scale, const std::string &resource_name)
00344 {
00345   if (!scene->HasMeshes())
00346   {
00347     logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str());
00348     return NULL;
00349   }
00350   EigenSTL::vector_Vector3d vertices;
00351   std::vector<unsigned int> triangles;
00352   extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles);
00353   if (vertices.empty())
00354   {
00355     logWarn("There are no vertices in the scene %s", resource_name.c_str());
00356     return NULL;
00357   }
00358   if (triangles.empty())
00359   {
00360     logWarn("There are no triangles in the scene %s", resource_name.c_str());
00361     return NULL;
00362   }
00363 
00364   return createMeshFromVertices(vertices, triangles);
00365 }
00366 
00367 Mesh* createMeshFromShape(const Shape *shape)
00368 {
00369   if (shape->type == shapes::SPHERE)
00370     return shapes::createMeshFromShape(static_cast<const shapes::Sphere&>(*shape));
00371   else
00372     if (shape->type == shapes::BOX)
00373       return shapes::createMeshFromShape(static_cast<const shapes::Box&>(*shape));
00374     else
00375       if (shape->type == shapes::CYLINDER)
00376         return shapes::createMeshFromShape(static_cast<const shapes::Cylinder&>(*shape));
00377       else
00378         if (shape->type == shapes::CONE)
00379           return shapes::createMeshFromShape(static_cast<const shapes::Cone&>(*shape));
00380         else
00381           logError("Conversion of shape of type '%s' to a mesh is not known", shapeStringName(shape).c_str());
00382   return NULL;
00383 }
00384 
00385 Mesh* createMeshFromShape(const Box &box)
00386 {
00387   double x = box.size[0] / 2.0;
00388   double y = box.size[1] / 2.0;
00389   double z = box.size[2] / 2.0;
00390 
00391   // define vertices of box mesh
00392   Mesh *result = new Mesh(8, 12);
00393   result->vertices[0] = -x;
00394   result->vertices[1] = -y;
00395   result->vertices[2] = -z;
00396 
00397   result->vertices[3] = x;
00398   result->vertices[4] = -y;
00399   result->vertices[5] = -z;
00400 
00401   result->vertices[6] = x;
00402   result->vertices[7] = -y;
00403   result->vertices[8] = z;
00404 
00405   result->vertices[9] = -x;
00406   result->vertices[10] = -y;
00407   result->vertices[11] = z;
00408 
00409   result->vertices[12] = -x;
00410   result->vertices[13] = y;
00411   result->vertices[14] = z;
00412 
00413   result->vertices[15] = -x;
00414   result->vertices[16] = y;
00415   result->vertices[17] = -z;
00416 
00417   result->vertices[18] = x;
00418   result->vertices[19] = y;
00419   result->vertices[20] = z;
00420 
00421   result->vertices[21] = x;
00422   result->vertices[22] = y;
00423   result->vertices[23] = -z;
00424 
00425   static const unsigned int tri[] = {0, 1, 2,
00426                                      2, 3, 0,
00427                                      4, 3, 2,
00428                                      2, 6, 4,
00429                                      7, 6, 2,
00430                                      2, 1, 7,
00431                                      3, 4, 5,
00432                                      5, 0, 3,
00433                                      0, 5, 7,
00434                                      7, 1, 0,
00435                                      7, 5, 4,
00436                                      4, 6, 7};
00437   memcpy(result->triangles, tri, sizeof(unsigned int) * 36);
00438   result->computeTriangleNormals();
00439   result->computeVertexNormals();
00440   return result;
00441 }
00442 
00443 Mesh* createMeshFromShape(const Sphere &sphere)
00444 {
00445   // this code is adapted from FCL
00446   EigenSTL::vector_Vector3d vertices;
00447   std::vector<unsigned int> triangles;
00448 
00449   const double r = sphere.radius;
00450   const double pi = boost::math::constants::pi<double>();
00451   const unsigned int seg = std::max<unsigned int>(6, 0.5 + 2.0 * pi * r / 0.01); // split the sphere longitudinally up to a resolution of 1 cm at the ecuator, or a minimum of 6 segments
00452   const unsigned int ring = std::max<unsigned int>(6, 2.0 * r / 0.01); // split the sphere into rings along latitude, up to a height of at most 1 cm, or a minimum of 6 rings
00453 
00454   double phi, phid;
00455   phid = pi * 2.0 / seg;
00456   phi = 0.0;
00457 
00458   double theta, thetad;
00459   thetad = pi / (ring + 1);
00460   theta = 0;
00461 
00462   for (unsigned int i = 0; i < ring; ++i)
00463   {
00464     double theta_ = theta + thetad * (i + 1);
00465     for (unsigned int j = 0; j < seg; ++j)
00466       vertices.push_back(Eigen::Vector3d(r * sin(theta_) * cos(phi + j * phid),
00467                                          r * sin(theta_) * sin(phi + j * phid),
00468                                          r * cos(theta_)));
00469   }
00470   vertices.push_back(Eigen::Vector3d(0.0, 0.0, r));
00471   vertices.push_back(Eigen::Vector3d(0.0, 0.0, -r));
00472 
00473   for (unsigned int i = 0 ; i < ring - 1; ++i)
00474   {
00475     for (unsigned int j = 0 ; j < seg ; ++j)
00476     {
00477       unsigned int a, b, c, d;
00478       a = i * seg + j;
00479       b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
00480       c = (i + 1) * seg + j;
00481       d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
00482       triangles.push_back(a);
00483       triangles.push_back(c);
00484       triangles.push_back(b);
00485       triangles.push_back(b);
00486       triangles.push_back(c);
00487       triangles.push_back(d);
00488     }
00489   }
00490 
00491   for (unsigned int j = 0 ; j < seg ; ++j)
00492   {
00493     unsigned int a, b;
00494     a = j;
00495     b = (j == seg - 1) ? 0 : (j + 1);
00496     triangles.push_back(ring * seg);
00497     triangles.push_back(a);
00498     triangles.push_back(b);
00499 
00500     a = (ring - 1) * seg + j;
00501     b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
00502     triangles.push_back(a);
00503     triangles.push_back(ring * seg + 1);
00504     triangles.push_back(b);
00505   }
00506   return createMeshFromVertices(vertices, triangles);
00507 }
00508 
00509 Mesh* createMeshFromShape(const Cylinder &cylinder)
00510 {
00511   // this code is adapted from FCL
00512   EigenSTL::vector_Vector3d vertices;
00513   std::vector<unsigned int> triangles;
00514 
00515   // magic number defining how many triangles to construct for the unit cylinder; perhaps this should be a param
00516   static unsigned int tot_for_unit_cylinder = 100;
00517 
00518   double r = cylinder.radius;
00519   double h = cylinder.length;
00520 
00521   const double pi = boost::math::constants::pi<double>();
00522   unsigned int tot = ceil(tot_for_unit_cylinder * r);
00523   double phid = pi * 2 / tot;
00524 
00525   double circle_edge = phid * r;
00526   unsigned int h_num = ceil(h / circle_edge);
00527 
00528   double phi = 0;
00529   double hd = h / h_num;
00530 
00531   for (unsigned int i = 0 ; i < tot ; ++i)
00532     vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
00533 
00534   for (unsigned int i = 0; i < h_num - 1 ; ++i)
00535     for(unsigned int j = 0; j < tot; ++j)
00536       vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
00537 
00538   for (unsigned int i = 0; i < tot; ++i)
00539     vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
00540 
00541   vertices.push_back(Eigen::Vector3d(0, 0, h / 2));
00542   vertices.push_back(Eigen::Vector3d(0, 0, -h / 2));
00543 
00544   for (unsigned int i = 0; i < tot ; ++i)
00545   {
00546     triangles.push_back((h_num + 1) * tot);
00547     triangles.push_back(i);
00548     triangles.push_back((i == tot - 1) ? 0 : (i + 1));
00549   }
00550 
00551   for (unsigned int i = 0; i < tot; ++i)
00552   {
00553     triangles.push_back((h_num + 1) * tot + 1);
00554     triangles.push_back(h_num * tot + ((i == tot - 1) ? 0 : (i + 1)));
00555     triangles.push_back(h_num * tot + i);
00556   }
00557 
00558   for (unsigned int i = 0; i < h_num; ++i)
00559   {
00560     for (unsigned int j = 0; j < tot; ++j)
00561     {
00562       int a, b, c, d;
00563       a = j;
00564       b = (j == tot - 1) ? 0 : (j + 1);
00565       c = j + tot;
00566       d = (j == tot - 1) ? tot : (j + 1 + tot);
00567 
00568       int start = i * tot;
00569       triangles.push_back(start + b);
00570       triangles.push_back(start + a);
00571       triangles.push_back(start + c);
00572       triangles.push_back(start + b);
00573       triangles.push_back(start + c);
00574       triangles.push_back(start + d);
00575     }
00576   }
00577   return createMeshFromVertices(vertices, triangles);
00578 }
00579 
00580 Mesh* createMeshFromShape(const Cone &cone)
00581 {
00582   // this code is adapted from FCL
00583   EigenSTL::vector_Vector3d vertices;
00584   std::vector<unsigned int> triangles;
00585 
00586   // magic number defining how many triangles to construct for the unit cylinder; perhaps this should be a param
00587   static unsigned int tot_for_unit_cone = 100;
00588 
00589   double r = cone.radius;
00590   double h = cone.length;
00591 
00592   const double pi = boost::math::constants::pi<double>();
00593   unsigned int tot = tot_for_unit_cone * r;
00594   double phid = pi * 2 / tot;
00595 
00596   double circle_edge = phid * r;
00597   unsigned int h_num = ceil(h / circle_edge);
00598 
00599   double phi = 0;
00600   double hd = h / h_num;
00601 
00602   for (unsigned int i = 0; i < h_num - 1; ++i)
00603   {
00604     double h_i = h / 2 - (i + 1) * hd;
00605     double rh = r * (0.5 - h_i / h);
00606     for(unsigned int j = 0; j < tot; ++j)
00607       vertices.push_back(Eigen::Vector3d(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
00608   }
00609 
00610   for (unsigned int i = 0; i < tot; ++i)
00611     vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
00612 
00613   vertices.push_back(Eigen::Vector3d(0, 0, h / 2));
00614   vertices.push_back(Eigen::Vector3d(0, 0, -h / 2));
00615 
00616   for (unsigned int i = 0; i < tot; ++i)
00617   {
00618     triangles.push_back(h_num * tot);
00619     triangles.push_back(i);
00620     triangles.push_back((i == tot - 1) ? 0 : (i + 1));
00621   }
00622 
00623   for (unsigned int i = 0; i < tot; ++i)
00624   {
00625     triangles.push_back(h_num * tot + 1);
00626     triangles.push_back((h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)));
00627     triangles.push_back((h_num - 1) * tot + i);
00628   }
00629 
00630   for (unsigned int i = 0; i < h_num - 1; ++i)
00631     for (unsigned int j = 0; j < tot; ++j)
00632     {
00633       int a, b, c, d;
00634       a = j;
00635       b = (j == tot - 1) ? 0 : (j + 1);
00636       c = j + tot;
00637       d = (j == tot - 1) ? tot : (j + 1 + tot);
00638 
00639       int start = i * tot;
00640       triangles.push_back(start + b);
00641       triangles.push_back(start + a);
00642       triangles.push_back(start + c);
00643       triangles.push_back(start + b);
00644       triangles.push_back(start + c);
00645       triangles.push_back(start + d);
00646     }
00647   return createMeshFromVertices(vertices, triangles);
00648 }
00649 
00650 namespace
00651 {
00652   inline void writeFloatToSTL(char *&ptr , float data)
00653   {
00654     memcpy(ptr, &data, sizeof(float));
00655     ptr += sizeof(float);
00656   }
00657   inline void writeFloatToSTL(char *&ptr , double datad)
00658   {
00659     float data = datad;
00660     memcpy(ptr, &data, sizeof(float));
00661     ptr += sizeof(float);
00662   }
00663 }
00664 
00665 void writeSTLBinary(const Mesh* mesh, std::vector<char> &buffer)
00666 {
00667   buffer.resize(84 + mesh->triangle_count * 50);
00668   memset(&buffer[0], 0, 80);
00669   char *ptr = &buffer[80];
00670   uint32_t nt = mesh->triangle_count;
00671   memcpy(ptr, &nt, sizeof(uint32_t));
00672   ptr += sizeof(uint32_t);
00673   for (unsigned int i = 0 ; i < mesh->triangle_count ; ++i)
00674   {
00675     unsigned int i3 = i * 3;
00676 
00677     if (mesh->triangle_normals)
00678     {
00679       writeFloatToSTL(ptr, mesh->triangle_normals[i3]);
00680       writeFloatToSTL(ptr, mesh->triangle_normals[i3 + 1]);
00681       writeFloatToSTL(ptr, mesh->triangle_normals[i3 + 2]);
00682     }
00683     else
00684     {
00685       memset(ptr, 0, sizeof(float) * 3);
00686       ptr += sizeof(float) * 3;
00687     }
00688 
00689     unsigned int index = mesh->triangles[i3] * 3;
00690     writeFloatToSTL(ptr, mesh->vertices[index]);
00691     writeFloatToSTL(ptr, mesh->vertices[index + 1]);
00692     writeFloatToSTL(ptr, mesh->vertices[index + 2]);
00693     index = mesh->triangles[i3 + 1] * 3;
00694     writeFloatToSTL(ptr, mesh->vertices[index]);
00695     writeFloatToSTL(ptr, mesh->vertices[index + 1]);
00696     writeFloatToSTL(ptr, mesh->vertices[index + 2]);
00697     index = mesh->triangles[i3 + 2] * 3;
00698     writeFloatToSTL(ptr, mesh->vertices[index]);
00699     writeFloatToSTL(ptr, mesh->vertices[index + 1]);
00700     writeFloatToSTL(ptr, mesh->vertices[index + 2]);
00701     memset(ptr, 0, 2);
00702     ptr += 2;
00703   }
00704 }
00705 
00706 }


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Thu Jun 8 2017 02:52:45