diff --git a/include/hpp/fcl/collision_utility.h b/include/hpp/fcl/collision_utility.h index 4dc889fe4..b231973ec 100644 --- a/include/hpp/fcl/collision_utility.h +++ b/include/hpp/fcl/collision_utility.h @@ -1,4 +1,5 @@ -// Copyright (c) 2017, Joseph Mirabel +// Copyright (c) 2017 CNRS +// Copyright (c) 2022 INRIA // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // // This file is part of hpp-fcl. @@ -21,11 +22,38 @@ namespace hpp { namespace fcl { + HPP_FCL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb); + +/** + * \brief Returns the name associated to a NODE_TYPE + */ +inline const char* get_node_type_name(NODE_TYPE node_type) { + static const char* node_type_name_all[] = { + "BV_UNKNOWN", "BV_AABB", "BV_OBB", "BV_RSS", + "BV_kIOS", "BV_OBBRSS", "BV_KDOP16", "BV_KDOP18", + "BV_KDOP24", "GEOM_BOX", "GEOM_SPHERE", "GEOM_CAPSULE", + "GEOM_CONE", "GEOM_CYLINDER", "GEOM_CONVEX", "GEOM_PLANE", + "GEOM_HALFSPACE", "GEOM_TRIANGLE", "GEOM_OCTREE", "GEOM_ELLIPSOID", + "HF_AABB", "HF_OBBRSS", "NODE_COUNT"}; + + return node_type_name_all[node_type]; } +/** + * \brief Returns the name associated to a OBJECT_TYPE + */ +inline const char* get_object_type_name(OBJECT_TYPE object_type) { + static const char* object_type_name_all[] = { + "OT_UNKNOWN", "OT_BVH", "OT_GEOM", "OT_OCTREE", "OT_HFIELD", "OT_COUNT"}; + + return object_type_name_all[object_type]; +} + +} // namespace fcl + } // namespace hpp #endif // FCL_COLLISION_UTILITY_H diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h index eacd611ea..44a50f0f0 100644 --- a/include/hpp/fcl/hfield.h +++ b/include/hpp/fcl/hfield.h @@ -193,7 +193,8 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { /// \param[in] x_dim Dimension along the X axis /// \param[in] y_dim Dimension along the Y axis /// \param[in] heights Matrix containing the altitude of each point compositng - /// the height field \param[in] min_height Minimal height of the height field + /// the height field + /// \param[in] min_height Minimal height of the height field /// HeightField(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights, const FCL_REAL min_height = (FCL_REAL)0) diff --git a/src/collision.cpp b/src/collision.cpp index 773a43326..296c09bb8 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -36,6 +36,7 @@ /** \author Jia Pan */ #include +#include #include #include @@ -97,8 +98,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.collision_matrix[node_type2][node_type1]) { HPP_FCL_THROW_PRETTY("Collision function between node type " - << node_type1 << " and node type " - << node_type2 << " is not supported.", + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", std::invalid_argument); res = 0; } else { @@ -109,8 +112,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, } else { if (!looktable.collision_matrix[node_type1][node_type2]) { HPP_FCL_THROW_PRETTY("Collision function between node type " - << node_type1 << " and node type " - << node_type2 << " is not supported.", + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", std::invalid_argument); res = 0; } else @@ -141,10 +146,12 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1, if ((swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) || (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) { - std::ostringstream oss; - oss << "Warning: collision function between node type " << node_type1 - << " and node type " << node_type2 << " is not supported"; - throw std::invalid_argument(oss.str()); + HPP_FCL_THROW_PRETTY("Collision function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } if (swap_geoms) func = looktable.collision_matrix[node_type2][node_type1]; diff --git a/src/distance.cpp b/src/distance.cpp index 3cd4bb8d7..04ea309c4 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -36,6 +36,7 @@ /** \author Jia Pan */ #include +#include #include #include @@ -78,9 +79,12 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.distance_matrix[node_type2][node_type1]) { - std::cerr << "Warning: distance function between node type " << node_type1 - << " and node type " << node_type2 << " is not supported" - << std::endl; + HPP_FCL_THROW_PRETTY("Distance function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } else { res = looktable.distance_matrix[node_type2][node_type1]( o2, tf2, o1, tf1, &solver, request, result); @@ -96,9 +100,12 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, } } else { if (!looktable.distance_matrix[node_type1][node_type2]) { - std::cerr << "Warning: distance function between node type " << node_type1 - << " and node type " << node_type2 << " is not supported" - << std::endl; + HPP_FCL_THROW_PRETTY("Distance function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } else { res = looktable.distance_matrix[node_type1][node_type2]( o1, tf1, o2, tf2, &solver, request, result); @@ -127,10 +134,12 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1, if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) || (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) { - std::ostringstream oss; - oss << "Warning: distance function between node type " << node_type1 - << " and node type " << node_type2 << " is not supported"; - throw std::invalid_argument(oss.str()); + HPP_FCL_THROW_PRETTY("Distance function between node type " + << std::string(get_node_type_name(node_type1)) + << " and node type " + << std::string(get_node_type_name(node_type2)) + << " is not yet supported.", + std::invalid_argument); } if (swap_geoms) func = looktable.distance_matrix[node_type2][node_type1];