Commit 3c3baf68 authored by Valenza Florian's avatar Valenza Florian
Browse files

[Geometry] Moved assimp module to hpp-fcl

Conflicts:

	CMakeLists.txt
parent a9f71a13
......@@ -90,12 +90,7 @@ IF(URDFDOM_FOUND)
ENDIF(${URDFDOM_VERSION} VERSION_LESS "0.3.0")
ENDIF(URDFDOM_FOUND)
IF(HPP_FCL_FOUND AND URDFDOM_FOUND)
ADD_REQUIRED_DEPENDENCY("assimp >= 2.0")
IF(NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
ADD_DEFINITIONS(-DASSIMP_UNIFIED_HEADER_NAMES)
ENDIF(NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
ENDIF(HPP_FCL_FOUND AND URDFDOM_FOUND)
SET(BOOST_COMPONENTS filesystem unit_test_framework system)
......
......@@ -75,9 +75,6 @@ IF(UNIX)
IF(URDFDOM_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} urdfdom)
IF(HPP_FCL_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} assimp)
ENDIF(HPP_FCL_FOUND)
ENDIF(URDFDOM_FOUND)
IF(HPP_FCL_FOUND)
......@@ -108,9 +105,6 @@ IF(BUILD_PYTHON_INTERFACE)
IF(URDFDOM_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} urdfdom)
IF(HPP_FCL_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} assimp)
ENDIF(HPP_FCL_FOUND)
ENDIF(URDFDOM_FOUND)
IF(HPP_FCL_FOUND)
PKG_CONFIG_USE_DEPENDENCY(${PYWRAP} hpp-fcl)
......
......@@ -26,25 +26,6 @@
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#ifdef ASSIMP_UNIFIED_HEADER_NAMES
# include <assimp/DefaultLogger.hpp>
# include <assimp/IOStream.hpp>
# include <assimp/IOSystem.hpp>
# include <assimp/scene.h>
# include <assimp/Importer.hpp>
# include <assimp/postprocess.h>
#else
# include <assimp/DefaultLogger.h>
# include <assimp/assimp.hpp>
# include <assimp/IOStream.h>
# include <assimp/IOSystem.h>
# include <assimp/aiScene.h>
# include <assimp/aiPostProcess.h>
#endif
#include <hpp/fcl/BV/OBBRSS.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include "pinocchio/tools/file-explorer.hpp"
#include <boost/filesystem.hpp>
......@@ -52,169 +33,7 @@
namespace se3
{
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)
{
if (!node) return;
aiMatrix4x4 transform = node->mTransformation;
aiNode *pnode = node->mParent;
while (pnode)
{
// 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;
}
for (uint32_t i = 0; i < node->mNumMeshes; i++)
{
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];
if (face.mNumIndices != 3) {
std::stringstream ss;
ss << "Mesh " << input_mesh->mName.C_Str() << " has a face with "
<< face.mNumIndices << " vertices. This is not supported\n";
ss << "Node name is: " << node->mName.C_Str() << "\n";
ss << "Mesh index: " << i << "\n";
ss << "Face index: " << j << "\n";
throw std::invalid_argument (ss.str());
}
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);
}
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)
{
TriangleAndVertices tv;
if (!scene->HasMeshes())
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)
{
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 package://.. mesh path to an absolute path, searching for a valid file
......
......@@ -25,12 +25,16 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/parser/from-collada-to-fcl.hpp"
#include <hpp/fcl/mesh_loader/assimp.h>
namespace se3
{
namespace urdf
{
typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;
inline fcl::CollisionObject retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
const std::vector < std::string > & package_dirs,
std::string & mesh_path)
......@@ -45,12 +49,15 @@
mesh_path = convertURDFMeshPathToAbsolutePath(collisionFilename, package_dirs);
::urdf::Vector3 scale = collisionGeometry->scale;
fcl::Vec3f scale = fcl::Vec3f(collisionGeometry->scale.x,
collisionGeometry->scale.y,
collisionGeometry->scale.z
);
// Create FCL mesh by parsing Collada file.
Polyhedron_ptr polyhedron (new PolyhedronType);
PolyhedronPtrType polyhedron (new PolyhedronType);
loadPolyhedronFromResource (mesh_path, scale, polyhedron);
fcl::loadPolyhedronFromResource (mesh_path, scale, polyhedron);
geometry = polyhedron;
}
......
......@@ -68,7 +68,7 @@ IF(URDFDOM_FOUND)
ADD_UNIT_TEST(value "eigen3;urdfdom")
ADD_TEST_CFLAGS(value '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
IF(HPP_FCL_FOUND)
ADD_UNIT_TEST(geom "eigen3;urdfdom;assimp;hpp-fcl")
ADD_UNIT_TEST(geom "eigen3;urdfdom;hpp-fcl")
ADD_TEST_CFLAGS(geom '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
ADD_TEST_CFLAGS(geom "-DWITH_HPP_FCL")
IF(BUILD_TESTS_WITH_HPP)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment