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 #include <assimp/assimp.hpp>
00047 #include <assimp/aiScene.h>
00048 #include <assimp/aiPostProcess.h>
00049
00050 namespace shapes
00051 {
00052
00053 Shape* cloneShape(const Shape *shape)
00054 {
00055 Shape *result = NULL;
00056 if (shape) {
00057 switch (shape->type)
00058 {
00059 case SPHERE:
00060 result = new Sphere(static_cast<const Sphere*>(shape)->radius);
00061 break;
00062 case CYLINDER:
00063 result = new Cylinder(static_cast<const Cylinder*>(shape)->radius, static_cast<const Cylinder*>(shape)->length);
00064 break;
00065 case BOX:
00066 result = new Box(static_cast<const Box*>(shape)->size[0], static_cast<const Box*>(shape)->size[1], static_cast<const Box*>(shape)->size[2]);
00067 break;
00068 case MESH:
00069 {
00070 const Mesh *src = static_cast<const Mesh*>(shape);
00071 Mesh *dest = new Mesh(src->vertexCount, src->triangleCount);
00072 unsigned int n = 3 * src->vertexCount;
00073 for (unsigned int i = 0 ; i < n ; ++i)
00074 dest->vertices[i] = src->vertices[i];
00075 n = 3 * src->triangleCount;
00076 for (unsigned int i = 0 ; i < n ; ++i)
00077 {
00078 dest->triangles[i] = src->triangles[i];
00079 dest->normals[i] = src->normals[i];
00080 }
00081 result = dest;
00082 }
00083 break;
00084 default:
00085 break;
00086 }
00087 }
00088 return result;
00089 }
00090
00091 std::vector<Shape*> cloneShapeVector(const std::vector<Shape*>& shapes)
00092 {
00093 std::vector<Shape*> ret;
00094 for(unsigned int i = 0; i < shapes.size(); i++) {
00095 ret.push_back(cloneShape(shapes[i]));
00096 }
00097 return ret;
00098 }
00099
00100 void deleteShapeVector(std::vector<Shape*>& shapes) {
00101 for(unsigned int i = 0; i < shapes.size(); i++) {
00102 delete shapes[i];
00103 }
00104 shapes.clear();
00105 }
00106
00107 StaticShape* cloneShape(const StaticShape *shape)
00108 {
00109 StaticShape *result = NULL;
00110 if (shape) {
00111 switch (shape->type)
00112 {
00113 case PLANE:
00114 result = new Plane(static_cast<const Plane*>(shape)->a, static_cast<const Plane*>(shape)->b,
00115 static_cast<const Plane*>(shape)->c, static_cast<const Plane*>(shape)->d);
00116 break;
00117 default:
00118 break;
00119 }
00120 }
00121
00122 return result;
00123 }
00124
00125
00126 namespace detail
00127 {
00128 struct myVertex
00129 {
00130 btVector3 point;
00131 unsigned int index;
00132 };
00133
00134 struct ltVertexValue
00135 {
00136 bool operator()(const myVertex &p1, const myVertex &p2) const
00137 {
00138 const btVector3 &v1 = p1.point;
00139 const btVector3 &v2 = p2.point;
00140 if (v1.getX() < v2.getX())
00141 return true;
00142 if (v1.getX() > v2.getX())
00143 return false;
00144 if (v1.getY() < v2.getY())
00145 return true;
00146 if (v1.getY() > v2.getY())
00147 return false;
00148 if (v1.getZ() < v2.getZ())
00149 return true;
00150 return false;
00151 }
00152 };
00153
00154 struct ltVertexIndex
00155 {
00156 bool operator()(const myVertex &p1, const myVertex &p2) const
00157 {
00158 return p1.index < p2.index;
00159 }
00160 };
00161
00162 }
00163
00164 shapes::Mesh* createMeshFromVertices(const std::vector<btVector3> &vertices, const std::vector<unsigned int> &triangles)
00165 {
00166 unsigned int nt = triangles.size() / 3;
00167 shapes::Mesh *mesh = new shapes::Mesh(vertices.size(), nt);
00168 for (unsigned int i = 0 ; i < vertices.size() ; ++i)
00169 {
00170 mesh->vertices[3 * i ] = vertices[i].getX();
00171 mesh->vertices[3 * i + 1] = vertices[i].getY();
00172 mesh->vertices[3 * i + 2] = vertices[i].getZ();
00173 }
00174
00175 std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00176
00177
00178 for (unsigned int i = 0 ; i < nt ; ++i)
00179 {
00180 btVector3 s1 = vertices[triangles[i * 3 ]] - vertices[triangles[i * 3 + 1]];
00181 btVector3 s2 = vertices[triangles[i * 3 + 1]] - vertices[triangles[i * 3 + 2]];
00182 btVector3 normal = s1.cross(s2);
00183 normal.normalize();
00184 mesh->normals[3 * i ] = normal.getX();
00185 mesh->normals[3 * i + 1] = normal.getY();
00186 mesh->normals[3 * i + 2] = normal.getZ();
00187 }
00188 return mesh;
00189 }
00190
00191 shapes::Mesh* createMeshFromVertices(const std::vector<btVector3> &source)
00192 {
00193 if (source.size() < 3)
00194 return NULL;
00195
00196 std::set<detail::myVertex, detail::ltVertexValue> vertices;
00197 std::vector<unsigned int> triangles;
00198
00199 for (unsigned int i = 0 ; i < source.size() / 3 ; ++i)
00200 {
00201
00202 detail::myVertex vt;
00203
00204 vt.point = source[3 * i];
00205 std::set<detail::myVertex, detail::ltVertexValue>::iterator p1 = vertices.find(vt);
00206 if (p1 == vertices.end())
00207 {
00208 vt.index = vertices.size();
00209 vertices.insert(vt);
00210 }
00211 else
00212 vt.index = p1->index;
00213 triangles.push_back(vt.index);
00214
00215 vt.point = source[3 * i + 1];
00216 std::set<detail::myVertex, detail::ltVertexValue>::iterator p2 = vertices.find(vt);
00217 if (p2 == vertices.end())
00218 {
00219 vt.index = vertices.size();
00220 vertices.insert(vt);
00221 }
00222 else
00223 vt.index = p2->index;
00224 triangles.push_back(vt.index);
00225
00226 vt.point = source[3 * i + 2];
00227 std::set<detail::myVertex, detail::ltVertexValue>::iterator p3 = vertices.find(vt);
00228 if (p3 == vertices.end())
00229 {
00230 vt.index = vertices.size();
00231 vertices.insert(vt);
00232 }
00233 else
00234 vt.index = p3->index;
00235
00236 triangles.push_back(vt.index);
00237 }
00238
00239
00240 std::vector<detail::myVertex> vt;
00241 vt.insert(vt.begin(), vertices.begin(), vertices.end());
00242 std::sort(vt.begin(), vt.end(), detail::ltVertexIndex());
00243
00244
00245 unsigned int nt = triangles.size() / 3;
00246
00247 shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
00248 for (unsigned int i = 0 ; i < vt.size() ; ++i)
00249 {
00250 mesh->vertices[3 * i ] = vt[i].point.getX();
00251 mesh->vertices[3 * i + 1] = vt[i].point.getY();
00252 mesh->vertices[3 * i + 2] = vt[i].point.getZ();
00253 }
00254
00255 std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00256
00257
00258 for (unsigned int i = 0 ; i < nt ; ++i)
00259 {
00260 btVector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point;
00261 btVector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
00262 btVector3 normal = s1.cross(s2);
00263 normal.normalize();
00264 mesh->normals[3 * i ] = normal.getX();
00265 mesh->normals[3 * i + 1] = normal.getY();
00266 mesh->normals[3 * i + 2] = normal.getZ();
00267 }
00268
00269 return mesh;
00270 }
00271
00272 shapes::Mesh* createMeshFromFilename(const std::string& filename, const btVector3* scale) {
00273 resource_retriever::Retriever retriever;
00274 resource_retriever::MemoryResource res;
00275 try {
00276 res = retriever.get(filename);
00277 } catch (resource_retriever::Exception& e) {
00278 ROS_ERROR("%s", e.what());
00279 return NULL;
00280 }
00281
00282 if (res.size == 0) {
00283 ROS_WARN("Retrieved empty mesh for resource '%s'", filename.c_str());
00284 return NULL;
00285 }
00286
00287
00288 Assimp::Importer importer;
00289
00290
00291 std::string hint;
00292 std::size_t pos = filename.find_last_of(".");
00293 if (pos != std::string::npos)
00294 {
00295 hint = filename.substr(pos + 1);
00296
00297
00298 if (hint.find("stl") != std::string::npos)
00299 hint = "stl";
00300 }
00301
00302
00303 const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<void*>(res.data.get()), res.size,
00304 aiProcess_Triangulate |
00305 aiProcess_JoinIdenticalVertices |
00306 aiProcess_SortByPType, hint.c_str());
00307 if(!scene) {
00308 ROS_WARN_STREAM("Assimp reports no scene in " << filename);
00309 return NULL;
00310 }
00311 if(!scene->HasMeshes()) {
00312 ROS_WARN_STREAM("Assimp reports scene in " << filename << " has no meshes");
00313 return NULL;
00314 }
00315 if(scene->mNumMeshes > 1) {
00316 ROS_WARN_STREAM("Mesh loaded from " << filename << " has " << scene->mNumMeshes << " but only the first one will be used");
00317 }
00318
00319 aiNode *node = scene->mRootNode;
00320
00321 bool found = false;
00322 if(node->mNumMeshes > 0 && node->mMeshes != NULL) {
00323 ROS_DEBUG_STREAM("Root node has meshes " << node->mMeshes);
00324 found = true;
00325 } else {
00326 for (uint32_t i=0; i < node->mNumChildren; ++i) {
00327 if(node->mChildren[i]->mNumMeshes > 0 && node->mChildren[i]->mMeshes != NULL) {
00328 ROS_DEBUG_STREAM("Child " << i << " has meshes");
00329 node = node->mChildren[i];
00330 found = true;
00331 break;
00332 }
00333 }
00334 }
00335 if(found == false) {
00336 ROS_WARN_STREAM("Can't find meshes in file");
00337 return NULL;
00338 }
00339 aiMatrix4x4 transform = node->mTransformation;
00340 btVector3 ts(1.0, 1.0, 1.0);
00341 if(scale != NULL) {
00342 ts = (*scale);
00343 }
00344 return(shapes::createMeshFromAsset(scene->mMeshes[node->mMeshes[0]], transform, ts));
00345 }
00346
00347 shapes::Mesh* createMeshFromAsset(const aiMesh* a, const aiMatrix4x4& transform, const btVector3& scale)
00348 {
00349 if (!a->HasFaces())
00350 {
00351 ROS_ERROR("Mesh asset has no faces");
00352 return NULL;
00353 }
00354
00355 if (!a->HasPositions())
00356 {
00357 ROS_ERROR("Mesh asset has no positions");
00358 return NULL;
00359 }
00360
00361 for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
00362 if (a->mFaces[i].mNumIndices != 3)
00363 {
00364 ROS_ERROR("Asset is not a triangle mesh");
00365 return NULL;
00366 }
00367
00368
00369 shapes::Mesh *mesh = new shapes::Mesh(a->mNumVertices, a->mNumFaces);
00370
00371
00372 for (unsigned int i = 0 ; i < a->mNumVertices ; ++i)
00373 {
00374 aiVector3D p;
00375 p.x = a->mVertices[i].x;
00376 p.y = a->mVertices[i].y;
00377 p.z = a->mVertices[i].z;
00378 p *= transform;
00379 mesh->vertices[3 * i ] = p.x*scale.x();
00380 mesh->vertices[3 * i + 1] = p.y*scale.y();
00381 mesh->vertices[3 * i + 2] = p.z*scale.z();
00382 }
00383
00384
00385 for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
00386 {
00387 mesh->triangles[3 * i ] = a->mFaces[i].mIndices[0];
00388 mesh->triangles[3 * i + 1] = a->mFaces[i].mIndices[1];
00389 mesh->triangles[3 * i + 2] = a->mFaces[i].mIndices[2];
00390 }
00391
00392
00393 for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
00394 {
00395 aiVector3D f1 = a->mVertices[a->mFaces[i].mIndices[0]];
00396 f1.x *= scale.x();
00397 f1.y *= scale.y();
00398 f1.z *= scale.z();
00399 aiVector3D f2 = a->mVertices[a->mFaces[i].mIndices[1]];
00400 f2.x *= scale.x();
00401 f2.y *= scale.y();
00402 f2.z *= scale.z();
00403 aiVector3D f3 = a->mVertices[a->mFaces[i].mIndices[2]];
00404 f3.x *= scale.x();
00405 f3.y *= scale.y();
00406 f3.z *= scale.z();
00407 aiVector3D as1 = f1-f2;
00408 aiVector3D as2 = f2-f3;
00409 btVector3 s1(as1.x, as1.y, as1.z);
00410 btVector3 s2(as2.x, as2.y, as2.z);
00411 btVector3 normal = s1.cross(s2);
00412 normal.normalize();
00413 mesh->normals[3 * i ] = normal.getX();
00414 mesh->normals[3 * i + 1] = normal.getY();
00415 mesh->normals[3 * i + 2] = normal.getZ();
00416 }
00417
00418 return mesh;
00419 }
00420
00421 }
