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


geometric_shapes
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 11:40:00