43 #if defined(ASSIMP_UNIFIED_HEADER_NAMES) 44 #include <assimp/Importer.hpp> 45 #include <assimp/scene.h> 46 #include <assimp/postprocess.h> 47 #include <assimp/IOStream.hpp> 48 #include <assimp/IOSystem.hpp> 50 #include <assimp/assimp.hpp> 51 #include <assimp/aiScene.h> 52 #include <assimp/aiPostProcess.h> 53 #include <assimp/IOStream.h> 54 #include <assimp/IOSystem.h> 75 size_t Read(
void* buffer,
size_t size,
size_t count)
77 size_t to_read = size * count;
83 memcpy(buffer,
pos_, to_read);
89 size_t Write(
const void* buffer,
size_t size,
size_t count) {
ROS_BREAK();
return 0; }
91 aiReturn
Seek(
size_t offset, aiOrigin origin)
100 new_pos =
pos_ + offset;
111 return aiReturn_FAILURE;
115 return aiReturn_SUCCESS;
156 res = retriever_.get(file);
173 Assimp::IOStream*
Open(
const char* file,
const char* mode =
"rb")
180 res = retriever_.get(file);
190 void Close(Assimp::IOStream* stream);
203 static std::map<std::string, float> rescale_cache;
206 TiXmlDocument xmlDoc;
207 float unit_scale(1.0);
212 res = retriever.
get(resource_path);
227 const char * data =
reinterpret_cast<const char *
> (res.
data.get());
233 TiXmlElement * colladaXml = xmlDoc.FirstChildElement(
"COLLADA");
236 TiXmlElement *assetXml = colladaXml->FirstChildElement(
"asset");
239 TiXmlElement *unitXml = assetXml->FirstChildElement(
"unit");
240 if (unitXml && unitXml->Attribute(
"meter"))
243 if(unitXml->QueryFloatAttribute(
"meter", &unit_scale) != 0)
244 ROS_WARN_STREAM(
"getMeshUnitRescale::Failed to convert unit element meter attribute to determine scaling. unit element: " 255 std::vector<tf::Vector3> vertices;
261 aiMatrix4x4 transform = node->mTransformation;
262 aiNode *pnode = node->mParent;
267 if (pnode->mParent != NULL)
268 transform = pnode->mTransformation * transform;
269 pnode = pnode->mParent;
272 aiMatrix3x3 rotation(transform);
273 aiMatrix3x3 inverse_transpose_rotation(rotation);
274 inverse_transpose_rotation.Inverse();
275 inverse_transpose_rotation.Transpose();
277 for (uint32_t i = 0; i < node->mNumMeshes; i++)
279 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
281 for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
283 aiVector3D p = input_mesh->mVertices[j];
287 vertices.push_back(v);
291 for (uint32_t i=0; i < node->mNumChildren; ++i)
295 for (
size_t j = 0; j < sub_vertices.size(); j++) {
296 vertices.push_back(sub_vertices[j]);
304 if (!scene->HasMeshes())
306 ROS_ERROR(
"No meshes found in file [%s]", name.c_str());
356 unsigned int nt = triangles.size() / 3;
358 for (
unsigned int i = 0 ; i < vertices.size() ; ++i)
360 mesh->
vertices[3 * i ] = vertices[i].getX();
361 mesh->
vertices[3 * i + 1] = vertices[i].getY();
362 mesh->
vertices[3 * i + 2] = vertices[i].getZ();
365 std::copy(triangles.begin(), triangles.end(), mesh->
triangles);
368 for (
unsigned int i = 0 ; i < nt ; ++i)
370 tf::Vector3 s1 = vertices[triangles[i * 3 ]] - vertices[triangles[i * 3 + 1]];
371 tf::Vector3 s2 = vertices[triangles[i * 3 + 1]] - vertices[triangles[i * 3 + 2]];
383 if (source.size() < 3)
386 std::set<detail::myVertex, detail::ltVertexValue> vertices;
387 std::vector<unsigned int> triangles;
389 for (
unsigned int i = 0 ; i < source.size() / 3 ; ++i)
394 vt.
point = source[3 * i];
395 std::set<detail::myVertex, detail::ltVertexValue>::iterator p1 = vertices.find(vt);
396 if (p1 == vertices.end())
398 vt.
index = vertices.size();
402 vt.
index = p1->index;
403 triangles.push_back(vt.
index);
405 vt.
point = source[3 * i + 1];
406 std::set<detail::myVertex, detail::ltVertexValue>::iterator p2 = vertices.find(vt);
407 if (p2 == vertices.end())
409 vt.
index = vertices.size();
413 vt.
index = p2->index;
414 triangles.push_back(vt.
index);
416 vt.
point = source[3 * i + 2];
417 std::set<detail::myVertex, detail::ltVertexValue>::iterator p3 = vertices.find(vt);
418 if (p3 == vertices.end())
420 vt.
index = vertices.size();
424 vt.
index = p3->index;
426 triangles.push_back(vt.
index);
430 std::vector<detail::myVertex> vt;
431 vt.insert(vt.begin(), vertices.begin(), vertices.end());
435 unsigned int nt = triangles.size() / 3;
438 for (
unsigned int i = 0 ; i < vt.size() ; ++i)
440 mesh->
vertices[3 * i ] = vt[i].point.getX();
441 mesh->
vertices[3 * i + 1] = vt[i].point.getY();
442 mesh->
vertices[3 * i + 2] = vt[i].point.getZ();
445 std::copy(triangles.begin(), triangles.end(), mesh->
triangles);
448 for (
unsigned int i = 0 ; i < nt ; ++i)
450 tf::Vector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point;
451 tf::Vector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point;
464 const char* pos = data;
467 unsigned int numTriangles = *(
unsigned int*)pos;
471 if ((
long)(50 * numTriangles + 84) <= size)
473 std::vector<tf::Vector3> vertices;
475 for (
unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
485 v1.
setX(*(
float*)pos);
487 v1.
setY(*(
float*)pos);
489 v1.
setZ(*(
float*)pos);
492 v2.
setX(*(
float*)pos);
494 v2.
setY(*(
float*)pos);
496 v2.
setZ(*(
float*)pos);
499 v3.
setX(*(
float*)pos);
501 v3.
setY(*(
float*)pos);
503 v3.
setZ(*(
float*)pos);
509 vertices.push_back(v1);
510 vertices.push_back(v2);
511 vertices.push_back(v3);
522 std::string resource_path(filename);
523 Assimp::Importer importer;
525 const aiScene* scene = importer.ReadFile(resource_path, aiProcess_SortByPType|aiProcess_GenNormals|aiProcess_Triangulate|aiProcess_GenUVCoords|aiProcess_FlipUVs);
528 ROS_ERROR(
"Could not load resource [%s]: %s", resource_path.c_str(), importer.GetErrorString());
536 FILE* input = fopen(filename,
"r");
540 fseek(input, 0, SEEK_END);
541 long fileSize = ftell(input);
542 fseek(input, 0, SEEK_SET);
544 char* buffer =
new char[fileSize];
545 size_t rd = fread(buffer, fileSize, 1, input);
Mesh * createMeshFromBinaryStl(const char *filename)
Load a mesh from a binary STL file. Normals are recomputed and repeating vertices are identified...
std::vector< tf::Vector3 > getVerticesFromAssimpNode(const aiScene *scene, const aiNode *node, const float scale)
TFSIMD_FORCE_INLINE void setX(tfScalar x)
Mesh * createMeshFromVertices(const std::vector< tf::Vector3 > &vertices, const std::vector< unsigned int > &triangles)
Load a mesh from a set of vertices. Triangles are constructed using index values from the triangles v...
TFSIMD_FORCE_INLINE void setY(tfScalar y)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
Assimp::IOStream * Open(const char *file, const char *mode="rb")
TFSIMD_FORCE_INLINE const tfScalar & getY() const
char getOsSeparator() const
bool Exists(const char *file) const
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
unsigned int * triangles
the vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
Mesh * createMeshFromBinaryStlData(const char *data, unsigned int size)
Load a mesh from a binary STL stream. Normals are recomputed and repeating vertices are identified...
Mesh * createMeshFromBinaryDAE(const char *filename)
Load a mesh from a binary DAE file. Normals are recomputed and repeating vertices are identified...
boost::shared_array< uint8_t > data
void Close(Assimp::IOStream *stream)
size_t Read(void *buffer, size_t size, size_t count)
double * vertices
the position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
ResourceIOStream(const resource_retriever::MemoryResource &res)
bool operator()(const myVertex &p1, const myVertex &p2) const
#define ROS_WARN_STREAM(args)
shapes::Mesh * meshFromAssimpScene(const std::string &name, const aiScene *scene)
size_t Write(const void *buffer, size_t size, size_t count)
TFSIMD_FORCE_INLINE Vector3 & normalize()
MemoryResource get(const std::string &url)
float getMeshUnitRescale(const std::string &resource_path)
aiReturn Seek(size_t offset, aiOrigin origin)
TFSIMD_FORCE_INLINE const tfScalar & getX() const
resource_retriever::MemoryResource res_
double * normals
the normal to each triangle unit vector represented as (x,y,z)
resource_retriever::Retriever retriever_
bool operator()(const myVertex &p1, const myVertex &p2) const