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);
}
