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 #include <cstdio>
00036 #include <cmath>
00037 #include <algorithm>
00038 #include <set>
00039 #include "pr2_navigation_self_filter/shapes.h"
00040 #include <resource_retriever/retriever.h>
00041 #include <ros/assert.h>
00042 #include <tinyxml.h>
00043 #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
00044 #include <assimp/Importer.hpp>
00045 #include <assimp/scene.h>
00046 #include <assimp/postprocess.h>
00047 #include <assimp/IOStream.hpp>
00048 #include <assimp/IOSystem.hpp>
00049 #else
00050 #include <assimp/assimp.hpp>
00051 #include <assimp/aiScene.h>
00052 #include <assimp/aiPostProcess.h>
00053 #include <assimp/IOStream.h>
00054 #include <assimp/IOSystem.h>
00055 #endif
00056
00057
00058
00059
00060 namespace shapes
00061 {
00062
00063
00064 class ResourceIOStream : public Assimp::IOStream
00065 {
00066 public:
00067 ResourceIOStream(const resource_retriever::MemoryResource& res)
00068 : res_(res)
00069 , pos_(res.data.get())
00070 {}
00071
00072 ~ResourceIOStream()
00073 {}
00074
00075 size_t Read(void* buffer, size_t size, size_t count)
00076 {
00077 size_t to_read = size * count;
00078 if (pos_ + to_read > res_.data.get() + res_.size)
00079 {
00080 to_read = res_.size - (pos_ - res_.data.get());
00081 }
00082
00083 memcpy(buffer, pos_, to_read);
00084 pos_ += to_read;
00085
00086 return to_read;
00087 }
00088
00089 size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
00090
00091 aiReturn Seek( size_t offset, aiOrigin origin)
00092 {
00093 uint8_t* new_pos = 0;
00094 switch (origin)
00095 {
00096 case aiOrigin_SET:
00097 new_pos = res_.data.get() + offset;
00098 break;
00099 case aiOrigin_CUR:
00100 new_pos = pos_ + offset;
00101 break;
00102 case aiOrigin_END:
00103 new_pos = res_.data.get() + res_.size - offset;
00104 break;
00105 default:
00106 ROS_BREAK();
00107 }
00108
00109 if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
00110 {
00111 return aiReturn_FAILURE;
00112 }
00113
00114 pos_ = new_pos;
00115 return aiReturn_SUCCESS;
00116 }
00117
00118 size_t Tell() const
00119 {
00120 return pos_ - res_.data.get();
00121 }
00122
00123 size_t FileSize() const
00124 {
00125 return res_.size;
00126 }
00127
00128 void Flush() {}
00129
00130 private:
00131 resource_retriever::MemoryResource res_;
00132 uint8_t* pos_;
00133 };
00134
00135
00136 class ResourceIOSystem : public Assimp::IOSystem
00137 {
00138 public:
00139 ResourceIOSystem()
00140 {
00141 }
00142
00143 ~ResourceIOSystem()
00144 {
00145 }
00146
00147
00148 bool Exists(const char* file) const
00149 {
00150
00151
00152
00153 resource_retriever::MemoryResource res;
00154 try
00155 {
00156 res = retriever_.get(file);
00157 }
00158 catch (resource_retriever::Exception& e)
00159 {
00160 return false;
00161 }
00162
00163 return true;
00164 }
00165
00166
00167 char getOsSeparator() const
00168 {
00169 return '/';
00170 }
00171
00172
00173 Assimp::IOStream* Open(const char* file, const char* mode = "rb")
00174 {
00175
00176
00177 resource_retriever::MemoryResource res;
00178 try
00179 {
00180 res = retriever_.get(file);
00181 }
00182 catch (resource_retriever::Exception& e)
00183 {
00184 return 0;
00185 }
00186
00187 return new ResourceIOStream(res);
00188 }
00189
00190 void Close(Assimp::IOStream* stream);
00191
00192 private:
00193 mutable resource_retriever::Retriever retriever_;
00194 };
00195
00196 void ResourceIOSystem::Close(Assimp::IOStream* stream)
00197 {
00198 delete stream;
00199 }
00200
00201 float getMeshUnitRescale(const std::string& resource_path)
00202 {
00203 static std::map<std::string, float> rescale_cache;
00204
00205
00206 TiXmlDocument xmlDoc;
00207 float unit_scale(1.0);
00208 resource_retriever::Retriever retriever;
00209 resource_retriever::MemoryResource res;
00210 try
00211 {
00212 res = retriever.get(resource_path);
00213 }
00214 catch (resource_retriever::Exception& e)
00215 {
00216 ROS_ERROR("%s", e.what());
00217 return unit_scale;
00218 }
00219
00220 if (res.size == 0)
00221 {
00222 return unit_scale;
00223 }
00224
00225
00226
00227 const char * data = reinterpret_cast<const char * > (res.data.get());
00228 xmlDoc.Parse(data);
00229
00230
00231 if(!xmlDoc.Error())
00232 {
00233 TiXmlElement * colladaXml = xmlDoc.FirstChildElement("COLLADA");
00234 if(colladaXml)
00235 {
00236 TiXmlElement *assetXml = colladaXml->FirstChildElement("asset");
00237 if(assetXml)
00238 {
00239 TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
00240 if (unitXml && unitXml->Attribute("meter"))
00241 {
00242
00243 if(unitXml->QueryFloatAttribute("meter", &unit_scale) != 0)
00244 ROS_WARN_STREAM("getMeshUnitRescale::Failed to convert unit element meter attribute to determine scaling. unit element: "
00245 << *unitXml);
00246 }
00247 }
00248 }
00249 }
00250 return unit_scale;
00251 }
00252
00253 std::vector<tf::Vector3> getVerticesFromAssimpNode(const aiScene* scene, const aiNode* node, const float scale)
00254 {
00255 std::vector<tf::Vector3> vertices;
00256 if (!node)
00257 {
00258 return vertices;
00259 }
00260
00261 aiMatrix4x4 transform = node->mTransformation;
00262 aiNode *pnode = node->mParent;
00263 while (pnode)
00264 {
00265
00266
00267 if (pnode->mParent != NULL)
00268 transform = pnode->mTransformation * transform;
00269 pnode = pnode->mParent;
00270 }
00271
00272 aiMatrix3x3 rotation(transform);
00273 aiMatrix3x3 inverse_transpose_rotation(rotation);
00274 inverse_transpose_rotation.Inverse();
00275 inverse_transpose_rotation.Transpose();
00276
00277 for (uint32_t i = 0; i < node->mNumMeshes; i++)
00278 {
00279 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
00280
00281 for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
00282 {
00283 aiVector3D p = input_mesh->mVertices[j];
00284 p *= transform;
00285 p *= scale;
00286 tf::Vector3 v(p.x, p.y, p.z);
00287 vertices.push_back(v);
00288 }
00289 }
00290
00291 for (uint32_t i=0; i < node->mNumChildren; ++i)
00292 {
00293 std::vector<tf::Vector3> sub_vertices = getVerticesFromAssimpNode(scene,node->mChildren[i], scale);
00294
00295 for (size_t j = 0; j < sub_vertices.size(); j++) {
00296 vertices.push_back(sub_vertices[j]);
00297 }
00298 }
00299 return vertices;
00300 }
00301
00302 shapes::Mesh* meshFromAssimpScene(const std::string& name, const aiScene* scene)
00303 {
00304 if (!scene->HasMeshes())
00305 {
00306 ROS_ERROR("No meshes found in file [%s]", name.c_str());
00307 return NULL;
00308 }
00309
00310 float scale = getMeshUnitRescale(name);
00311
00312 std::vector<tf::Vector3> vertices = getVerticesFromAssimpNode(scene, scene->mRootNode, scale);
00313
00314 return createMeshFromVertices(vertices);
00315 }
00316
00317 namespace detail
00318 {
00319 struct myVertex
00320 {
00321 tf::Vector3 point;
00322 unsigned int index;
00323 };
00324
00325 struct ltVertexValue
00326 {
00327 bool operator()(const myVertex &p1, const myVertex &p2) const
00328 {
00329 const tf::Vector3 &v1 = p1.point;
00330 const tf::Vector3 &v2 = p2.point;
00331 if (v1.getX() < v2.getX())
00332 return true;
00333 if (v1.getX() > v2.getX())
00334 return false;
00335 if (v1.getY() < v2.getY())
00336 return true;
00337 if (v1.getY() > v2.getY())
00338 return false;
00339 if (v1.getZ() < v2.getZ())
00340 return true;
00341 return false;
00342 }
00343 };
00344
00345 struct ltVertexIndex
00346 {
00347 bool operator()(const myVertex &p1, const myVertex &p2) const
00348 {
00349 return p1.index < p2.index;
00350 }
00351 };
00352 }
00353
00354 shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &vertices, const std::vector<unsigned int> &triangles)
00355 {
00356 unsigned int nt = triangles.size() / 3;
00357 shapes::Mesh *mesh = new shapes::Mesh(vertices.size(), nt);
00358 for (unsigned int i = 0 ; i < vertices.size() ; ++i)
00359 {
00360 mesh->vertices[3 * i ] = vertices[i].getX();
00361 mesh->vertices[3 * i + 1] = vertices[i].getY();
00362 mesh->vertices[3 * i + 2] = vertices[i].getZ();
00363 }
00364
00365 std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00366
00367
00368 for (unsigned int i = 0 ; i < nt ; ++i)
00369 {
00370 tf::Vector3 s1 = vertices[triangles[i * 3 ]] - vertices[triangles[i * 3 + 1]];
00371 tf::Vector3 s2 = vertices[triangles[i * 3 + 1]] - vertices[triangles[i * 3 + 2]];
00372 tf::Vector3 normal = s1.cross(s2);
00373 normal.normalize();
00374 mesh->normals[3 * i ] = normal.getX();
00375 mesh->normals[3 * i + 1] = normal.getY();
00376 mesh->normals[3 * i + 2] = normal.getZ();
00377 }
00378 return mesh;
00379 }
00380
00381 shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &source)
00382 {
00383 if (source.size() < 3)
00384 return NULL;
00385
00386 std::set<detail::myVertex, detail::ltVertexValue> vertices;
00387 std::vector<unsigned int> triangles;
00388
00389 for (unsigned int i = 0 ; i < source.size() / 3 ; ++i)
00390 {
00391
00392 detail::myVertex vt;
00393
00394 vt.point = source[3 * i];
00395 std::set<detail::myVertex, detail::ltVertexValue>::iterator p1 = vertices.find(vt);
00396 if (p1 == vertices.end())
00397 {
00398 vt.index = vertices.size();
00399 vertices.insert(vt);
00400 }
00401 else
00402 vt.index = p1->index;
00403 triangles.push_back(vt.index);
00404
00405 vt.point = source[3 * i + 1];
00406 std::set<detail::myVertex, detail::ltVertexValue>::iterator p2 = vertices.find(vt);
00407 if (p2 == vertices.end())
00408 {
00409 vt.index = vertices.size();
00410 vertices.insert(vt);
00411 }
00412 else
00413 vt.index = p2->index;
00414 triangles.push_back(vt.index);
00415
00416 vt.point = source[3 * i + 2];
00417 std::set<detail::myVertex, detail::ltVertexValue>::iterator p3 = vertices.find(vt);
00418 if (p3 == vertices.end())
00419 {
00420 vt.index = vertices.size();
00421 vertices.insert(vt);
00422 }
00423 else
00424 vt.index = p3->index;
00425
00426 triangles.push_back(vt.index);
00427 }
00428
00429
00430 std::vector<detail::myVertex> vt;
00431 vt.insert(vt.begin(), vertices.begin(), vertices.end());
00432 std::sort(vt.begin(), vt.end(), detail::ltVertexIndex());
00433
00434
00435 unsigned int nt = triangles.size() / 3;
00436
00437 shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt);
00438 for (unsigned int i = 0 ; i < vt.size() ; ++i)
00439 {
00440 mesh->vertices[3 * i ] = vt[i].point.getX();
00441 mesh->vertices[3 * i + 1] = vt[i].point.getY();
00442 mesh->vertices[3 * i + 2] = vt[i].point.getZ();
00443 }
00444
00445 std::copy(triangles.begin(), triangles.end(), mesh->triangles);
00446
00447
00448 for (unsigned int i = 0 ; i < nt ; ++i)
00449 {
00450 tf::Vector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point;
00451 tf::Vector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
00452 tf::Vector3 normal = s1.cross(s2);
00453 normal.normalize();
00454 mesh->normals[3 * i ] = normal.getX();
00455 mesh->normals[3 * i + 1] = normal.getY();
00456 mesh->normals[3 * i + 2] = normal.getZ();
00457 }
00458
00459 return mesh;
00460 }
00461
00462 shapes::Mesh* createMeshFromBinaryStlData(const char *data, unsigned int size)
00463 {
00464 const char* pos = data;
00465 pos += 80;
00466
00467 unsigned int numTriangles = *(unsigned int*)pos;
00468 pos += 4;
00469
00470
00471 if ((long)(50 * numTriangles + 84) <= size)
00472 {
00473 std::vector<tf::Vector3> vertices;
00474
00475 for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
00476 {
00477
00478 pos += 12;
00479
00480
00481 tf::Vector3 v1(0,0,0);
00482 tf::Vector3 v2(0,0,0);
00483 tf::Vector3 v3(0,0,0);
00484
00485 v1.setX(*(float*)pos);
00486 pos += 4;
00487 v1.setY(*(float*)pos);
00488 pos += 4;
00489 v1.setZ(*(float*)pos);
00490 pos += 4;
00491
00492 v2.setX(*(float*)pos);
00493 pos += 4;
00494 v2.setY(*(float*)pos);
00495 pos += 4;
00496 v2.setZ(*(float*)pos);
00497 pos += 4;
00498
00499 v3.setX(*(float*)pos);
00500 pos += 4;
00501 v3.setY(*(float*)pos);
00502 pos += 4;
00503 v3.setZ(*(float*)pos);
00504 pos += 4;
00505
00506
00507 pos += 2;
00508
00509 vertices.push_back(v1);
00510 vertices.push_back(v2);
00511 vertices.push_back(v3);
00512 }
00513
00514 return createMeshFromVertices(vertices);
00515 }
00516
00517 return NULL;
00518 }
00519
00520 shapes::Mesh* createMeshFromBinaryDAE(const char* filename)
00521 {
00522 std::string resource_path(filename);
00523 Assimp::Importer importer;
00524 importer.SetIOHandler(new ResourceIOSystem());
00525 const aiScene* scene = importer.ReadFile(resource_path, aiProcess_SortByPType|aiProcess_GenNormals|aiProcess_Triangulate|aiProcess_GenUVCoords|aiProcess_FlipUVs);
00526 if (!scene)
00527 {
00528 ROS_ERROR("Could not load resource [%s]: %s", resource_path.c_str(), importer.GetErrorString());
00529 return NULL;
00530 }
00531 return meshFromAssimpScene(resource_path, scene);
00532 }
00533
00534 shapes::Mesh* createMeshFromBinaryStl(const char *filename)
00535 {
00536 FILE* input = fopen(filename, "r");
00537 if (!input)
00538 return NULL;
00539
00540 fseek(input, 0, SEEK_END);
00541 long fileSize = ftell(input);
00542 fseek(input, 0, SEEK_SET);
00543
00544 char* buffer = new char[fileSize];
00545 size_t rd = fread(buffer, fileSize, 1, input);
00546
00547 fclose(input);
00548
00549 shapes::Mesh *result = NULL;
00550
00551 if (rd == 1)
00552 result = createMeshFromBinaryStlData(buffer, fileSize);
00553
00554 delete[] buffer;
00555
00556 return result;
00557 }
00558
00559 }