shapes::Mesh* createMeshFromBinaryStlData(const char *data, unsigned int size) { const char* pos = data; pos += 80; // skip the 80 byte header unsigned int numTriangles = *(unsigned int*)pos; pos += 4; // make sure we have read enough data if ((long)(50 * numTriangles + 84) <= size) { std::vector<tf::Vector3> vertices; for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle) { // skip the normal pos += 12; // read vertices tf::Vector3 v1(0,0,0); tf::Vector3 v2(0,0,0); tf::Vector3 v3(0,0,0); v1.setX(*(float*)pos); pos += 4; v1.setY(*(float*)pos); pos += 4; v1.setZ(*(float*)pos); pos += 4; v2.setX(*(float*)pos); pos += 4; v2.setY(*(float*)pos); pos += 4; v2.setZ(*(float*)pos); pos += 4; v3.setX(*(float*)pos); pos += 4; v3.setY(*(float*)pos); pos += 4; v3.setZ(*(float*)pos); pos += 4; // skip attribute pos += 2; vertices.push_back(v1); vertices.push_back(v2); vertices.push_back(v3); } return createMeshFromVertices(vertices); } return NULL; }
shapes::Mesh* meshFromAssimpScene(const std::string& name, const aiScene* scene) { if (!scene->HasMeshes()) { ROS_ERROR("No meshes found in file [%s]", name.c_str()); return NULL; } float scale = getMeshUnitRescale(name); std::vector<tf::Vector3> vertices = getVerticesFromAssimpNode(scene, scene->mRootNode, scale); return createMeshFromVertices(vertices); }
Shape* constructShapeFromMsg(const shape_msgs::Mesh &shape_msg) { if (shape_msg.triangles.empty() || shape_msg.vertices.empty()) { logWarn("Mesh definition is empty"); return NULL; } else { EigenSTL::vector_Vector3d vertices(shape_msg.vertices.size()); std::vector<unsigned int> triangles(shape_msg.triangles.size() * 3); for (unsigned int i = 0 ; i < shape_msg.vertices.size() ; ++i) vertices[i] = Eigen::Vector3d(shape_msg.vertices[i].x, shape_msg.vertices[i].y, shape_msg.vertices[i].z); for (unsigned int i = 0 ; i < shape_msg.triangles.size() ; ++i) { unsigned int i3 = i * 3; triangles[i3++] = shape_msg.triangles[i].vertex_indices[0]; triangles[i3++] = shape_msg.triangles[i].vertex_indices[1]; triangles[i3] = shape_msg.triangles[i].vertex_indices[2]; } return createMeshFromVertices(vertices, triangles); } }
Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d &scale, const std::string &resource_name) { if (!scene->HasMeshes()) { logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str()); return NULL; } EigenSTL::vector_Vector3d vertices; std::vector<unsigned int> triangles; extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles); if (vertices.empty()) { logWarn("There are no vertices in the scene %s", resource_name.c_str()); return NULL; } if (triangles.empty()) { logWarn("There are no triangles in the scene %s", resource_name.c_str()); return NULL; } return createMeshFromVertices(vertices, triangles); }