Go to the documentation of this file.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
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
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
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
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
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
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
00295 Assimp::Importer importer;
00296
00297
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
00305 if (hint.find("stl") != std::string::npos)
00306 hint = "stl";
00307 }
00308
00309
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
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
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
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 }