00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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
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
00245 Assimp::Importer importer;
00246
00247
00248
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
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
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);
00432 const unsigned int ring = std::max<unsigned int>(6, 2.0 * r / 0.01);
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
00492 EigenSTL::vector_Vector3d vertices;
00493 std::vector<unsigned int> triangles;
00494
00495
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
00563 EigenSTL::vector_Vector3d vertices;
00564 std::vector<unsigned int> triangles;
00565
00566
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 }