Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add enum helpers #290

Merged
merged 2 commits into from
May 9, 2022
Merged

Add enum helpers #290

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b20a575
core: add helper for enum quantities and display better throw message
jcarpent May 4, 2022
9aeb12a
doc: tiny fix
jcarpent May 4, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 29 additions & 1 deletion include/hpp/fcl/collision_utility.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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) {
jcarpent marked this conversation as resolved.
Show resolved Hide resolved
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
3 changes: 2 additions & 1 deletion include/hpp/fcl/hfield.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
23 changes: 15 additions & 8 deletions src/collision.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
/** \author Jia Pan */

#include <hpp/fcl/collision.h>
#include <hpp/fcl/collision_utility.h>
#include <hpp/fcl/collision_func_matrix.h>
#include <hpp/fcl/narrowphase/narrowphase.h>

Expand Down Expand Up @@ -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 {
Expand All @@ -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
Expand Down Expand Up @@ -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];
Expand Down
29 changes: 19 additions & 10 deletions src/distance.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
/** \author Jia Pan */

#include <hpp/fcl/distance.h>
#include <hpp/fcl/collision_utility.h>
#include <hpp/fcl/distance_func_matrix.h>
#include <hpp/fcl/narrowphase/narrowphase.h>

Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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];
Expand Down