Skip to content
Snippets Groups Projects
Commit eb4c3092 authored by jcarpent's avatar jcarpent
Browse files

[C++] Add missing namespace

parent 80cfbe61
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/multibody/parser/from-collada-to-fcl.hpp
+ 146
− 143
View file @ eb4c3092
......@@ -37,180 +37,183 @@
#include <exception>
typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
typedef boost::shared_ptr <PolyhedronType> Polyhedron_ptr;
struct TriangleAndVertices
{
void clear()
{
vertices_.clear ();
triangles_.clear ();
}
std::vector <fcl::Vec3f> vertices_;
std::vector <fcl::Triangle> triangles_;
};
/**
* @brief Recursive procedure for building a mesh
*
* @param[in] scale Scale to apply when reading the ressource
* @param[in] scene Pointer to the assimp scene
* @param[in] node Current node of the scene
* @param subMeshIndexes Submesh triangles indexes interval
* @param[in] mesh The mesh that must be built
* @param tv Triangles and Vertices of the mesh submodels
*/
inline void buildMesh (const ::urdf::Vector3 & scale,
const aiScene* scene,
const aiNode* node,
std::vector<unsigned> & subMeshIndexes,
const Polyhedron_ptr & mesh,
TriangleAndVertices & tv)
namespace se3
{
if (!node) return;
aiMatrix4x4 transform = node->mTransformation;
aiNode *pnode = node->mParent;
while (pnode)
typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
typedef boost::shared_ptr <PolyhedronType> Polyhedron_ptr;
struct TriangleAndVertices
{
// Don't convert to y-up orientation, which is what the root node in
// Assimp does
if (pnode->mParent != NULL)
void clear()
{
transform = pnode->mTransformation * transform;
vertices_.clear ();
triangles_.clear ();
}
pnode = pnode->mParent;
}
for (uint32_t i = 0; i < node->mNumMeshes; i++)
std::vector <fcl::Vec3f> vertices_;
std::vector <fcl::Triangle> triangles_;
};
/**
* @brief Recursive procedure for building a mesh
*
* @param[in] scale Scale to apply when reading the ressource
* @param[in] scene Pointer to the assimp scene
* @param[in] node Current node of the scene
* @param subMeshIndexes Submesh triangles indexes interval
* @param[in] mesh The mesh that must be built
* @param tv Triangles and Vertices of the mesh submodels
*/
inline void buildMesh (const ::urdf::Vector3 & scale,
const aiScene* scene,
const aiNode* node,
std::vector<unsigned> & subMeshIndexes,
const Polyhedron_ptr & mesh,
TriangleAndVertices & tv)
{
aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
unsigned oldNbPoints = (unsigned) mesh->num_vertices;
unsigned oldNbTriangles = (unsigned) mesh->num_tris;
// Add the vertices
for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
if (!node) return;
aiMatrix4x4 transform = node->mTransformation;
aiNode *pnode = node->mParent;
while (pnode)
{
aiVector3D p = input_mesh->mVertices[j];
p *= transform;
tv.vertices_.push_back (fcl::Vec3f (p.x * scale.x,
p.y * scale.y,
p.z * scale.z));
// Don't convert to y-up orientation, which is what the root node in
// Assimp does
if (pnode->mParent != NULL)
{
transform = pnode->mTransformation * transform;
}
pnode = pnode->mParent;
}
// add the indices
for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
for (uint32_t i = 0; i < node->mNumMeshes; i++)
{
aiFace& face = input_mesh->mFaces[j];
// FIXME: can add only triangular faces.
tv.triangles_.push_back (fcl::Triangle( oldNbPoints + face.mIndices[0],
oldNbPoints + face.mIndices[1],
oldNbPoints + face.mIndices[2]));
aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
unsigned oldNbPoints = (unsigned) mesh->num_vertices;
unsigned oldNbTriangles = (unsigned) mesh->num_tris;
// Add the vertices
for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
{
aiVector3D p = input_mesh->mVertices[j];
p *= transform;
tv.vertices_.push_back (fcl::Vec3f (p.x * scale.x,
p.y * scale.y,
p.z * scale.z));
}
// add the indices
for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
{
aiFace& face = input_mesh->mFaces[j];
// FIXME: can add only triangular faces.
tv.triangles_.push_back (fcl::Triangle( oldNbPoints + face.mIndices[0],
oldNbPoints + face.mIndices[1],
oldNbPoints + face.mIndices[2]));
}
// Save submesh triangles indexes interval.
if (subMeshIndexes.size () == 0)
{
subMeshIndexes.push_back (0);
}
subMeshIndexes.push_back (oldNbTriangles + input_mesh->mNumFaces);
}
// Save submesh triangles indexes interval.
if (subMeshIndexes.size () == 0)
for (uint32_t i=0; i < node->mNumChildren; ++i)
{
subMeshIndexes.push_back (0);
buildMesh(scale, scene, node->mChildren[i], subMeshIndexes, mesh, tv);
}
subMeshIndexes.push_back (oldNbTriangles + input_mesh->mNumFaces);
}
for (uint32_t i=0; i < node->mNumChildren; ++i)
{
buildMesh(scale, scene, node->mChildren[i], subMeshIndexes, mesh, tv);
}
}
/**
* @brief Convert an assimp scene to a mesh
*
* @param[in] name File (ressource) transformed into an assimp scene in loa
* @param[in] scale Scale to apply when reading the ressource
* @param[in] scene Pointer to the assimp scene
* @param[out] mesh The mesh that must be built
*/
inline void meshFromAssimpScene (const std::string & name,
const ::urdf::Vector3 & scale,
const aiScene* scene,
const Polyhedron_ptr & mesh) throw (std::invalid_argument)
/**
* @brief Convert an assimp scene to a mesh
*
* @param[in] name File (ressource) transformed into an assimp scene in loa
* @param[in] scale Scale to apply when reading the ressource
* @param[in] scene Pointer to the assimp scene
* @param[out] mesh The mesh that must be built
*/
inline void meshFromAssimpScene (const std::string & name,
const ::urdf::Vector3 & scale,
const aiScene* scene,
const Polyhedron_ptr & mesh) throw (std::invalid_argument)
{
TriangleAndVertices tv;
if (!scene->HasMeshes())
{
throw std::invalid_argument (std::string ("No meshes found in file ")+
name);
}
throw std::invalid_argument (std::string ("No meshes found in file ")+name);
std::vector<unsigned> subMeshIndexes;
int res = mesh->beginModel ();
if (res != fcl::BVH_OK)
{
std::ostringstream error;
error << "fcl BVHReturnCode = " << res;
throw std::runtime_error (error.str ());
}
tv.clear();
buildMesh (scale, scene, scene->mRootNode, subMeshIndexes, mesh, tv);
mesh->addSubModel (tv.vertices_, tv.triangles_);
mesh->endModel ();
}
/**
* @brief Read a mesh file and convert it to a polyhedral mesh
*
* @param[in] resource_path Path to the ressource mesh file to be read
* @param[in] scale Scale to apply when reading the ressource
* @param[out] polyhedron The resulted polyhedron
*/
inline void loadPolyhedronFromResource (const std::string & resource_path,
const ::urdf::Vector3 & scale,
const Polyhedron_ptr & polyhedron) throw (std::invalid_argument)
{
Assimp::Importer importer;
const aiScene* scene = importer.ReadFile(resource_path.c_str(), aiProcess_SortByPType| aiProcess_GenNormals|
aiProcess_Triangulate|aiProcess_GenUVCoords|
aiProcess_FlipUVs);
if (!scene)
/**
* @brief Read a mesh file and convert it to a polyhedral mesh
*
* @param[in] resource_path Path to the ressource mesh file to be read
* @param[in] scale Scale to apply when reading the ressource
* @param[out] polyhedron The resulted polyhedron
*/
inline void loadPolyhedronFromResource (const std::string & resource_path,
const ::urdf::Vector3 & scale,
const Polyhedron_ptr & polyhedron) throw (std::invalid_argument)
{
const std::string exception_message (std::string ("Could not load resource ") + resource_path + std::string("\n") +
importer.GetErrorString () + std::string("\n") +
"Hint: the mesh directory may be wrong.");
throw std::invalid_argument(exception_message);
Assimp::Importer importer;
const aiScene* scene = importer.ReadFile(resource_path.c_str(), aiProcess_SortByPType| aiProcess_GenNormals|
aiProcess_Triangulate|aiProcess_GenUVCoords|
aiProcess_FlipUVs);
if (!scene)
{
const std::string exception_message (std::string ("Could not load resource ") + resource_path + std::string("\n") +
importer.GetErrorString () + std::string("\n") +
"Hint: the mesh directory may be wrong.");
throw std::invalid_argument(exception_message);
}
meshFromAssimpScene (resource_path, scale, scene, polyhedron);
}
/**
* @brief Transform a cURL readable path (package://..) to an absolute path for urdf collision path
*
* @param[in] urdf_mesh_path The path given in the urdf file (package://..)
* @param[in] meshRootDir Root path to the directory where meshes are located
*
* @return The absolute path to the mesh file
*/
inline std::string fromURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path,
const std::string & meshRootDir)
{
std::string absolutePath = std::string(meshRootDir +
urdf_mesh_path.substr(10, urdf_mesh_path.size()));
return absolutePath;
}
meshFromAssimpScene (resource_path, scale, scene, polyhedron);
}
/**
* @brief Transform a cURL readable path (package://..) to an absolute path for urdf collision path
*
* @param[in] urdf_mesh_path The path given in the urdf file (package://..)
* @param[in] meshRootDir Root path to the directory where meshes are located
*
* @return The absolute path to the mesh file
*/
inline std::string fromURDFMeshPathToAbsolutePath(const std::string & urdf_mesh_path,
const std::string & meshRootDir)
{
std::string absolutePath = std::string(meshRootDir +
urdf_mesh_path.substr(10, urdf_mesh_path.size()));
} // namespace se3
return absolutePath;
}
#endif // __se3_collada_to_fcl_hpp__
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment