From 339f1a0bd626d99c564c6d13d4f3ec1eaa37dfd0 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 18 Jun 2024 17:23:29 +0200 Subject: [PATCH 01/57] all: change `hpp::fcl::` namespace to `coal::` --- include/hpp/fcl/BV/AABB.h | 11 +- include/hpp/fcl/BV/BV.h | 11 +- include/hpp/fcl/BV/BV_node.h | 11 +- include/hpp/fcl/BV/OBB.h | 11 +- include/hpp/fcl/BV/OBBRSS.h | 11 +- include/hpp/fcl/BV/RSS.h | 11 +- include/hpp/fcl/BV/kDOP.h | 11 +- include/hpp/fcl/BV/kIOS.h | 11 +- include/hpp/fcl/BVH/BVH_front.h | 11 +- include/hpp/fcl/BVH/BVH_internal.h | 11 +- include/hpp/fcl/BVH/BVH_model.h | 11 +- include/hpp/fcl/BVH/BVH_utility.h | 11 +- include/hpp/fcl/broadphase/broadphase.h | 6 +- include/hpp/fcl/broadphase/broadphase_SSaP.h | 10 +- include/hpp/fcl/broadphase/broadphase_SaP.h | 10 +- .../fcl/broadphase/broadphase_bruteforce.h | 11 +- .../hpp/fcl/broadphase/broadphase_callbacks.h | 12 +- .../broadphase/broadphase_collision_manager.h | 10 +- ...adphase_continuous_collision_manager-inl.h | 11 +- .../broadphase_continuous_collision_manager.h | 11 +- .../broadphase_dynamic_AABB_tree-inl.h | 10 +- .../broadphase/broadphase_dynamic_AABB_tree.h | 11 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 10 +- .../broadphase_dynamic_AABB_tree_array.h | 11 +- .../fcl/broadphase/broadphase_interval_tree.h | 11 +- .../broadphase/broadphase_spatialhash-inl.h | 11 +- .../fcl/broadphase/broadphase_spatialhash.h | 11 +- .../broadphase/default_broadphase_callbacks.h | 13 +- .../broadphase/detail/hierarchy_tree-inl.h | 10 +- .../fcl/broadphase/detail/hierarchy_tree.h | 10 +- .../detail/hierarchy_tree_array-inl.h | 10 +- .../broadphase/detail/hierarchy_tree_array.h | 10 +- .../hpp/fcl/broadphase/detail/interval_tree.h | 10 +- .../detail/interval_tree_node-inl.h | 10 +- .../broadphase/detail/interval_tree_node.h | 10 +- .../hpp/fcl/broadphase/detail/morton-inl.h | 10 +- include/hpp/fcl/broadphase/detail/morton.h | 10 +- .../hpp/fcl/broadphase/detail/node_base-inl.h | 10 +- include/hpp/fcl/broadphase/detail/node_base.h | 10 +- .../broadphase/detail/node_base_array-inl.h | 10 +- .../fcl/broadphase/detail/node_base_array.h | 10 +- .../broadphase/detail/simple_hash_table-inl.h | 10 +- .../fcl/broadphase/detail/simple_hash_table.h | 10 +- .../broadphase/detail/simple_interval-inl.h | 10 +- .../fcl/broadphase/detail/simple_interval.h | 10 +- .../broadphase/detail/sparse_hash_table-inl.h | 10 +- .../fcl/broadphase/detail/sparse_hash_table.h | 10 +- .../fcl/broadphase/detail/spatial_hash-inl.h | 10 +- .../hpp/fcl/broadphase/detail/spatial_hash.h | 10 +- include/hpp/fcl/collision.h | 10 +- include/hpp/fcl/collision_data.h | 11 +- include/hpp/fcl/collision_func_matrix.h | 11 +- include/hpp/fcl/collision_object.h | 11 +- include/hpp/fcl/collision_utility.h | 13 +- include/hpp/fcl/contact_patch.h | 10 +- .../fcl/contact_patch/contact_patch_solver.h | 12 +- .../contact_patch/contact_patch_solver.hxx | 12 +- include/hpp/fcl/contact_patch_func_matrix.h | 10 +- include/hpp/fcl/data_types.h | 14 +- include/hpp/fcl/distance.h | 10 +- include/hpp/fcl/distance_func_matrix.h | 11 +- include/hpp/fcl/doc.hh | 6 +- include/hpp/fcl/fwd.hh | 12 +- include/hpp/fcl/hfield.h | 13 +- include/hpp/fcl/internal/BV_fitter.h | 11 +- include/hpp/fcl/internal/BV_splitter.h | 11 +- .../internal/shape_shape_contact_patch_func.h | 10 +- include/hpp/fcl/internal/shape_shape_func.h | 12 +- include/hpp/fcl/internal/tools.h | 10 +- include/hpp/fcl/internal/traversal.h | 11 +- .../hpp/fcl/internal/traversal_node_base.h | 11 +- .../fcl/internal/traversal_node_bvh_hfield.h | 11 +- .../fcl/internal/traversal_node_bvh_shape.h | 10 +- .../hpp/fcl/internal/traversal_node_bvhs.h | 11 +- .../internal/traversal_node_hfield_shape.h | 10 +- .../hpp/fcl/internal/traversal_node_octree.h | 13 +- .../hpp/fcl/internal/traversal_node_setup.h | 10 +- .../hpp/fcl/internal/traversal_node_shapes.h | 11 +- include/hpp/fcl/internal/traversal_recurse.h | 11 +- include/hpp/fcl/logging.h | 4 +- include/hpp/fcl/math/matrix_3f.h | 4 +- include/hpp/fcl/math/transform.h | 10 +- include/hpp/fcl/math/types.h | 6 +- include/hpp/fcl/math/vec_3f.h | 4 +- include/hpp/fcl/mesh_loader/assimp.h | 24 ++- include/hpp/fcl/mesh_loader/loader.h | 13 +- include/hpp/fcl/narrowphase/gjk.h | 11 +- .../fcl/narrowphase/minkowski_difference.h | 12 +- include/hpp/fcl/narrowphase/narrowphase.h | 10 +- .../fcl/narrowphase/narrowphase_defaults.h | 12 +- .../hpp/fcl/narrowphase/support_functions.h | 12 +- include/hpp/fcl/octree.h | 11 +- include/hpp/fcl/serialization/AABB.h | 7 +- include/hpp/fcl/serialization/BVH_model.h | 64 ++++---- include/hpp/fcl/serialization/BV_node.h | 12 +- include/hpp/fcl/serialization/BV_splitter.h | 18 +-- include/hpp/fcl/serialization/OBB.h | 8 +- include/hpp/fcl/serialization/OBBRSS.h | 9 +- include/hpp/fcl/serialization/RSS.h | 8 +- include/hpp/fcl/serialization/archive.h | 12 +- .../hpp/fcl/serialization/collision_data.h | 73 +++++---- .../hpp/fcl/serialization/collision_object.h | 18 +-- include/hpp/fcl/serialization/contact_patch.h | 18 +-- include/hpp/fcl/serialization/convex.h | 40 +++-- include/hpp/fcl/serialization/eigen.h | 6 +- include/hpp/fcl/serialization/fwd.h | 16 +- .../hpp/fcl/serialization/geometric_shapes.h | 87 ++++++----- include/hpp/fcl/serialization/hfield.h | 27 ++-- include/hpp/fcl/serialization/kDOP.h | 12 +- include/hpp/fcl/serialization/kIOS.h | 25 ++-- include/hpp/fcl/serialization/memory.h | 12 +- include/hpp/fcl/serialization/octree.h | 30 ++-- include/hpp/fcl/serialization/quadrilateral.h | 8 +- include/hpp/fcl/serialization/serializer.h | 36 +++-- include/hpp/fcl/serialization/transform.h | 8 +- include/hpp/fcl/serialization/triangle.h | 8 +- include/hpp/fcl/shape/convex.h | 11 +- include/hpp/fcl/shape/details/convex.hxx | 11 +- .../fcl/shape/geometric_shape_to_BVH_model.h | 11 +- include/hpp/fcl/shape/geometric_shapes.h | 11 +- .../hpp/fcl/shape/geometric_shapes_traits.h | 12 +- .../hpp/fcl/shape/geometric_shapes_utility.h | 11 +- include/hpp/fcl/timings.h | 12 +- python/broadphase/broadphase.cc | 2 +- python/broadphase/broadphase_callbacks.hh | 12 +- .../broadphase_collision_manager.hh | 12 +- python/broadphase/fwd.hh | 12 +- python/collision-geometries.cc | 10 +- python/collision.cc | 4 +- python/contact_patch.cc | 4 +- python/deprecation.hh | 12 +- python/distance.cc | 4 +- python/fcl.cc | 2 +- python/fwd.hh | 6 +- python/gjk.cc | 10 +- python/math.cc | 8 +- python/octree.cc | 4 +- python/pickle.hh | 8 +- python/serializable.hh | 14 +- python/utils/std-pair.hh | 6 +- src/BV/AABB.cpp | 7 +- src/BV/OBB.cpp | 7 +- src/BV/OBB.h | 13 +- src/BV/OBBRSS.cpp | 7 +- src/BV/RSS.cpp | 7 +- src/BV/kDOP.cpp | 7 +- src/BV/kIOS.cpp | 7 +- src/BVH/BVH_model.cpp | 7 +- src/BVH/BVH_utility.cpp | 7 +- src/BVH/BV_fitter.cpp | 7 +- src/BVH/BV_splitter.cpp | 7 +- src/broadphase/broadphase_SSaP.cpp | 6 +- src/broadphase/broadphase_SaP.cpp | 7 +- src/broadphase/broadphase_bruteforce.cpp | 6 +- .../broadphase_collision_manager.cpp | 6 +- .../broadphase_dynamic_AABB_tree.cpp | 6 +- .../broadphase_dynamic_AABB_tree_array.cpp | 6 +- src/broadphase/broadphase_interval_tree.cpp | 6 +- .../default_broadphase_callbacks.cpp | 6 +- src/broadphase/detail/interval_tree.cpp | 10 +- src/broadphase/detail/interval_tree_node.cpp | 10 +- src/broadphase/detail/morton-inl.h | 10 +- src/broadphase/detail/morton.cpp | 6 +- src/broadphase/detail/simple_interval.cpp | 10 +- src/broadphase/detail/spatial_hash.cpp | 10 +- src/collision.cpp | 6 +- src/collision_data.cpp | 6 +- src/collision_func_matrix.cpp | 15 +- src/collision_node.cpp | 7 +- src/collision_node.h | 7 +- src/collision_object.cpp | 7 +- src/collision_utility.cpp | 7 +- src/contact_patch.cpp | 6 +- src/contact_patch/contact_patch_solver.cpp | 6 +- src/contact_patch_func_matrix.cpp | 6 +- src/distance.cpp | 6 +- src/distance/box_halfspace.cpp | 6 +- src/distance/box_plane.cpp | 6 +- src/distance/box_sphere.cpp | 6 +- src/distance/capsule_capsule.cpp | 20 ++- src/distance/capsule_halfspace.cpp | 6 +- src/distance/capsule_plane.cpp | 6 +- src/distance/cone_halfspace.cpp | 6 +- src/distance/cone_plane.cpp | 6 +- src/distance/convex_halfspace.cpp | 6 +- src/distance/convex_plane.cpp | 6 +- src/distance/cylinder_halfspace.cpp | 6 +- src/distance/cylinder_plane.cpp | 6 +- src/distance/ellipsoid_halfspace.cpp | 6 +- src/distance/ellipsoid_plane.cpp | 6 +- src/distance/halfspace_halfspace.cpp | 6 +- src/distance/halfspace_plane.cpp | 6 +- src/distance/plane_plane.cpp | 6 +- src/distance/sphere_capsule.cpp | 6 +- src/distance/sphere_cylinder.cpp | 7 +- src/distance/sphere_halfspace.cpp | 7 +- src/distance/sphere_plane.cpp | 6 +- src/distance/sphere_sphere.cpp | 6 +- src/distance/triangle_halfspace.cpp | 6 +- src/distance/triangle_plane.cpp | 6 +- src/distance/triangle_sphere.cpp | 6 +- src/distance/triangle_triangle.cpp | 8 +- src/distance_func_matrix.cpp | 11 +- src/hfield.cpp | 7 +- src/intersect.cpp | 7 +- src/math/transform.cpp | 7 +- src/mesh_loader/assimp.cpp | 18 +-- src/mesh_loader/loader.cpp | 9 +- src/narrowphase/details.h | 16 +- src/narrowphase/gjk.cpp | 23 ++- src/narrowphase/minkowski_difference.cpp | 6 +- src/narrowphase/support_functions.cpp | 6 +- src/octree.cpp | 6 +- src/serialization/serialization.cpp | 2 +- src/shape/convex.cpp | 6 +- src/shape/geometric_shapes.cpp | 7 +- src/shape/geometric_shapes_utility.cpp | 7 +- src/traits_traversal.h | 7 +- src/traversal/traversal_recurse.cpp | 7 +- test/accelerated_gjk.cpp | 32 ++-- test/benchmark.cpp | 2 +- test/box_box_collision.cpp | 16 +- test/box_box_distance.cpp | 59 ++++---- test/broadphase.cpp | 4 +- test/broadphase_collision_1.cpp | 2 +- test/broadphase_collision_2.cpp | 2 +- test/broadphase_dynamic_AABB_tree.cpp | 2 +- test/bvh_models.cpp | 4 +- test/capsule_box_1.cpp | 34 ++--- test/capsule_box_2.cpp | 27 ++-- test/capsule_capsule.cpp | 2 +- test/collision.cpp | 2 +- test/collision_node_asserts.cpp | 2 +- test/contact_patch.cpp | 140 +++++++++--------- test/convex.cpp | 2 +- test/distance.cpp | 2 +- test/distance_lower_bound.cpp | 44 +++--- test/frontlist.cpp | 2 +- test/geometric_shapes.cpp | 22 ++- test/gjk.cpp | 28 ++-- test/gjk_asserts.cpp | 2 +- test/gjk_convergence_criterion.cpp | 22 +-- test/hfields.cpp | 2 +- test/math.cpp | 2 +- test/normal_and_nearest_points.cpp | 8 +- test/obb.cpp | 2 +- test/octree.cpp | 55 ++++--- test/profiling.cpp | 10 +- test/security_margin.cpp | 50 +++---- test/serialization.cpp | 40 ++--- test/shape_inflation.cpp | 34 ++--- test/shape_mesh_consistency.cpp | 2 +- test/swept_sphere_radius.cpp | 6 +- test/utility.cpp | 9 +- test/utility.h | 13 +- 255 files changed, 1274 insertions(+), 1750 deletions(-) diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h index bdcc8de64..4f228416f 100644 --- a/include/hpp/fcl/BV/AABB.h +++ b/include/hpp/fcl/BV/AABB.h @@ -35,13 +35,12 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_AABB_H -#define HPP_FCL_AABB_H +#ifndef COAL_AABB_H +#define COAL_AABB_H #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { struct CollisionRequest; class Plane; @@ -262,8 +261,6 @@ HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h index b42316ea0..1dad8f132 100644 --- a/include/hpp/fcl/BV/BV.h +++ b/include/hpp/fcl/BV/BV.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BV_H -#define HPP_FCL_BV_H +#ifndef COAL_BV_H +#define COAL_BV_H #include #include @@ -47,8 +47,7 @@ #include /** @brief Main namespace */ -namespace hpp { -namespace fcl { +namespace coal { /// @cond IGNORE namespace details { @@ -285,8 +284,6 @@ static inline void convertBV(const BV1& bv1, BV2& bv2) { details::Converter::convert(bv1, bv2); } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h index 300b24ecc..86cab1036 100644 --- a/include/hpp/fcl/BV/BV_node.h +++ b/include/hpp/fcl/BV/BV_node.h @@ -35,14 +35,13 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BV_NODE_H -#define HPP_FCL_BV_NODE_H +#ifndef COAL_BV_NODE_H +#define COAL_BV_NODE_H #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @defgroup Construction_Of_BVH Construction of BVHModel /// Classes which are used to build a BVHModel (Bounding Volume Hierarchy) @@ -162,8 +161,6 @@ inline const Matrix3f& BVNode::getOrientation() const { return bv.obb.axes; } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h index a8e11a006..b47bef7d3 100644 --- a/include/hpp/fcl/BV/OBB.h +++ b/include/hpp/fcl/BV/OBB.h @@ -35,13 +35,12 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_OBB_H -#define HPP_FCL_OBB_H +#ifndef COAL_OBB_H +#define COAL_OBB_H #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { struct CollisionRequest; @@ -146,8 +145,6 @@ HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, /// The second box is in identity configuration. HPP_FCL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h index d224ee162..5529ec95b 100644 --- a/include/hpp/fcl/BV/OBBRSS.h +++ b/include/hpp/fcl/BV/OBBRSS.h @@ -35,14 +35,13 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_OBBRSS_H -#define HPP_FCL_OBBRSS_H +#ifndef COAL_OBBRSS_H +#define COAL_OBBRSS_H #include "hpp/fcl/BV/OBB.h" #include "hpp/fcl/BV/RSS.h" -namespace hpp { -namespace fcl { +namespace coal { struct CollisionRequest; @@ -155,8 +154,6 @@ inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, return distance(R0, T0, b1.rss, b2.rss, P, Q); } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h index 734e20a74..2250dac5b 100644 --- a/include/hpp/fcl/BV/RSS.h +++ b/include/hpp/fcl/BV/RSS.h @@ -35,15 +35,14 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_RSS_H -#define HPP_FCL_RSS_H +#ifndef COAL_RSS_H +#define COAL_RSS_H #include "hpp/fcl/data_types.h" #include -namespace hpp { -namespace fcl { +namespace coal { struct CollisionRequest; @@ -169,8 +168,6 @@ HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h index 879bff1f1..7186a9213 100644 --- a/include/hpp/fcl/BV/kDOP.h +++ b/include/hpp/fcl/BV/kDOP.h @@ -35,14 +35,13 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_KDOP_H -#define HPP_FCL_KDOP_H +#ifndef COAL_KDOP_H +#define COAL_KDOP_H #include "hpp/fcl/fwd.hh" #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { struct CollisionRequest; @@ -186,8 +185,6 @@ bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, template HPP_FCL_DLLAPI KDOP translate(const KDOP& bv, const Vec3f& t); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h index 7234b7740..e96608d6e 100644 --- a/include/hpp/fcl/BV/kIOS.h +++ b/include/hpp/fcl/BV/kIOS.h @@ -35,13 +35,12 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_KIOS_H -#define HPP_FCL_KIOS_H +#ifndef COAL_KIOS_H +#define COAL_KIOS_H #include "hpp/fcl/BV/OBB.h" -namespace hpp { -namespace fcl { +namespace coal { struct CollisionRequest; @@ -188,8 +187,6 @@ HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BVH/BVH_front.h b/include/hpp/fcl/BVH/BVH_front.h index 9a3a99eea..3ec1522d1 100644 --- a/include/hpp/fcl/BVH/BVH_front.h +++ b/include/hpp/fcl/BVH/BVH_front.h @@ -35,15 +35,14 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BVH_FRONT_H -#define HPP_FCL_BVH_FRONT_H +#ifndef COAL_BVH_FRONT_H +#define COAL_BVH_FRONT_H #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Front list acceleration for collision /// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where @@ -72,8 +71,6 @@ inline void updateFrontList(BVHFrontList* front_list, unsigned int b1, if (front_list) front_list->push_back(BVHFrontNode(b1, b2)); } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BVH/BVH_internal.h b/include/hpp/fcl/BVH/BVH_internal.h index 407a430ca..892911959 100644 --- a/include/hpp/fcl/BVH/BVH_internal.h +++ b/include/hpp/fcl/BVH/BVH_internal.h @@ -35,13 +35,12 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BVH_INTERNAL_H -#define HPP_FCL_BVH_INTERNAL_H +#ifndef COAL_BVH_INTERNAL_H +#define COAL_BVH_INTERNAL_H #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief States for BVH construction /// empty->begun->processed ->replace_begun->processed -> ...... @@ -83,8 +82,6 @@ enum BVHModelType { BVH_MODEL_POINTCLOUD /// @brief point cloud model }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h index 34186d0a6..16ae11e67 100644 --- a/include/hpp/fcl/BVH/BVH_model.h +++ b/include/hpp/fcl/BVH/BVH_model.h @@ -36,8 +36,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BVH_MODEL_H -#define HPP_FCL_BVH_MODEL_H +#ifndef COAL_BVH_MODEL_H +#define COAL_BVH_MODEL_H #include "hpp/fcl/fwd.hh" #include "hpp/fcl/collision_object.h" @@ -48,8 +48,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Construction_Of_BVH /// @{ @@ -535,8 +534,6 @@ NODE_TYPE BVHModel>::getNodeType() const; template <> NODE_TYPE BVHModel>::getNodeType() const; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h index 508af2fec..27093fb37 100644 --- a/include/hpp/fcl/BVH/BVH_utility.h +++ b/include/hpp/fcl/BVH/BVH_utility.h @@ -35,13 +35,12 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BVH_UTILITY_H -#define HPP_FCL_BVH_UTILITY_H +#ifndef COAL_BVH_UTILITY_H +#define COAL_BVH_UTILITY_H #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Extract the part of the BVHModel that is inside an AABB. /// A triangle in collision with the AABB is considered inside. template @@ -112,8 +111,6 @@ HPP_FCL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Vec3f& query); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/hpp/fcl/broadphase/broadphase.h index 762792de3..fafd6f954 100644 --- a/include/hpp/fcl/broadphase/broadphase.h +++ b/include/hpp/fcl/broadphase/broadphase.h @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef HPP_FCL_BROADPHASE_BROADPHASE_H -#define HPP_FCL_BROADPHASE_BROADPHASE_H +#ifndef COAL_BROADPHASE_BROADPHASE_H +#define COAL_BROADPHASE_BROADPHASE_H #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" @@ -45,4 +45,4 @@ #include "hpp/fcl/broadphase/default_broadphase_callbacks.h" -#endif // ifndef HPP_FCL_BROADPHASE_BROADPHASE_H +#endif // ifndef COAL_BROADPHASE_BROADPHASE_H diff --git a/include/hpp/fcl/broadphase/broadphase_SSaP.h b/include/hpp/fcl/broadphase/broadphase_SSaP.h index b99eb90c8..d3511c5f0 100644 --- a/include/hpp/fcl/broadphase/broadphase_SSaP.h +++ b/include/hpp/fcl/broadphase/broadphase_SSaP.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_SSAP_H -#define HPP_FCL_BROAD_PHASE_SSAP_H +#ifndef COAL_BROAD_PHASE_SSAP_H +#define COAL_BROAD_PHASE_SSAP_H #include #include "hpp/fcl/broadphase/broadphase_collision_manager.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Simple SAP collision manager class HPP_FCL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { @@ -142,7 +141,6 @@ class HPP_FCL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { bool setup_; }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h index a71f78ad4..8ae3f48f3 100644 --- a/include/hpp/fcl/broadphase/broadphase_SaP.h +++ b/include/hpp/fcl/broadphase/broadphase_SaP.h @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_SAP_H -#define HPP_FCL_BROAD_PHASE_SAP_H +#ifndef COAL_BROAD_PHASE_SAP_H +#define COAL_BROAD_PHASE_SAP_H #include #include #include "hpp/fcl/broadphase/broadphase_collision_manager.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Rigorous SAP collision manager class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { @@ -220,7 +219,6 @@ class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { void removeFromOverlapPairs(const SaPPair& p); }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_bruteforce.h b/include/hpp/fcl/broadphase/broadphase_bruteforce.h index 734c74fb7..ac5dce5ee 100644 --- a/include/hpp/fcl/broadphase/broadphase_bruteforce.h +++ b/include/hpp/fcl/broadphase/broadphase_bruteforce.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_BRUTE_FORCE_H -#define HPP_FCL_BROAD_PHASE_BRUTE_FORCE_H +#ifndef COAL_BROAD_PHASE_BRUTE_FORCE_H +#define COAL_BROAD_PHASE_BRUTE_FORCE_H #include #include "hpp/fcl/broadphase/broadphase_collision_manager.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Brute force N-body collision manager class HPP_FCL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { @@ -108,8 +107,6 @@ class HPP_FCL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { std::list objs; }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_callbacks.h b/include/hpp/fcl/broadphase/broadphase_callbacks.h index 8a7d6b0ff..d63318dbf 100644 --- a/include/hpp/fcl/broadphase/broadphase_callbacks.h +++ b/include/hpp/fcl/broadphase/broadphase_callbacks.h @@ -34,14 +34,13 @@ /** @author Justin Carpentier (justin.carpentier@inria.fr) */ -#ifndef HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H -#define HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H +#ifndef COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H +#define COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H #include "hpp/fcl/fwd.hh" #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Base callback class for collision queries. /// This class can be supersed by child classes to provide desired behaviors @@ -92,7 +91,6 @@ struct HPP_FCL_DLLAPI DistanceCallBackBase { } }; -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_BROADPHASE_BROAD_PHASE_CALLBACKS_H +#endif // COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H diff --git a/include/hpp/fcl/broadphase/broadphase_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_collision_manager.h index 750907b28..76ad5d4b1 100644 --- a/include/hpp/fcl/broadphase/broadphase_collision_manager.h +++ b/include/hpp/fcl/broadphase/broadphase_collision_manager.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H -#define HPP_FCL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H +#ifndef COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H +#define COAL_BROADPHASE_BROADPHASECOLLISIONMANAGER_H #include #include @@ -45,8 +45,7 @@ #include "hpp/fcl/collision_object.h" #include "hpp/fcl/broadphase/broadphase_callbacks.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Base class for broad phase collision. It helps to accelerate the /// collision/distance between N objects. Also support self collision, self @@ -135,7 +134,6 @@ class HPP_FCL_DLLAPI BroadPhaseCollisionManager { void insertTestedSet(CollisionObject* a, CollisionObject* b) const; }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h index 93691b200..93c038aee 100644 --- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H -#define HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H +#ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H +#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H #include "hpp/fcl/broadphase/broadphase_continuous_collision_manager.h" #include -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== BroadPhaseContinuousCollisionManager::BroadPhaseContinuousCollisionManager() { @@ -80,8 +79,6 @@ void BroadPhaseContinuousCollisionManager::update( update(); } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h index 79bfe6aa6..4dd102db5 100644 --- a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h @@ -35,15 +35,14 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H -#define HPP_FCL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H +#ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H +#define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H #include "hpp/fcl/broadphase/broadphase_collision_manager.h" #include "hpp/fcl/collision_object.h" #include "hpp/fcl/narrowphase/continuous_collision_object.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Callback for continuous collision between two objects. Return value /// is whether can stop now. @@ -137,9 +136,7 @@ using BroadPhaseContinuousCollisionManagerf = using BroadPhaseContinuousCollisionManagerd = BroadPhaseContinuousCollisionManager; -} // namespace fcl - -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h" diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h index 0ef013214..45a114c79 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" @@ -49,8 +49,7 @@ #include "hpp/fcl/BV/BV.h" #include "hpp/fcl/shape/geometric_shapes_utility.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { namespace dynamic_AABB_tree { @@ -230,7 +229,6 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, } // namespace dynamic_AABB_tree } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h index 10b4341c3..deabdf4bd 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_H #include #include @@ -48,8 +48,7 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" #include "hpp/fcl/broadphase/detail/hierarchy_tree.h" -namespace hpp { -namespace fcl { +namespace coal { class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager { @@ -144,9 +143,7 @@ class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager void update_(CollisionObject* updated_obj); }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h" diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 439f93b11..b12283592 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "hpp/fcl/shape/geometric_shapes_utility.h" @@ -45,8 +45,7 @@ #include "hpp/fcl/octree.h" #endif -namespace hpp { -namespace fcl { +namespace coal { namespace detail { namespace dynamic_AABB_tree_array { @@ -229,7 +228,6 @@ bool distanceRecurse_( } // namespace dynamic_AABB_tree_array } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index e4756300b..cb1c24a91 100644 --- a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H -#define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H +#ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H +#define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H #include #include @@ -49,8 +49,7 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" #include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" -namespace hpp { -namespace fcl { +namespace coal { class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager : public BroadPhaseCollisionManager { @@ -140,9 +139,7 @@ class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager void update_(CollisionObject* updated_obj); }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" diff --git a/include/hpp/fcl/broadphase/broadphase_interval_tree.h b/include/hpp/fcl/broadphase/broadphase_interval_tree.h index 78b1037d0..97a1db7b8 100644 --- a/include/hpp/fcl/broadphase/broadphase_interval_tree.h +++ b/include/hpp/fcl/broadphase/broadphase_interval_tree.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H -#define HPP_FCL_BROAD_PHASE_INTERVAL_TREE_H +#ifndef COAL_BROAD_PHASE_INTERVAL_TREE_H +#define COAL_BROAD_PHASE_INTERVAL_TREE_H #include #include @@ -44,8 +44,7 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" #include "hpp/fcl/broadphase/detail/interval_tree.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Collision manager based on interval tree class HPP_FCL_DLLAPI IntervalTreeCollisionManager @@ -165,8 +164,6 @@ class HPP_FCL_DLLAPI IntervalTreeCollisionManager bool setup_; }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h index 29c462c3e..5b0eb7df9 100644 --- a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h @@ -35,13 +35,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H -#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H +#ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H +#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H #include "hpp/fcl/broadphase/broadphase_spatialhash.h" -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== template @@ -535,8 +534,6 @@ bool SpatialHashingCollisionManager::distanceObjectToObjects( return false; } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash.h b/include/hpp/fcl/broadphase/broadphase_spatialhash.h index 606b82ada..5e5b18920 100644 --- a/include/hpp/fcl/broadphase/broadphase_spatialhash.h +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_H -#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_H +#ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_H +#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_H #include #include @@ -46,8 +46,7 @@ #include "hpp/fcl/broadphase/detail/sparse_hash_table.h" #include "hpp/fcl/broadphase/detail/spatial_hash.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief spatial hashing collision mananger template { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h index 54cddd079..baaeb7b3b 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_HIERARCHY_TREE_H -#define HPP_FCL_HIERARCHY_TREE_H +#ifndef COAL_HIERARCHY_TREE_H +#define COAL_HIERARCHY_TREE_H #include #include @@ -47,8 +47,7 @@ #include "hpp/fcl/broadphase/detail/morton.h" #include "hpp/fcl/broadphase/detail/node_base.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -286,8 +285,7 @@ size_t select(const BV& query, const NodeBase& node1, const NodeBase& node2); } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/detail/hierarchy_tree-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h index 8bb715fe9..131b82b87 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H -#define HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H +#ifndef COAL_HIERARCHY_TREE_ARRAY_INL_H +#define COAL_HIERARCHY_TREE_ARRAY_INL_H #include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -974,7 +973,6 @@ struct SelectImpl { } // namespace implementation_array } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h index 8709a848b..d5d9b66ab 100644 --- a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_HIERARCHY_TREE_ARRAY_H -#define HPP_FCL_HIERARCHY_TREE_ARRAY_H +#ifndef COAL_HIERARCHY_TREE_ARRAY_H +#define COAL_HIERARCHY_TREE_ARRAY_H #include #include @@ -47,8 +47,7 @@ #include "hpp/fcl/broadphase/detail/morton.h" #include "hpp/fcl/broadphase/detail/node_base_array.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -294,8 +293,7 @@ size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes); } // namespace implementation_array } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/interval_tree.h b/include/hpp/fcl/broadphase/detail/interval_tree.h index bbcb1fec8..46001856b 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_INTERVAL_TREE_H -#define HPP_FCL_INTERVAL_TREE_H +#ifndef COAL_INTERVAL_TREE_H +#define COAL_INTERVAL_TREE_H #include #include @@ -44,8 +44,7 @@ #include "hpp/fcl/broadphase/detail/interval_tree_node.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { /// @brief Class describes the information needed when we take the @@ -123,7 +122,6 @@ class HPP_FCL_DLLAPI IntervalTree { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h b/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h index b9771ae18..58e01c3fc 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #include "hpp/fcl/broadphase/detail/interval_tree_node.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -90,7 +89,6 @@ void IntervalTreeNode::print(IntervalTreeNode* nil, } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node.h b/include/hpp/fcl/broadphase/detail/interval_tree_node.h index e8a653c15..a6956de2b 100644 --- a/include/hpp/fcl/broadphase/detail/interval_tree_node.h +++ b/include/hpp/fcl/broadphase/detail/interval_tree_node.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H -#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_H +#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H +#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H #include "hpp/fcl/broadphase/detail/simple_interval.h" #include "hpp/fcl/fwd.hh" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -86,7 +85,6 @@ class HPP_FCL_DLLAPI IntervalTreeNode { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/morton-inl.h b/include/hpp/fcl/broadphase/detail/morton-inl.h index 53d7d90f7..80354a76d 100644 --- a/include/hpp/fcl/broadphase/detail/morton-inl.h +++ b/include/hpp/fcl/broadphase/detail/morton-inl.h @@ -36,13 +36,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_MORTON_INL_H -#define HPP_FCL_MORTON_INL_H +#ifndef COAL_MORTON_INL_H +#define COAL_MORTON_INL_H #include "hpp/fcl/broadphase/detail/morton.h" -namespace hpp { -namespace fcl { /// @cond IGNORE +namespace coal { /// @cond IGNORE namespace detail { //============================================================================== @@ -147,7 +146,6 @@ constexpr size_t morton_functor>::bits() { } // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/morton.h b/include/hpp/fcl/broadphase/detail/morton.h index cf9968af4..a43abdf9a 100644 --- a/include/hpp/fcl/broadphase/detail/morton.h +++ b/include/hpp/fcl/broadphase/detail/morton.h @@ -36,15 +36,14 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_MORTON_H -#define HPP_FCL_MORTON_H +#ifndef COAL_MORTON_H +#define COAL_MORTON_H #include "hpp/fcl/BV/AABB.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @cond IGNORE namespace detail { @@ -114,8 +113,7 @@ struct morton_functor> { } // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/detail/morton-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/node_base-inl.h b/include/hpp/fcl/broadphase/detail/node_base-inl.h index d7a69680c..a4b30052b 100644 --- a/include/hpp/fcl/broadphase/detail/node_base-inl.h +++ b/include/hpp/fcl/broadphase/detail/node_base-inl.h @@ -35,13 +35,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASE_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASE_INL_H +#ifndef COAL_BROADPHASE_DETAIL_NODEBASE_INL_H +#define COAL_BROADPHASE_DETAIL_NODEBASE_INL_H #include "hpp/fcl/broadphase/detail/node_base.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================// @@ -71,7 +70,6 @@ NodeBase::NodeBase() { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/node_base.h b/include/hpp/fcl/broadphase/detail/node_base.h index 5937b9cef..33ba47892 100644 --- a/include/hpp/fcl/broadphase/detail/node_base.h +++ b/include/hpp/fcl/broadphase/detail/node_base.h @@ -35,13 +35,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASE_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASE_H +#ifndef COAL_BROADPHASE_DETAIL_NODEBASE_H +#define COAL_BROADPHASE_DETAIL_NODEBASE_H #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -73,8 +72,7 @@ struct NodeBase { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/detail/node_base-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h index 0ee3e5f05..b2f4f6ba2 100644 --- a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h +++ b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h @@ -35,13 +35,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H +#ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H +#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H #include "hpp/fcl/broadphase/detail/node_base_array.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -61,7 +60,6 @@ bool NodeBase::isInternal() const { } // namespace implementation_array } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/node_base_array.h b/include/hpp/fcl/broadphase/detail/node_base_array.h index ecf8dc01d..814b401e6 100644 --- a/include/hpp/fcl/broadphase/detail/node_base_array.h +++ b/include/hpp/fcl/broadphase/detail/node_base_array.h @@ -35,13 +35,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H -#define HPP_FCL_BROADPHASE_DETAIL_NODEBASEARRAY_H +#ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H +#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -69,8 +68,7 @@ struct NodeBase { } // namespace implementation_array } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/detail/node_base_array-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h index de51472a2..9922982e5 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H -#define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_INL_H +#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H +#define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H #include "hpp/fcl/broadphase/detail/simple_hash_table.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -107,7 +106,6 @@ void SimpleHashTable::clear() { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/hpp/fcl/broadphase/detail/simple_hash_table.h index ed1f0fcd8..8c480a3fb 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table.h @@ -35,15 +35,14 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H -#define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H +#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_H +#define COAL_BROADPHASE_SIMPLEHASHTABLE_H #include #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -81,8 +80,7 @@ class SimpleHashTable { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/detail/simple_hash_table-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h index 78719156c..4625c31e6 100644 --- a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H +#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H +#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #include "hpp/fcl/broadphase/detail/simple_interval.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -56,7 +55,6 @@ void SimpleInterval::print() { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/simple_interval.h b/include/hpp/fcl/broadphase/detail/simple_interval.h index 0f1976ebf..d94f81e4d 100644 --- a/include/hpp/fcl/broadphase/detail/simple_interval.h +++ b/include/hpp/fcl/broadphase/detail/simple_interval.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H -#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H +#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H +#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H #include "hpp/fcl/fwd.hh" #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { /// @brief Interval trees implemented using red-black-trees as described in @@ -58,7 +57,6 @@ struct HPP_FCL_DLLAPI SimpleInterval { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h index b0668d989..519e49396 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h @@ -35,13 +35,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SPARSEHASHTABLE_INL_H -#define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_INL_H +#ifndef COAL_BROADPHASE_SPARSEHASHTABLE_INL_H +#define COAL_BROADPHASE_SPARSEHASHTABLE_INL_H #include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -107,7 +106,6 @@ void SparseHashTable::clear() { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h index 5c28ccf3f..be28e379b 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H -#define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H +#ifndef COAL_BROADPHASE_SPARSEHASHTABLE_H +#define COAL_BROADPHASE_SPARSEHASHTABLE_H #include #include #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -87,8 +86,7 @@ class SparseHashTable { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/broadphase/detail/sparse_hash_table-inl.h" diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h index 1248b9eae..76714036e 100644 --- a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h +++ b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SPATIALHASH_INL_H -#define HPP_FCL_BROADPHASE_SPATIALHASH_INL_H +#ifndef COAL_BROADPHASE_SPATIALHASH_INL_H +#define COAL_BROADPHASE_SPATIALHASH_INL_H #include "hpp/fcl/broadphase/detail/spatial_hash.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -76,7 +75,6 @@ std::vector SpatialHash::operator()(const AABB& aabb) const { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash.h b/include/hpp/fcl/broadphase/detail/spatial_hash.h index a7b711ea2..e51a403a8 100644 --- a/include/hpp/fcl/broadphase/detail/spatial_hash.h +++ b/include/hpp/fcl/broadphase/detail/spatial_hash.h @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SPATIALHASH_H -#define HPP_FCL_BROADPHASE_SPATIALHASH_H +#ifndef COAL_BROADPHASE_SPATIALHASH_H +#define COAL_BROADPHASE_SPATIALHASH_H #include "hpp/fcl/BV/AABB.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { @@ -59,7 +58,6 @@ struct HPP_FCL_DLLAPI SpatialHash { }; } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h index cbc0bd562..0a53146b0 100644 --- a/include/hpp/fcl/collision.h +++ b/include/hpp/fcl/collision.h @@ -36,8 +36,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_COLLISION_H -#define HPP_FCL_COLLISION_H +#ifndef COAL_COLLISION_H +#define COAL_COLLISION_H #include #include @@ -45,8 +45,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Main collision interface: given two collision objects, and the /// requirements for contacts, including num of max contacts, whether perform @@ -116,7 +115,6 @@ class HPP_FCL_DLLAPI ComputeCollision { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 1285e5833..cec422aaa 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -36,8 +36,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_COLLISION_DATA_H -#define HPP_FCL_COLLISION_DATA_H +#ifndef COAL_COLLISION_DATA_H +#define COAL_COLLISION_DATA_H #include #include @@ -52,8 +52,7 @@ #include "hpp/fcl/narrowphase/narrowphase_defaults.h" #include "hpp/fcl/logging.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Contact information returned by collision struct HPP_FCL_DLLAPI Contact { @@ -1234,8 +1233,6 @@ inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, return (CollisionRequestFlag&)((int&)(a) ^= static_cast(b)); } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/collision_func_matrix.h b/include/hpp/fcl/collision_func_matrix.h index 57b78bdfa..5d504c69c 100644 --- a/include/hpp/fcl/collision_func_matrix.h +++ b/include/hpp/fcl/collision_func_matrix.h @@ -35,15 +35,14 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_COLLISION_FUNC_MATRIX_H -#define HPP_FCL_COLLISION_FUNC_MATRIX_H +#ifndef COAL_COLLISION_FUNC_MATRIX_H +#define COAL_COLLISION_FUNC_MATRIX_H #include #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief collision matrix stores the functions for collision between different /// types of objects and provides a uniform call interface @@ -73,8 +72,6 @@ struct HPP_FCL_DLLAPI CollisionFunctionMatrix { CollisionFunctionMatrix(); }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h index fae651464..efad46b3c 100644 --- a/include/hpp/fcl/collision_object.h +++ b/include/hpp/fcl/collision_object.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_COLLISION_OBJECT_BASE_H -#define HPP_FCL_COLLISION_OBJECT_BASE_H +#ifndef COAL_COLLISION_OBJECT_BASE_H +#define COAL_COLLISION_OBJECT_BASE_H #include #include @@ -46,8 +46,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief object type: BVH (mesh, points), basic geometry, octree enum OBJECT_TYPE { @@ -356,8 +355,6 @@ class HPP_FCL_DLLAPI CollisionObject { void* user_data; }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/collision_utility.h b/include/hpp/fcl/collision_utility.h index b231973ec..e151dd533 100644 --- a/include/hpp/fcl/collision_utility.h +++ b/include/hpp/fcl/collision_utility.h @@ -15,13 +15,12 @@ // received a copy of the GNU Lesser General Public License along with // hpp-fcl. If not, see . -#ifndef HPP_FCL_COLLISION_UTILITY_H -#define HPP_FCL_COLLISION_UTILITY_H +#ifndef COAL_COLLISION_UTILITY_H +#define COAL_COLLISION_UTILITY_H #include -namespace hpp { -namespace fcl { +namespace coal { HPP_FCL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, const Transform3f& pose, @@ -52,8 +51,6 @@ inline const char* get_object_type_name(OBJECT_TYPE object_type) { return object_type_name_all[object_type]; } -} // namespace fcl +} // namespace coal -} // namespace hpp - -#endif // FCL_COLLISION_UTILITY_H +#endif // COAL_COLLISION_UTILITY_H diff --git a/include/hpp/fcl/contact_patch.h b/include/hpp/fcl/contact_patch.h index e3dfa7547..7b9159bd3 100644 --- a/include/hpp/fcl/contact_patch.h +++ b/include/hpp/fcl/contact_patch.h @@ -34,16 +34,15 @@ /** \author Louis Montaut */ -#ifndef HPP_FCL_CONTACT_PATCH_H -#define HPP_FCL_CONTACT_PATCH_H +#ifndef COAL_CONTACT_PATCH_H +#define COAL_CONTACT_PATCH_H #include "hpp/fcl/data_types.h" #include "hpp/fcl/collision_data.h" #include "hpp/fcl/contact_patch/contact_patch_solver.h" #include "hpp/fcl/contact_patch_func_matrix.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Main contact patch computation interface. /// @note Please see @ref ContactPatchRequest and @ref ContactPatchResult for @@ -118,7 +117,6 @@ class HPP_FCL_DLLAPI ComputeContactPatch { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.h b/include/hpp/fcl/contact_patch/contact_patch_solver.h index ed2ae9478..e6af26b57 100644 --- a/include/hpp/fcl/contact_patch/contact_patch_solver.h +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.h @@ -33,15 +33,14 @@ /** \author Louis Montaut */ -#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_H -#define HPP_FCL_CONTACT_PATCH_SOLVER_H +#ifndef COAL_CONTACT_PATCH_SOLVER_H +#define COAL_CONTACT_PATCH_SOLVER_H #include "hpp/fcl/collision_data.h" #include "hpp/fcl/logging.h" #include "hpp/fcl/narrowphase/gjk.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Solver to compute contact patches, i.e. the intersection between two /// contact surfaces projected onto the shapes' separating plane. @@ -202,9 +201,8 @@ struct HPP_FCL_DLLAPI ContactPatchSolver { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace fcl -} // namespace hpp +} // namespace coal #include "hpp/fcl/contact_patch/contact_patch_solver.hxx" -#endif // HPP_FCL_CONTACT_PATCH_SOLVER_H +#endif // COAL_CONTACT_PATCH_SOLVER_H diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx index 1960ff0ac..d6cc0c96a 100644 --- a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx @@ -34,14 +34,13 @@ /** \author Louis Montaut */ -#ifndef HPP_FCL_CONTACT_PATCH_SOLVER_HXX -#define HPP_FCL_CONTACT_PATCH_SOLVER_HXX +#ifndef COAL_CONTACT_PATCH_SOLVER_HXX +#define COAL_CONTACT_PATCH_SOLVER_HXX #include "hpp/fcl/data_types.h" #include "hpp/fcl/shape/geometric_shapes_traits.h" -namespace hpp { -namespace fcl { +namespace coal { // ============================================================================ inline void ContactPatchSolver::set(const ContactPatchRequest& request) { @@ -421,7 +420,6 @@ inline Vec2f ContactPatchSolver::computeLineSegmentIntersection( return alpha * c + (1 - alpha) * d; } -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_CONTACT_PATCH_SOLVER_HXX +#endif // COAL_CONTACT_PATCH_SOLVER_HXX diff --git a/include/hpp/fcl/contact_patch_func_matrix.h b/include/hpp/fcl/contact_patch_func_matrix.h index 7476650a4..faa01a9a5 100644 --- a/include/hpp/fcl/contact_patch_func_matrix.h +++ b/include/hpp/fcl/contact_patch_func_matrix.h @@ -34,15 +34,14 @@ /** \author Louis Montaut */ -#ifndef HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H -#define HPP_FCL_CONTACT_PATCH_FUNC_MATRIX_H +#ifndef COAL_CONTACT_PATCH_FUNC_MATRIX_H +#define COAL_CONTACT_PATCH_FUNC_MATRIX_H #include "hpp/fcl/collision_data.h" #include "hpp/fcl/contact_patch/contact_patch_solver.h" #include "hpp/fcl/narrowphase/narrowphase.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief The contact patch matrix stores the functions for contact patches /// computation between different types of objects and provides a uniform call @@ -81,7 +80,6 @@ struct HPP_FCL_DLLAPI ContactPatchFunctionMatrix { ContactPatchFunctionMatrix(); }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h index bcbb3c9a3..2cbb183e9 100644 --- a/include/hpp/fcl/data_types.h +++ b/include/hpp/fcl/data_types.h @@ -36,16 +36,14 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_DATA_TYPES_H -#define HPP_FCL_DATA_TYPES_H +#ifndef COAL_DATA_TYPES_H +#define COAL_DATA_TYPES_H #include #include #include -namespace hpp { - #ifdef HPP_FCL_HAS_OCTOMAP #define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ (OCTOMAP_MAJOR_VERSION > x || \ @@ -59,10 +57,8 @@ namespace hpp { (OCTOMAP_MINOR_VERSION < y || \ (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z)))) #endif // HPP_FCL_HAS_OCTOMAP -} // namespace hpp -namespace hpp { -namespace fcl { +namespace coal { typedef double FCL_REAL; typedef Eigen::Matrix Vec3f; typedef Eigen::Matrix Vec2f; @@ -182,8 +178,6 @@ struct HPP_FCL_DLLAPI Quadrilateral { index_type vids[4]; }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h index c0c456494..f734c760a 100644 --- a/include/hpp/fcl/distance.h +++ b/include/hpp/fcl/distance.h @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_DISTANCE_H -#define HPP_FCL_DISTANCE_H +#ifndef COAL_DISTANCE_H +#define COAL_DISTANCE_H #include #include #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Main distance interface: given two collision objects, and the /// requirements for contacts, including whether return the nearest points, this @@ -111,7 +110,6 @@ class HPP_FCL_DLLAPI ComputeDistance { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/distance_func_matrix.h b/include/hpp/fcl/distance_func_matrix.h index cf51cb906..29d79a5cb 100644 --- a/include/hpp/fcl/distance_func_matrix.h +++ b/include/hpp/fcl/distance_func_matrix.h @@ -35,15 +35,14 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_DISTANCE_FUNC_MATRIX_H -#define HPP_FCL_DISTANCE_FUNC_MATRIX_H +#ifndef COAL_DISTANCE_FUNC_MATRIX_H +#define COAL_DISTANCE_FUNC_MATRIX_H #include #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief distance matrix stores the functions for distance between different /// types of objects and provides a uniform call interface @@ -70,8 +69,6 @@ struct HPP_FCL_DLLAPI DistanceFunctionMatrix { DistanceFunctionMatrix(); }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/doc.hh b/include/hpp/fcl/doc.hh index 4d645bf23..ade896a29 100644 --- a/include/hpp/fcl/doc.hh +++ b/include/hpp/fcl/doc.hh @@ -32,8 +32,7 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -namespace hpp { -namespace fcl { +namespace coal { /// \mainpage /// \anchor hpp_fcl_documentation @@ -76,5 +75,4 @@ namespace fcl { /// hpp::fcl::ComputeCollision and the objects are considered as not /// colliding. -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh index cb756a84e..14b673c20 100644 --- a/include/hpp/fcl/fwd.hh +++ b/include/hpp/fcl/fwd.hh @@ -35,8 +35,8 @@ // POSSIBILITY OF SUCH DAMAGE. // -#ifndef HPP_FCL_FWD_HH -#define HPP_FCL_FWD_HH +#ifndef COAL_FWD_HH +#define COAL_FWD_HH #include #include @@ -123,8 +123,7 @@ #define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED #endif // __GNUC__ -namespace hpp { -namespace fcl { +namespace coal { using std::dynamic_pointer_cast; using std::make_shared; using std::shared_ptr; @@ -145,7 +144,6 @@ typedef shared_ptr BVHModelPtr_t; class OcTree; typedef shared_ptr OcTreePtr_t; typedef shared_ptr OcTreeConstPtr_t; -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_FWD_HH +#endif // COAL_FWD_HH diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h index 18ab9e5a1..af6b43629 100644 --- a/include/hpp/fcl/hfield.h +++ b/include/hpp/fcl/hfield.h @@ -34,8 +34,8 @@ /** \author Justin Carpentier */ -#ifndef HPP_FCL_HEIGHT_FIELD_H -#define HPP_FCL_HEIGHT_FIELD_H +#ifndef COAL_HEIGHT_FIELD_H +#define COAL_HEIGHT_FIELD_H #include "hpp/fcl/fwd.hh" #include "hpp/fcl/data_types.h" @@ -45,8 +45,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Construction_Of_HeightField /// @{ @@ -161,7 +160,7 @@ struct HPP_FCL_DLLAPI HFNode : public HFNodeBase { Vec3f getCenter() const { return bv.center(); } /// @brief Access to the orientation of the BV - hpp::fcl::Matrix3f::IdentityReturnType getOrientation() const { + coal::Matrix3f::IdentityReturnType getOrientation() const { return Matrix3f::Identity(); } @@ -538,8 +537,6 @@ NODE_TYPE HeightField>::getNodeType() const; /// @} -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/internal/BV_fitter.h b/include/hpp/fcl/internal/BV_fitter.h index 514741f0b..4461b0b22 100644 --- a/include/hpp/fcl/internal/BV_fitter.h +++ b/include/hpp/fcl/internal/BV_fitter.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BV_FITTER_H -#define HPP_FCL_BV_FITTER_H +#ifndef COAL_BV_FITTER_H +#define COAL_BV_FITTER_H #include #include @@ -44,8 +44,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Compute a bounding volume that fits a set of n points. template @@ -216,8 +215,6 @@ class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { AABB fit(unsigned int* primitive_indices, unsigned int num_primitives); }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/internal/BV_splitter.h b/include/hpp/fcl/internal/BV_splitter.h index 24c5c6077..d5edd706c 100644 --- a/include/hpp/fcl/internal/BV_splitter.h +++ b/include/hpp/fcl/internal/BV_splitter.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_BV_SPLITTER_H -#define HPP_FCL_BV_SPLITTER_H +#ifndef COAL_BV_SPLITTER_H +#define COAL_BV_SPLITTER_H #include #include @@ -44,8 +44,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Three types of split algorithms are provided in FCL as default enum SplitMethodType { @@ -279,8 +278,6 @@ void HPP_FCL_DLLAPI BVSplitter::computeRule_median( const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h index 9e3f6ec01..1f6fc4923 100644 --- a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h +++ b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h @@ -34,8 +34,8 @@ /** \author Louis Montaut */ -#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H -#define HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H +#ifndef COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H +#define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H #include "hpp/fcl/collision_data.h" #include "hpp/fcl/collision_utility.h" @@ -43,8 +43,7 @@ #include "hpp/fcl/contact_patch/contact_patch_solver.h" #include "hpp/fcl/shape/geometric_shapes_traits.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Shape-shape contact patch computation. /// Assumes that `csolver` and the `ContactPatchResult` have already been set up @@ -260,7 +259,6 @@ void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, o1, tf1, o2, tf2, collision_result, csolver, request, result); } -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index 3321b4b38..cee08823c 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -35,8 +35,8 @@ /** \author Florent Lamiraux */ -#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H -#define HPP_FCL_INTERNAL_SHAPE_SHAPE_FUNC_H +#ifndef COAL_INTERNAL_SHAPE_SHAPE_FUNC_H +#define COAL_INTERNAL_SHAPE_SHAPE_FUNC_H /// @cond INTERNAL @@ -45,8 +45,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { template struct ShapeShapeDistancer { @@ -115,7 +114,7 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) { - return ::hpp::fcl::ShapeShapeDistancer::run( + return ::coal::ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal); } } // namespace internal @@ -308,8 +307,7 @@ SHAPE_SELF_DISTANCE_SPECIALIZATION(Halfspace) #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION #undef SHAPE_SELF_DISTANCE_SPECIALIZATION -} // namespace fcl -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/tools.h b/include/hpp/fcl/internal/tools.h index 346141a50..e9a918a12 100644 --- a/include/hpp/fcl/internal/tools.h +++ b/include/hpp/fcl/internal/tools.h @@ -35,8 +35,8 @@ /** \author Joseph Mirabel */ -#ifndef HPP_FCL_INTERNAL_TOOLS_H -#define HPP_FCL_INTERNAL_TOOLS_H +#ifndef COAL_INTERNAL_TOOLS_H +#define COAL_INTERNAL_TOOLS_H #include @@ -46,8 +46,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { template static inline typename Derived::Scalar triple( @@ -209,7 +208,6 @@ bool isEqual(const Eigen::MatrixBase& lhs, return ((lhs - rhs).array().abs() < tol).all(); } -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/internal/traversal.h b/include/hpp/fcl/internal/traversal.h index e999d8c43..eb74e2ab8 100644 --- a/include/hpp/fcl/internal/traversal.h +++ b/include/hpp/fcl/internal/traversal.h @@ -34,13 +34,12 @@ /** \author Joseph Mirabel */ -#ifndef HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H -#define HPP_FCL_TRAVERSAL_DETAILS_TRAVERSAL_H +#ifndef COAL_TRAVERSAL_DETAILS_TRAVERSAL_H +#define COAL_TRAVERSAL_DETAILS_TRAVERSAL_H /// @cond INTERNAL -namespace hpp { -namespace fcl { +namespace coal { enum { RelativeTransformationIsIdentity = 1 }; @@ -67,9 +66,7 @@ struct HPP_FCL_DLLAPI RelativeTransformation { }; } // namespace details -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h index a3c56f8d4..b27334b4c 100644 --- a/include/hpp/fcl/internal/traversal_node_base.h +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_NODE_BASE_H -#define HPP_FCL_TRAVERSAL_NODE_BASE_H +#ifndef COAL_TRAVERSAL_NODE_BASE_H +#define COAL_TRAVERSAL_NODE_BASE_H /// @cond INTERNAL @@ -44,8 +44,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Node structure encoding the information required for traversal. @@ -166,9 +165,7 @@ class DistanceTraversalNodeBase : public TraversalNodeBase { ///@} -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h b/include/hpp/fcl/internal/traversal_node_bvh_hfield.h index 27a0caed5..dc43abbc5 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_hfield.h @@ -34,8 +34,8 @@ /** \author Justin Carpentier */ -#ifndef HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H -#define HPP_FCL_TRAVERSAL_NODE_BVH_HFIELD_H +#ifndef COAL_TRAVERSAL_NODE_BVH_HFIELD_H +#define COAL_TRAVERSAL_NODE_BVH_HFIELD_H /// @cond INTERNAL @@ -55,8 +55,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Traversal_For_Collision /// @{ @@ -546,9 +545,7 @@ inline const Matrix3f& getBVAxes(const OBBRSS& bv) { } // namespace details -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h index c24a0c002..7e03bf9dd 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H -#define HPP_FCL_TRAVERSAL_NODE_MESH_SHAPE_H +#ifndef COAL_TRAVERSAL_NODE_MESH_SHAPE_H +#define COAL_TRAVERSAL_NODE_MESH_SHAPE_H /// @cond INTERNAL @@ -49,8 +49,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Traversal_For_Collision /// @{ @@ -479,8 +478,7 @@ class MeshShapeDistanceTraversalNodeOBBRSS /// @} -} // namespace fcl -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h index bd20af2ac..56cb923b5 100644 --- a/include/hpp/fcl/internal/traversal_node_bvhs.h +++ b/include/hpp/fcl/internal/traversal_node_bvhs.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H -#define HPP_FCL_TRAVERSAL_NODE_MESHES_H +#ifndef COAL_TRAVERSAL_NODE_MESHES_H +#define COAL_TRAVERSAL_NODE_MESHES_H /// @cond INTERNAL @@ -53,8 +53,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Traversal_For_Collision /// @{ @@ -548,9 +547,7 @@ inline const Matrix3f& getBVAxes(const OBBRSS& bv) { } // namespace details -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 8bd5617c6..2b62dead1 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -34,8 +34,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H -#define HPP_FCL_TRAVERSAL_NODE_HFIELD_SHAPE_H +#ifndef COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H +#define COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H /// @cond INTERNAL @@ -50,8 +50,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Traversal_For_Collision /// @{ @@ -750,8 +749,7 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { /// @} -} // namespace fcl -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h index 9a0094e13..55a622cd1 100644 --- a/include/hpp/fcl/internal/traversal_node_octree.h +++ b/include/hpp/fcl/internal/traversal_node_octree.h @@ -36,8 +36,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_NODE_OCTREE_H -#define HPP_FCL_TRAVERSAL_NODE_OCTREE_H +#ifndef COAL_TRAVERSAL_NODE_OCTREE_H +#define COAL_TRAVERSAL_NODE_OCTREE_H /// @cond INTERNAL @@ -51,8 +51,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Algorithms for collision related with octree class HPP_FCL_DLLAPI OcTreeSolver { @@ -1079,7 +1078,7 @@ class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const { + bool BVDisjoints(unsigned int, unsigned int, coal::FCL_REAL&) const { return false; } @@ -1354,9 +1353,7 @@ class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode /// @} -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h index 20b5c71db..b3644f528 100644 --- a/include/hpp/fcl/internal/traversal_node_setup.h +++ b/include/hpp/fcl/internal/traversal_node_setup.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H -#define HPP_FCL_TRAVERSAL_NODE_SETUP_H +#ifndef COAL_TRAVERSAL_NODE_SETUP_H +#define COAL_TRAVERSAL_NODE_SETUP_H /// @cond INTERNAL @@ -55,8 +55,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { #ifdef HPP_FCL_HAS_OCTOMAP /// @brief Initialize traversal node for collision between two octrees, given @@ -809,8 +808,7 @@ bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, node, model1, tf1, model2, tf2, nsolver, request, result); } -} // namespace fcl -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h index e30db3cb6..4e0326a94 100644 --- a/include/hpp/fcl/internal/traversal_node_shapes.h +++ b/include/hpp/fcl/internal/traversal_node_shapes.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_NODE_SHAPES_H -#define HPP_FCL_TRAVERSAL_NODE_SHAPES_H +#ifndef COAL_TRAVERSAL_NODE_SHAPES_H +#define COAL_TRAVERSAL_NODE_SHAPES_H /// @cond INTERNAL @@ -46,8 +46,7 @@ #include "hpp/fcl/internal/traversal_node_base.h" #include "hpp/fcl/internal/shape_shape_func.h" -namespace hpp { -namespace fcl { +namespace coal { /// @addtogroup Traversal_For_Collision /// @{ @@ -118,9 +117,7 @@ class HPP_FCL_DLLAPI ShapeDistanceTraversalNode /// @} -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/internal/traversal_recurse.h b/include/hpp/fcl/internal/traversal_recurse.h index 1f15fe4f2..2d19879ec 100644 --- a/include/hpp/fcl/internal/traversal_recurse.h +++ b/include/hpp/fcl/internal/traversal_recurse.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRAVERSAL_RECURSE_H -#define HPP_FCL_TRAVERSAL_RECURSE_H +#ifndef COAL_TRAVERSAL_RECURSE_H +#define COAL_TRAVERSAL_RECURSE_H /// @cond INTERNAL @@ -45,8 +45,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// Recurse function for collision /// @param node collision node, @@ -74,9 +73,7 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, CollisionResult& result, BVHFrontList* front_list); -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/include/hpp/fcl/logging.h b/include/hpp/fcl/logging.h index c65624b3d..732a890e9 100644 --- a/include/hpp/fcl/logging.h +++ b/include/hpp/fcl/logging.h @@ -35,8 +35,8 @@ /// This file defines basic logging macros for HPP-FCL, based on Boost.Log. /// To enable logging, define the preprocessor macro `HPP_FCL_ENABLE_LOGGING`. -#ifndef HPP_FCL_LOGGING_H -#define HPP_FCL_LOGGING_H +#ifndef COAL_LOGGING_H +#define COAL_LOGGING_H #ifdef HPP_FCL_ENABLE_LOGGING #include diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h index 9c1bdb0b5..408d0229a 100644 --- a/include/hpp/fcl/math/matrix_3f.h +++ b/include/hpp/fcl/math/matrix_3f.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_MATRIX_3F_H -#define HPP_FCL_MATRIX_3F_H +#ifndef COAL_MATRIX_3F_H +#define COAL_MATRIX_3F_H #warning "This file is deprecated. Include instead." diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h index 9a1d31e5d..dfa468c6e 100644 --- a/include/hpp/fcl/math/transform.h +++ b/include/hpp/fcl/math/transform.h @@ -35,14 +35,13 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_TRANSFORM_H -#define HPP_FCL_TRANSFORM_H +#ifndef COAL_TRANSFORM_H +#define COAL_TRANSFORM_H #include "hpp/fcl/fwd.hh" #include "hpp/fcl/data_types.h" -namespace hpp { -namespace fcl { +namespace coal { HPP_FCL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; typedef Eigen::Quaternion Quatf; @@ -266,7 +265,6 @@ inline Matrix3f constructOrthonormalBasisFromVector(const Vec3f& vec) { return basis; } -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/math/types.h b/include/hpp/fcl/math/types.h index ece985db8..ea9569499 100644 --- a/include/hpp/fcl/math/types.h +++ b/include/hpp/fcl/math/types.h @@ -35,12 +35,12 @@ /** \author Joseph Mirabel */ -#ifndef HPP_FCL_MATH_TYPES_H -#define HPP_FCL_MATH_TYPES_H +#ifndef COAL_MATH_TYPES_H +#define COAL_MATH_TYPES_H #warning "This file is deprecated. Include instead." // List of equivalent includes. #include -#endif \ No newline at end of file +#endif diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h index 4389bba75..62df29a0e 100644 --- a/include/hpp/fcl/math/vec_3f.h +++ b/include/hpp/fcl/math/vec_3f.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_VEC_3F_H -#define HPP_FCL_VEC_3F_H +#ifndef COAL_VEC_3F_H +#define COAL_VEC_3F_H #warning "This file is deprecated. Include instead." diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h index 115933740..d5672a44a 100644 --- a/include/hpp/fcl/mesh_loader/assimp.h +++ b/include/hpp/fcl/mesh_loader/assimp.h @@ -35,8 +35,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef HPP_FCL_MESH_LOADER_ASSIMP_H -#define HPP_FCL_MESH_LOADER_ASSIMP_H +#ifndef COAL_MESH_LOADER_ASSIMP_H +#define COAL_MESH_LOADER_ASSIMP_H #include #include @@ -48,14 +48,13 @@ namespace Assimp { class Importer; } -namespace hpp { -namespace fcl { +namespace coal { namespace internal { struct HPP_FCL_DLLAPI TriangleAndVertices { - std::vector vertices_; - std::vector triangles_; + std::vector vertices_; + std::vector triangles_; }; struct HPP_FCL_DLLAPI Loader { @@ -76,7 +75,7 @@ struct HPP_FCL_DLLAPI Loader { * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -HPP_FCL_DLLAPI void buildMesh(const fcl::Vec3f& scale, const aiScene* scene, +HPP_FCL_DLLAPI void buildMesh(const coal::Vec3f& scale, const aiScene* scene, unsigned vertices_offset, TriangleAndVertices& tv); @@ -89,13 +88,13 @@ HPP_FCL_DLLAPI void buildMesh(const fcl::Vec3f& scale, const aiScene* scene, */ template inline void meshFromAssimpScene( - const fcl::Vec3f& scale, const aiScene* scene, + const coal::Vec3f& scale, const aiScene* scene, const shared_ptr >& mesh) { TriangleAndVertices tv; int res = mesh->beginModel(); - if (res != fcl::BVH_OK) { + if (res != coal::BVH_OK) { HPP_FCL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); } @@ -116,7 +115,7 @@ inline void meshFromAssimpScene( */ template inline void loadPolyhedronFromResource( - const std::string& resource_path, const fcl::Vec3f& scale, + const std::string& resource_path, const coal::Vec3f& scale, const shared_ptr >& polyhedron) { internal::Loader scene; scene.load(resource_path); @@ -124,7 +123,6 @@ inline void loadPolyhedronFromResource( internal::meshFromAssimpScene(scale, scene.scene, polyhedron); } -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_MESH_LOADER_ASSIMP_H +#endif // COAL_MESH_LOADER_ASSIMP_H diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/hpp/fcl/mesh_loader/loader.h index 2c3f36249..167873592 100644 --- a/include/hpp/fcl/mesh_loader/loader.h +++ b/include/hpp/fcl/mesh_loader/loader.h @@ -35,8 +35,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef HPP_FCL_MESH_LOADER_LOADER_H -#define HPP_FCL_MESH_LOADER_LOADER_H +#ifndef COAL_MESH_LOADER_LOADER_H +#define COAL_MESH_LOADER_LOADER_H #include #include @@ -46,8 +46,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// Base class for building polyhedron from files. /// This class builds a new object for each file. class HPP_FCL_DLLAPI MeshLoader { @@ -98,8 +97,6 @@ class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader { private: Cache_t cache_; }; -} // namespace fcl +} // namespace coal -} // namespace hpp - -#endif // FCL_MESH_LOADER_LOADER_H +#endif // COAL_MESH_LOADER_LOADER_H diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 206109ef8..b84e6eadf 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -36,15 +36,14 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_GJK_H -#define HPP_FCL_GJK_H +#ifndef COAL_GJK_H +#define COAL_GJK_H #include #include "hpp/fcl/narrowphase/minkowski_difference.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { @@ -450,8 +449,6 @@ struct HPP_FCL_DLLAPI EPA { } // namespace details -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.h b/include/hpp/fcl/narrowphase/minkowski_difference.h index f6c0f93f4..619b00c78 100644 --- a/include/hpp/fcl/narrowphase/minkowski_difference.h +++ b/include/hpp/fcl/narrowphase/minkowski_difference.h @@ -36,15 +36,14 @@ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ -#ifndef HPP_FCL_MINKOWSKI_DIFFERENCE_H -#define HPP_FCL_MINKOWSKI_DIFFERENCE_H +#ifndef COAL_MINKOWSKI_DIFFERENCE_H +#define COAL_MINKOWSKI_DIFFERENCE_H #include "hpp/fcl/shape/geometric_shapes.h" #include "hpp/fcl/math/transform.h" #include "hpp/fcl/narrowphase/support_functions.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { @@ -182,7 +181,6 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { } // namespace details -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_MINKOWSKI_DIFFERENCE_H +#endif // COAL_MINKOWSKI_DIFFERENCE_H diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index ff9a5785b..b0f463d59 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -37,8 +37,8 @@ /** \author Jia Pan, Florent Lamiraux */ -#ifndef HPP_FCL_NARROWPHASE_H -#define HPP_FCL_NARROWPHASE_H +#ifndef COAL_NARROWPHASE_H +#define COAL_NARROWPHASE_H #include @@ -47,8 +47,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief collision and distance solver based on the GJK and EPA algorithms. /// Originally, GJK and EPA were implemented in fcl which itself took @@ -723,7 +722,6 @@ struct HPP_FCL_DLLAPI GJKSolver { } }; -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/narrowphase/narrowphase_defaults.h b/include/hpp/fcl/narrowphase/narrowphase_defaults.h index fcf3cd41c..5ed5bb224 100644 --- a/include/hpp/fcl/narrowphase/narrowphase_defaults.h +++ b/include/hpp/fcl/narrowphase/narrowphase_defaults.h @@ -35,13 +35,12 @@ /// This file defines different macros used to characterize the default behavior /// of the narrowphase algorithms GJK and EPA. -#ifndef HPP_FCL_NARROWPHASE_DEFAULTS -#define HPP_FCL_NARROWPHASE_DEFAULTS +#ifndef COAL_NARROWPHASE_DEFAULTS_H +#define COAL_NARROWPHASE_DEFAULTS_H #include -namespace hpp { -namespace fcl { +namespace coal { /// GJK constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128; @@ -61,7 +60,6 @@ constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64; constexpr FCL_REAL EPA_DEFAULT_TOLERANCE = 1e-6; constexpr FCL_REAL EPA_MINIMUM_TOLERANCE = 1e-6; -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_NARROWPHASE_DEFAULTS +#endif // COAL_NARROWPHASE_DEFAULTS_H diff --git a/include/hpp/fcl/narrowphase/support_functions.h b/include/hpp/fcl/narrowphase/support_functions.h index 951240cf5..d1ecec32c 100644 --- a/include/hpp/fcl/narrowphase/support_functions.h +++ b/include/hpp/fcl/narrowphase/support_functions.h @@ -36,15 +36,14 @@ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ -#ifndef HPP_FCL_SUPPORT_FUNCTIONS_H -#define HPP_FCL_SUPPORT_FUNCTIONS_H +#ifndef COAL_SUPPORT_FUNCTIONS_H +#define COAL_SUPPORT_FUNCTIONS_H #include "hpp/fcl/shape/geometric_shapes.h" #include "hpp/fcl/math/transform.h" #include "hpp/fcl/collision_data.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { @@ -304,7 +303,6 @@ HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud, } // namespace details -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_SUPPORT_FUNCTIONS_H +#endif // COAL_SUPPORT_FUNCTIONS_H diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h index feec9a7eb..2ee09b5eb 100644 --- a/include/hpp/fcl/octree.h +++ b/include/hpp/fcl/octree.h @@ -36,8 +36,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_OCTREE_H -#define HPP_FCL_OCTREE_H +#ifndef COAL_OCTREE_H +#define COAL_OCTREE_H #include @@ -46,8 +46,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Octree is one type of collision geometry which can encode uncertainty /// information in the sensor data. @@ -335,8 +334,6 @@ HPP_FCL_DLLAPI OcTreePtr_t makeOctree(const Eigen::Matrix& point_cloud, const FCL_REAL resolution); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/serialization/AABB.h b/include/hpp/fcl/serialization/AABB.h index 85ff64915..853cf073c 100644 --- a/include/hpp/fcl/serialization/AABB.h +++ b/include/hpp/fcl/serialization/AABB.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_AABB_H -#define HPP_FCL_SERIALIZATION_AABB_H +#ifndef COAL_SERIALIZATION_AABB_H +#define COAL_SERIALIZATION_AABB_H #include "hpp/fcl/BV/AABB.h" #include "hpp/fcl/serialization/fwd.h" @@ -12,8 +12,7 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::AABB& aabb, - const unsigned int /*version*/) { +void serialize(Archive& ar, coal::AABB& aabb, const unsigned int /*version*/) { ar& make_nvp("min_", aabb.min_); ar& make_nvp("max_", aabb.max_); } diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/hpp/fcl/serialization/BVH_model.h index 47e4fb787..4783004c5 100644 --- a/include/hpp/fcl/serialization/BVH_model.h +++ b/include/hpp/fcl/serialization/BVH_model.h @@ -18,17 +18,17 @@ namespace boost { namespace serialization { namespace internal { -struct BVHModelBaseAccessor : hpp::fcl::BVHModelBase { - typedef hpp::fcl::BVHModelBase Base; +struct BVHModelBaseAccessor : coal::BVHModelBase { + typedef coal::BVHModelBase Base; using Base::num_tris_allocated; using Base::num_vertices_allocated; }; } // namespace internal template -void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model, +void save(Archive &ar, const coal::BVHModelBase &bvh_model, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || bvh_model.build_state == BVH_BUILD_STATE_UPDATED) && (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) { @@ -39,9 +39,9 @@ void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model, std::invalid_argument); } - ar &make_nvp("base", - boost::serialization::base_object( - bvh_model)); + ar &make_nvp( + "base", + boost::serialization::base_object(bvh_model)); ar &make_nvp("num_vertices", bvh_model.num_vertices); ar &make_nvp("vertices", bvh_model.vertices); @@ -65,12 +65,12 @@ void save(Archive &ar, const hpp::fcl::BVHModelBase &bvh_model, } template -void load(Archive &ar, hpp::fcl::BVHModelBase &bvh_model, +void load(Archive &ar, coal::BVHModelBase &bvh_model, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; ar >> make_nvp("base", - boost::serialization::base_object( + boost::serialization::base_object( bvh_model)); ar >> make_nvp("num_vertices", bvh_model.num_vertices); @@ -86,12 +86,12 @@ void load(Archive &ar, hpp::fcl::BVHModelBase &bvh_model, // ar >> make_nvp("has_convex",has_convex); } -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::BVHModelBase) +HPP_FCL_SERIALIZATION_SPLIT(coal::BVHModelBase) namespace internal { template -struct BVHModelAccessor : hpp::fcl::BVHModel { - typedef hpp::fcl::BVHModel Base; +struct BVHModelAccessor : coal::BVHModel { + typedef coal::BVHModel Base; using Base::bvs; using Base::num_bvs; using Base::num_bvs_allocated; @@ -100,15 +100,15 @@ struct BVHModelAccessor : hpp::fcl::BVHModel { } // namespace internal template -void serialize(Archive &ar, hpp::fcl::BVHModel &bvh_model, +void serialize(Archive &ar, coal::BVHModel &bvh_model, const unsigned int version) { split_free(ar, bvh_model, version); } template -void save(Archive &ar, const hpp::fcl::BVHModel &bvh_model_, +void save(Archive &ar, const coal::BVHModel &bvh_model_, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; typedef internal::BVHModelAccessor Accessor; typedef BVNode Node; @@ -171,9 +171,9 @@ void save(Archive &ar, const hpp::fcl::BVHModel &bvh_model_, } template -void load(Archive &ar, hpp::fcl::BVHModel &bvh_model_, +void load(Archive &ar, coal::BVHModel &bvh_model_, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; typedef internal::BVHModelAccessor Accessor; typedef BVNode Node; @@ -225,28 +225,26 @@ void load(Archive &ar, hpp::fcl::BVHModel &bvh_model_, } // namespace serialization } // namespace boost -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template -struct memory_footprint_evaluator<::hpp::fcl::BVHModel> { - static size_t run(const ::hpp::fcl::BVHModel &bvh_model) { +struct memory_footprint_evaluator<::coal::BVHModel> { + static size_t run(const ::coal::BVHModel &bvh_model) { return static_cast(bvh_model.memUsage(false)); } }; } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::AABB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::RSS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::OBBRSS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::kIOS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<16>>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<18>>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::BVHModel<::hpp::fcl::KDOP<24>>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::AABB>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBB>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::RSS>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBBRSS>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::kIOS>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<16>>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<18>>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<24>>) -#endif // ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H +#endif // ifndef COAL_SERIALIZATION_BVH_MODEL_H diff --git a/include/hpp/fcl/serialization/BV_node.h b/include/hpp/fcl/serialization/BV_node.h index 78193cda5..2fc0406d3 100644 --- a/include/hpp/fcl/serialization/BV_node.h +++ b/include/hpp/fcl/serialization/BV_node.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_BV_NODE_H -#define HPP_FCL_SERIALIZATION_BV_NODE_H +#ifndef COAL_SERIALIZATION_BV_NODE_H +#define COAL_SERIALIZATION_BV_NODE_H #include "hpp/fcl/BV/BV_node.h" @@ -14,7 +14,7 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::BVNodeBase& node, +void serialize(Archive& ar, coal::BVNodeBase& node, const unsigned int /*version*/) { ar& make_nvp("first_child", node.first_child); ar& make_nvp("first_primitive", node.first_primitive); @@ -22,14 +22,14 @@ void serialize(Archive& ar, hpp::fcl::BVNodeBase& node, } template -void serialize(Archive& ar, hpp::fcl::BVNode& node, +void serialize(Archive& ar, coal::BVNode& node, const unsigned int /*version*/) { ar& make_nvp("base", - boost::serialization::base_object(node)); + boost::serialization::base_object(node)); ar& make_nvp("bv", node.bv); } } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_BV_NODE_H +#endif // ifndef COAL_SERIALIZATION_BV_NODE_H diff --git a/include/hpp/fcl/serialization/BV_splitter.h b/include/hpp/fcl/serialization/BV_splitter.h index 24eac7649..1cd380550 100644 --- a/include/hpp/fcl/serialization/BV_splitter.h +++ b/include/hpp/fcl/serialization/BV_splitter.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H -#define HPP_FCL_SERIALIZATION_BV_SPLITTER_H +#ifndef COAL_SERIALIZATION_BV_SPLITTER_H +#define COAL_SERIALIZATION_BV_SPLITTER_H #include "hpp/fcl/internal/BV_splitter.h" @@ -14,8 +14,8 @@ namespace serialization { namespace internal { template -struct BVSplitterAccessor : hpp::fcl::BVSplitter { - typedef hpp::fcl::BVSplitter Base; +struct BVSplitterAccessor : coal::BVSplitter { + typedef coal::BVSplitter Base; using Base::split_axis; using Base::split_method; using Base::split_value; @@ -27,9 +27,9 @@ struct BVSplitterAccessor : hpp::fcl::BVSplitter { } // namespace internal template -void save(Archive &ar, const hpp::fcl::BVSplitter &splitter_, +void save(Archive &ar, const coal::BVSplitter &splitter_, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; typedef internal::BVSplitterAccessor Accessor; const Accessor &splitter = reinterpret_cast(splitter_); @@ -41,9 +41,9 @@ void save(Archive &ar, const hpp::fcl::BVSplitter &splitter_, } template -void load(Archive &ar, hpp::fcl::BVSplitter &splitter_, +void load(Archive &ar, coal::BVSplitter &splitter_, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; typedef internal::BVSplitterAccessor Accessor; Accessor &splitter = reinterpret_cast(splitter_); @@ -59,4 +59,4 @@ void load(Archive &ar, hpp::fcl::BVSplitter &splitter_, } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_BV_SPLITTER_H +#endif // ifndef COAL_SERIALIZATION_BV_SPLITTER_H diff --git a/include/hpp/fcl/serialization/OBB.h b/include/hpp/fcl/serialization/OBB.h index 2c07a44e7..0a584b350 100644 --- a/include/hpp/fcl/serialization/OBB.h +++ b/include/hpp/fcl/serialization/OBB.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_OBB_H -#define HPP_FCL_SERIALIZATION_OBB_H +#ifndef COAL_SERIALIZATION_OBB_H +#define COAL_SERIALIZATION_OBB_H #include "hpp/fcl/BV/OBB.h" @@ -13,7 +13,7 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::OBB& bv, const unsigned int /*version*/) { +void serialize(Archive& ar, coal::OBB& bv, const unsigned int /*version*/) { ar& make_nvp("axes", bv.axes); ar& make_nvp("To", bv.To); ar& make_nvp("extent", bv.extent); @@ -22,4 +22,4 @@ void serialize(Archive& ar, hpp::fcl::OBB& bv, const unsigned int /*version*/) { } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_OBB_H +#endif // ifndef COAL_SERIALIZATION_OBB_H diff --git a/include/hpp/fcl/serialization/OBBRSS.h b/include/hpp/fcl/serialization/OBBRSS.h index b19ce3a36..2e769d9d2 100644 --- a/include/hpp/fcl/serialization/OBBRSS.h +++ b/include/hpp/fcl/serialization/OBBRSS.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_OBBRSS_H -#define HPP_FCL_SERIALIZATION_OBBRSS_H +#ifndef COAL_SERIALIZATION_OBBRSS_H +#define COAL_SERIALIZATION_OBBRSS_H #include "hpp/fcl/BV/OBBRSS.h" @@ -15,8 +15,7 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::OBBRSS& bv, - const unsigned int /*version*/) { +void serialize(Archive& ar, coal::OBBRSS& bv, const unsigned int /*version*/) { ar& make_nvp("obb", bv.obb); ar& make_nvp("rss", bv.rss); } @@ -24,4 +23,4 @@ void serialize(Archive& ar, hpp::fcl::OBBRSS& bv, } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_OBBRSS_H +#endif // ifndef COAL_SERIALIZATION_OBBRSS_H diff --git a/include/hpp/fcl/serialization/RSS.h b/include/hpp/fcl/serialization/RSS.h index 4f1f7ee75..06b586993 100644 --- a/include/hpp/fcl/serialization/RSS.h +++ b/include/hpp/fcl/serialization/RSS.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_RSS_H -#define HPP_FCL_SERIALIZATION_RSS_H +#ifndef COAL_SERIALIZATION_RSS_H +#define COAL_SERIALIZATION_RSS_H #include "hpp/fcl/BV/RSS.h" @@ -13,7 +13,7 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::RSS& bv, const unsigned int /*version*/) { +void serialize(Archive& ar, coal::RSS& bv, const unsigned int /*version*/) { ar& make_nvp("axes", bv.axes); ar& make_nvp("Tr", bv.Tr); ar& make_nvp("length", make_array(bv.length, 2)); @@ -23,4 +23,4 @@ void serialize(Archive& ar, hpp::fcl::RSS& bv, const unsigned int /*version*/) { } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_RSS_H +#endif // ifndef COAL_SERIALIZATION_RSS_H diff --git a/include/hpp/fcl/serialization/archive.h b/include/hpp/fcl/serialization/archive.h index 2da3a5db1..5063e2c3e 100644 --- a/include/hpp/fcl/serialization/archive.h +++ b/include/hpp/fcl/serialization/archive.h @@ -4,8 +4,8 @@ // https://github.com/stack-of-tasks/pinocchio // -#ifndef HPP_FCL_SERIALIZATION_ARCHIVE_H -#define HPP_FCL_SERIALIZATION_ARCHIVE_H +#ifndef COAL_SERIALIZATION_ARCHIVE_H +#define COAL_SERIALIZATION_ARCHIVE_H #include "hpp/fcl/fwd.hh" @@ -48,8 +48,7 @@ // Handle NAN inside TXT or XML archives #include -namespace hpp { -namespace fcl { +namespace coal { namespace serialization { /// @@ -290,7 +289,6 @@ void saveToBuffer(const T& object, boost::asio::streambuf& buffer) { } } // namespace serialization -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_SERIALIZATION_ARCHIVE_H +#endif // ifndef COAL_SERIALIZATION_ARCHIVE_H diff --git a/include/hpp/fcl/serialization/collision_data.h b/include/hpp/fcl/serialization/collision_data.h index e32a54330..b1a15ab4e 100644 --- a/include/hpp/fcl/serialization/collision_data.h +++ b/include/hpp/fcl/serialization/collision_data.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H -#define HPP_FCL_SERIALIZATION_COLLISION_DATA_H +#ifndef COAL_SERIALIZATION_COLLISION_DATA_H +#define COAL_SERIALIZATION_COLLISION_DATA_H #include "hpp/fcl/collision_data.h" #include "hpp/fcl/serialization/fwd.h" @@ -12,7 +12,7 @@ namespace boost { namespace serialization { template -void save(Archive& ar, const hpp::fcl::Contact& contact, +void save(Archive& ar, const coal::Contact& contact, const unsigned int /*version*/) { ar& make_nvp("b1", contact.b1); ar& make_nvp("b2", contact.b2); @@ -23,12 +23,11 @@ void save(Archive& ar, const hpp::fcl::Contact& contact, } template -void load(Archive& ar, hpp::fcl::Contact& contact, - const unsigned int /*version*/) { +void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) { ar >> make_nvp("b1", contact.b1); ar >> make_nvp("b2", contact.b2); ar >> make_nvp("normal", contact.normal); - std::array nearest_points; + std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); contact.nearest_points[0] = nearest_points[0]; contact.nearest_points[1] = nearest_points[1]; @@ -38,10 +37,10 @@ void load(Archive& ar, hpp::fcl::Contact& contact, contact.o2 = NULL; } -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::Contact) +HPP_FCL_SERIALIZATION_SPLIT(coal::Contact) template -void serialize(Archive& ar, hpp::fcl::QueryRequest& query_request, +void serialize(Archive& ar, coal::QueryRequest& query_request, const unsigned int /*version*/) { ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess); // TODO: use gjk_initial_guess instead @@ -68,7 +67,7 @@ void serialize(Archive& ar, hpp::fcl::QueryRequest& query_request, } template -void serialize(Archive& ar, hpp::fcl::QueryResult& query_result, +void serialize(Archive& ar, coal::QueryResult& query_result, const unsigned int /*version*/) { ar& make_nvp("cached_gjk_guess", query_result.cached_gjk_guess); ar& make_nvp("cached_support_func_guess", @@ -76,11 +75,10 @@ void serialize(Archive& ar, hpp::fcl::QueryResult& query_result, } template -void serialize(Archive& ar, hpp::fcl::CollisionRequest& collision_request, +void serialize(Archive& ar, coal::CollisionRequest& collision_request, const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object( - collision_request)); + ar& make_nvp("base", boost::serialization::base_object( + collision_request)); ar& make_nvp("num_max_contacts", collision_request.num_max_contacts); ar& make_nvp("enable_contact", collision_request.enable_contact); HPP_FCL_COMPILER_DIAGNOSTIC_PUSH @@ -94,9 +92,9 @@ void serialize(Archive& ar, hpp::fcl::CollisionRequest& collision_request, } template -void save(Archive& ar, const hpp::fcl::CollisionResult& collision_result, +void save(Archive& ar, const coal::CollisionResult& collision_result, const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( + ar& make_nvp("base", boost::serialization::base_object( collision_result)); ar& make_nvp("contacts", collision_result.getContacts()); ar& make_nvp("distance_lower_bound", collision_result.distance_lower_bound); @@ -105,32 +103,30 @@ void save(Archive& ar, const hpp::fcl::CollisionResult& collision_result, } template -void load(Archive& ar, hpp::fcl::CollisionResult& collision_result, +void load(Archive& ar, coal::CollisionResult& collision_result, const unsigned int /*version*/) { - ar >> - make_nvp("base", boost::serialization::base_object( - collision_result)); - std::vector contacts; + ar >> make_nvp("base", boost::serialization::base_object( + collision_result)); + std::vector contacts; ar >> make_nvp("contacts", contacts); collision_result.clear(); for (size_t k = 0; k < contacts.size(); ++k) collision_result.addContact(contacts[k]); ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound); - std::array nearest_points; + std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); collision_result.nearest_points[0] = nearest_points[0]; collision_result.nearest_points[1] = nearest_points[1]; ar >> make_nvp("normal", collision_result.normal); } -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionResult) +HPP_FCL_SERIALIZATION_SPLIT(coal::CollisionResult) template -void serialize(Archive& ar, hpp::fcl::DistanceRequest& distance_request, +void serialize(Archive& ar, coal::DistanceRequest& distance_request, const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object( - distance_request)); + ar& make_nvp("base", boost::serialization::base_object( + distance_request)); HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points); @@ -142,9 +138,9 @@ void serialize(Archive& ar, hpp::fcl::DistanceRequest& distance_request, } template -void save(Archive& ar, const hpp::fcl::DistanceResult& distance_result, +void save(Archive& ar, const coal::DistanceResult& distance_result, const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( + ar& make_nvp("base", boost::serialization::base_object( distance_result)); ar& make_nvp("min_distance", distance_result.min_distance); ar& make_nvp("nearest_points", distance_result.nearest_points); @@ -154,13 +150,12 @@ void save(Archive& ar, const hpp::fcl::DistanceResult& distance_result, } template -void load(Archive& ar, hpp::fcl::DistanceResult& distance_result, +void load(Archive& ar, coal::DistanceResult& distance_result, const unsigned int /*version*/) { - ar >> - make_nvp("base", boost::serialization::base_object( - distance_result)); + ar >> make_nvp("base", boost::serialization::base_object( + distance_result)); ar >> make_nvp("min_distance", distance_result.min_distance); - std::array nearest_points; + std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); distance_result.nearest_points[0] = nearest_points[0]; distance_result.nearest_points[1] = nearest_points[1]; @@ -171,14 +166,14 @@ void load(Archive& ar, hpp::fcl::DistanceResult& distance_result, distance_result.o2 = NULL; } -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::DistanceResult) +HPP_FCL_SERIALIZATION_SPLIT(coal::DistanceResult) } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionRequest) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionResult) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::DistanceRequest) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::DistanceResult) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionRequest) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionResult) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceRequest) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceResult) -#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_DATA_H +#endif // ifndef COAL_SERIALIZATION_COLLISION_DATA_H diff --git a/include/hpp/fcl/serialization/collision_object.h b/include/hpp/fcl/serialization/collision_object.h index 018728cf5..e1c26582e 100644 --- a/include/hpp/fcl/serialization/collision_object.h +++ b/include/hpp/fcl/serialization/collision_object.h @@ -2,8 +2,8 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H -#define HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H +#ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H +#define COAL_SERIALIZATION_COLLISION_OBJECT_H #include "hpp/fcl/collision_object.h" @@ -14,7 +14,7 @@ namespace boost { namespace serialization { template -void save(Archive& ar, const hpp::fcl::CollisionGeometry& collision_geometry, +void save(Archive& ar, const coal::CollisionGeometry& collision_geometry, const unsigned int /*version*/) { ar& make_nvp("aabb_center", collision_geometry.aabb_center); ar& make_nvp("aabb_radius", collision_geometry.aabb_radius); @@ -25,7 +25,7 @@ void save(Archive& ar, const hpp::fcl::CollisionGeometry& collision_geometry, } template -void load(Archive& ar, hpp::fcl::CollisionGeometry& collision_geometry, +void load(Archive& ar, coal::CollisionGeometry& collision_geometry, const unsigned int /*version*/) { ar >> make_nvp("aabb_center", collision_geometry.aabb_center); ar >> make_nvp("aabb_radius", collision_geometry.aabb_radius); @@ -36,13 +36,12 @@ void load(Archive& ar, hpp::fcl::CollisionGeometry& collision_geometry, collision_geometry.user_data = NULL; // no way to recover this } -HPP_FCL_SERIALIZATION_SPLIT(hpp::fcl::CollisionGeometry) +HPP_FCL_SERIALIZATION_SPLIT(coal::CollisionGeometry) } // namespace serialization } // namespace boost -namespace hpp { -namespace fcl { +namespace coal { // fwd declaration template @@ -89,7 +88,6 @@ struct register_type { } }; } // namespace serialization -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_SERIALIZATION_COLLISION_OBJECT_H +#endif // ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H diff --git a/include/hpp/fcl/serialization/contact_patch.h b/include/hpp/fcl/serialization/contact_patch.h index 71d984ca9..e9024e157 100644 --- a/include/hpp/fcl/serialization/contact_patch.h +++ b/include/hpp/fcl/serialization/contact_patch.h @@ -2,8 +2,8 @@ // Copyright (c) 2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_CONTACT_PATCH_H -#define HPP_FCL_SERIALIZATION_CONTACT_PATCH_H +#ifndef COAL_SERIALIZATION_CONTACT_PATCH_H +#define COAL_SERIALIZATION_CONTACT_PATCH_H #include "hpp/fcl/collision_data.h" #include "hpp/fcl/serialization/fwd.h" @@ -13,9 +13,9 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::ContactPatch& contact_patch, +void serialize(Archive& ar, coal::ContactPatch& contact_patch, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; typedef Eigen::Matrix PolygonPoints; size_t patch_size = contact_patch.size(); @@ -36,9 +36,9 @@ void serialize(Archive& ar, hpp::fcl::ContactPatch& contact_patch, } template -void serialize(Archive& ar, hpp::fcl::ContactPatchRequest& request, +void serialize(Archive& ar, coal::ContactPatchRequest& request, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; ar& make_nvp("max_num_patch", request.max_num_patch); @@ -54,9 +54,9 @@ void serialize(Archive& ar, hpp::fcl::ContactPatchRequest& request, } template -void serialize(Archive& ar, hpp::fcl::ContactPatchResult& result, +void serialize(Archive& ar, coal::ContactPatchResult& result, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; size_t num_patches = result.numContactPatches(); ar& make_nvp("num_patches", num_patches); @@ -84,4 +84,4 @@ void serialize(Archive& ar, hpp::fcl::ContactPatchResult& result, } // namespace serialization } // namespace boost -#endif // HPP_FCL_SERIALIZATION_CONTACT_PATCH_H +#endif // COAL_SERIALIZATION_CONTACT_PATCH_H diff --git a/include/hpp/fcl/serialization/convex.h b/include/hpp/fcl/serialization/convex.h index 7bbf37b5d..390007e2d 100644 --- a/include/hpp/fcl/serialization/convex.h +++ b/include/hpp/fcl/serialization/convex.h @@ -2,8 +2,8 @@ // Copyright (c) 2022-2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_CONVEX_H -#define HPP_FCL_SERIALIZATION_CONVEX_H +#ifndef COAL_SERIALIZATION_CONVEX_H +#define COAL_SERIALIZATION_CONVEX_H #include "hpp/fcl/shape/convex.h" @@ -17,19 +17,19 @@ namespace boost { namespace serialization { namespace internal { -struct ConvexBaseAccessor : hpp::fcl::ConvexBase { - typedef hpp::fcl::ConvexBase Base; +struct ConvexBaseAccessor : coal::ConvexBase { + typedef coal::ConvexBase Base; }; } // namespace internal template -void serialize(Archive& ar, hpp::fcl::ConvexBase& convex_base, +void serialize(Archive& ar, coal::ConvexBase& convex_base, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; - ar& make_nvp("base", boost::serialization::base_object( - convex_base)); + ar& make_nvp("base", + boost::serialization::base_object(convex_base)); const unsigned int num_points_previous = convex_base.num_points; ar& make_nvp("num_points", convex_base.num_points); @@ -115,17 +115,17 @@ void serialize(Archive& ar, hpp::fcl::ConvexBase& convex_base, namespace internal { template -struct ConvexAccessor : hpp::fcl::Convex { - typedef hpp::fcl::Convex Base; +struct ConvexAccessor : coal::Convex { + typedef coal::Convex Base; using Base::fillNeighbors; }; } // namespace internal template -void serialize(Archive& ar, hpp::fcl::Convex& convex_, +void serialize(Archive& ar, coal::Convex& convex_, const unsigned int /*version*/) { - using namespace hpp::fcl; + using namespace coal; typedef internal::ConvexAccessor Accessor; Accessor& convex = reinterpret_cast(convex_); @@ -148,22 +148,20 @@ void serialize(Archive& ar, hpp::fcl::Convex& convex_, } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(hpp::fcl::Convex) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) -namespace hpp { -namespace fcl { +namespace coal { // namespace internal { // template -// struct memory_footprint_evaluator< ::hpp::fcl::BVHModel > { -// static size_t run(const ::hpp::fcl::BVHModel &bvh_model) { +// struct memory_footprint_evaluator< ::coal::BVHModel > { +// static size_t run(const ::coal::BVHModel &bvh_model) { // return static_cast(bvh_model.memUsage(false)); // } // }; // } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_SERIALIZATION_CONVEX_H +#endif // ifndef COAL_SERIALIZATION_CONVEX_H diff --git a/include/hpp/fcl/serialization/eigen.h b/include/hpp/fcl/serialization/eigen.h index fa7c49dc9..16c053dc0 100644 --- a/include/hpp/fcl/serialization/eigen.h +++ b/include/hpp/fcl/serialization/eigen.h @@ -8,8 +8,8 @@ Copyright (c) 2015 Michael Tao */ -#ifndef HPP_FCL_SERIALIZATION_EIGEN_H -#define HPP_FCL_SERIALIZATION_EIGEN_H +#ifndef COAL_SERIALIZATION_EIGEN_H +#define COAL_SERIALIZATION_EIGEN_H #ifndef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION @@ -113,4 +113,4 @@ void serialize(Archive& ar, // #endif // ifned HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION -#endif // ifndef HPP_FCL_SERIALIZATION_EIGEN_H +#endif // ifndef COAL_SERIALIZATION_EIGEN_H diff --git a/include/hpp/fcl/serialization/fwd.h b/include/hpp/fcl/serialization/fwd.h index abb1943fc..6657898e7 100644 --- a/include/hpp/fcl/serialization/fwd.h +++ b/include/hpp/fcl/serialization/fwd.h @@ -2,8 +2,8 @@ // Copyright (c) 2021-2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_FWD_H -#define HPP_FCL_SERIALIZATION_FWD_H +#ifndef COAL_SERIALIZATION_FWD_H +#define COAL_SERIALIZATION_FWD_H #include @@ -58,8 +58,7 @@ } \ /**/ -namespace hpp { -namespace fcl { +namespace coal { namespace serialization { namespace detail { @@ -92,12 +91,10 @@ struct register_type { static void on(Archive& /*ar*/) {} }; } // namespace serialization -} // namespace fcl -} // namespace hpp +} // namespace coal #define HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ - namespace hpp { \ - namespace fcl { \ + namespace coal { \ namespace serialization { \ namespace detail { \ template <> \ @@ -111,7 +108,6 @@ struct register_type { .init(); \ } \ } \ - } \ } -#endif // ifndef HPP_FCL_SERIALIZATION_FWD_H +#endif // ifndef COAL_SERIALIZATION_FWD_H diff --git a/include/hpp/fcl/serialization/geometric_shapes.h b/include/hpp/fcl/serialization/geometric_shapes.h index 9122a2011..7edd15600 100644 --- a/include/hpp/fcl/serialization/geometric_shapes.h +++ b/include/hpp/fcl/serialization/geometric_shapes.h @@ -2,8 +2,8 @@ // Copyright (c) 2021-2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H -#define HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H +#ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H +#define COAL_SERIALIZATION_GEOMETRIC_SHAPES_H #include "hpp/fcl/shape/geometric_shapes.h" #include "hpp/fcl/serialization/fwd.h" @@ -13,12 +13,12 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::ShapeBase& shape_base, +void serialize(Archive& ar, coal::ShapeBase& shape_base, const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object( - shape_base)); - ::hpp::fcl::FCL_REAL radius = shape_base.getSweptSphereRadius(); + ar& make_nvp( + "base", + boost::serialization::base_object(shape_base)); + ::coal::FCL_REAL radius = shape_base.getSweptSphereRadius(); ar& make_nvp("swept_sphere_radius", radius); if (Archive::is_loading::value) { @@ -27,80 +27,77 @@ void serialize(Archive& ar, hpp::fcl::ShapeBase& shape_base, } template -void serialize(Archive& ar, hpp::fcl::TriangleP& triangle, +void serialize(Archive& ar, coal::TriangleP& triangle, const unsigned int /*version*/) { - ar& make_nvp( - "base", boost::serialization::base_object(triangle)); + ar& make_nvp("base", + boost::serialization::base_object(triangle)); ar& make_nvp("a", triangle.a); ar& make_nvp("b", triangle.b); ar& make_nvp("c", triangle.c); } template -void serialize(Archive& ar, hpp::fcl::Box& box, - const unsigned int /*version*/) { - ar& make_nvp("base", - boost::serialization::base_object(box)); +void serialize(Archive& ar, coal::Box& box, const unsigned int /*version*/) { + ar& make_nvp("base", boost::serialization::base_object(box)); ar& make_nvp("halfSide", box.halfSide); } template -void serialize(Archive& ar, hpp::fcl::Sphere& sphere, +void serialize(Archive& ar, coal::Sphere& sphere, const unsigned int /*version*/) { ar& make_nvp("base", - boost::serialization::base_object(sphere)); + boost::serialization::base_object(sphere)); ar& make_nvp("radius", sphere.radius); } template -void serialize(Archive& ar, hpp::fcl::Ellipsoid& ellipsoid, +void serialize(Archive& ar, coal::Ellipsoid& ellipsoid, const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( - ellipsoid)); + ar& make_nvp("base", + boost::serialization::base_object(ellipsoid)); ar& make_nvp("radii", ellipsoid.radii); } template -void serialize(Archive& ar, hpp::fcl::Capsule& capsule, +void serialize(Archive& ar, coal::Capsule& capsule, const unsigned int /*version*/) { ar& make_nvp("base", - boost::serialization::base_object(capsule)); + boost::serialization::base_object(capsule)); ar& make_nvp("radius", capsule.radius); ar& make_nvp("halfLength", capsule.halfLength); } template -void serialize(Archive& ar, hpp::fcl::Cone& cone, - const unsigned int /*version*/) { +void serialize(Archive& ar, coal::Cone& cone, const unsigned int /*version*/) { ar& make_nvp("base", - boost::serialization::base_object(cone)); + boost::serialization::base_object(cone)); ar& make_nvp("radius", cone.radius); ar& make_nvp("halfLength", cone.halfLength); } template -void serialize(Archive& ar, hpp::fcl::Cylinder& cylinder, +void serialize(Archive& ar, coal::Cylinder& cylinder, const unsigned int /*version*/) { - ar& make_nvp( - "base", boost::serialization::base_object(cylinder)); + ar& make_nvp("base", + boost::serialization::base_object(cylinder)); ar& make_nvp("radius", cylinder.radius); ar& make_nvp("halfLength", cylinder.halfLength); } template -void serialize(Archive& ar, hpp::fcl::Halfspace& half_space, +void serialize(Archive& ar, coal::Halfspace& half_space, const unsigned int /*version*/) { - ar& make_nvp("base", boost::serialization::base_object( - half_space)); + ar& make_nvp("base", + boost::serialization::base_object(half_space)); ar& make_nvp("n", half_space.n); ar& make_nvp("d", half_space.d); } template -void serialize(Archive& ar, hpp::fcl::Plane& plane, +void serialize(Archive& ar, coal::Plane& plane, const unsigned int /*version*/) { ar& make_nvp("base", - boost::serialization::base_object(plane)); + boost::serialization::base_object(plane)); ar& make_nvp("n", plane.n); ar& make_nvp("d", plane.d); } @@ -108,16 +105,16 @@ void serialize(Archive& ar, hpp::fcl::Plane& plane, } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::ShapeBase) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::CollisionGeometry) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::TriangleP) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Box) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Sphere) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Ellipsoid) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Capsule) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cone) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Cylinder) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Halfspace) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::Plane) - -#endif // ifndef HPP_FCL_SERIALIZATION_GEOMETRIC_SHAPES_H +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::ShapeBase) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionGeometry) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::TriangleP) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Box) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Sphere) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Ellipsoid) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Capsule) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Cone) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Cylinder) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Halfspace) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Plane) + +#endif // ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h index 4e456a78a..544de9d2e 100644 --- a/include/hpp/fcl/serialization/hfield.h +++ b/include/hpp/fcl/serialization/hfield.h @@ -2,8 +2,8 @@ // Copyright (c) 2021-2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_HFIELD_H -#define HPP_FCL_SERIALIZATION_HFIELD_H +#ifndef COAL_SERIALIZATION_HFIELD_H +#define COAL_SERIALIZATION_HFIELD_H #include "hpp/fcl/hfield.h" @@ -14,7 +14,7 @@ namespace boost { namespace serialization { template -void serialize(Archive &ar, hpp::fcl::HFNodeBase &node, +void serialize(Archive &ar, coal::HFNodeBase &node, const unsigned int /*version*/) { ar &make_nvp("first_child", node.first_child); ar &make_nvp("x_id", node.x_id); @@ -26,17 +26,17 @@ void serialize(Archive &ar, hpp::fcl::HFNodeBase &node, } template -void serialize(Archive &ar, hpp::fcl::HFNode &node, +void serialize(Archive &ar, coal::HFNode &node, const unsigned int /*version*/) { ar &make_nvp("base", - boost::serialization::base_object(node)); + boost::serialization::base_object(node)); ar &make_nvp("bv", node.bv); } namespace internal { template -struct HeightFieldAccessor : hpp::fcl::HeightField { - typedef hpp::fcl::HeightField Base; +struct HeightFieldAccessor : coal::HeightField { + typedef coal::HeightField Base; using Base::bvs; using Base::heights; using Base::max_height; @@ -50,11 +50,11 @@ struct HeightFieldAccessor : hpp::fcl::HeightField { } // namespace internal template -void serialize(Archive &ar, hpp::fcl::HeightField &hf_model, +void serialize(Archive &ar, coal::HeightField &hf_model, const unsigned int /*version*/) { ar &make_nvp( "base", - boost::serialization::base_object(hf_model)); + boost::serialization::base_object(hf_model)); typedef internal::HeightFieldAccessor Accessor; Accessor &access = reinterpret_cast(hf_model); @@ -73,9 +73,8 @@ void serialize(Archive &ar, hpp::fcl::HeightField &hf_model, } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::AABB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::HeightField<::hpp::fcl::OBB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT( - ::hpp::fcl::HeightField<::hpp::fcl::OBBRSS>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::AABB>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBB>) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBBRSS>) -#endif // ifndef HPP_FCL_SERIALIZATION_HFIELD_H +#endif // ifndef COAL_SERIALIZATION_HFIELD_H diff --git a/include/hpp/fcl/serialization/kDOP.h b/include/hpp/fcl/serialization/kDOP.h index 36edda0d3..954df2993 100644 --- a/include/hpp/fcl/serialization/kDOP.h +++ b/include/hpp/fcl/serialization/kDOP.h @@ -2,8 +2,8 @@ // Copyright (c) 2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_kDOP_H -#define HPP_FCL_SERIALIZATION_kDOP_H +#ifndef COAL_SERIALIZATION_kDOP_H +#define COAL_SERIALIZATION_kDOP_H #include "hpp/fcl/BV/kDOP.h" @@ -14,14 +14,14 @@ namespace serialization { namespace internal { template -struct KDOPAccessor : hpp::fcl::KDOP { - typedef hpp::fcl::KDOP Base; +struct KDOPAccessor : coal::KDOP { + typedef coal::KDOP Base; using Base::dist_; }; } // namespace internal template -void serialize(Archive& ar, hpp::fcl::KDOP& bv_, +void serialize(Archive& ar, coal::KDOP& bv_, const unsigned int /*version*/) { typedef internal::KDOPAccessor Accessor; Accessor& access = reinterpret_cast(bv_); @@ -31,4 +31,4 @@ void serialize(Archive& ar, hpp::fcl::KDOP& bv_, } // namespace serialization } // namespace boost -#endif // HPP_FCL_SERIALIZATION_kDOP_H +#endif // COAL_SERIALIZATION_kDOP_H diff --git a/include/hpp/fcl/serialization/kIOS.h b/include/hpp/fcl/serialization/kIOS.h index 8ed8d8725..27a0e5a69 100644 --- a/include/hpp/fcl/serialization/kIOS.h +++ b/include/hpp/fcl/serialization/kIOS.h @@ -2,8 +2,8 @@ // Copyright (c) 2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_kIOS_H -#define HPP_FCL_SERIALIZATION_kIOS_H +#ifndef COAL_SERIALIZATION_kIOS_H +#define COAL_SERIALIZATION_kIOS_H #include "hpp/fcl/BV/kIOS.h" @@ -14,19 +14,18 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::kIOS& bv, const unsigned int version) { +void serialize(Archive& ar, coal::kIOS& bv, const unsigned int version) { split_free(ar, bv, version); } template -void save(Archive& ar, const hpp::fcl::kIOS& bv, - const unsigned int /*version*/) { +void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) { // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres ar& make_nvp("num_spheres", bv.num_spheres); - std::array centers{}; - std::array radii; - for (std::size_t i = 0; i < hpp::fcl::kIOS::max_num_spheres; ++i) { + std::array centers{}; + std::array radii; + for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { centers[i] = bv.spheres[i].o; radii[i] = bv.spheres[i].r; } @@ -37,14 +36,14 @@ void save(Archive& ar, const hpp::fcl::kIOS& bv, } template -void load(Archive& ar, hpp::fcl::kIOS& bv, const unsigned int /*version*/) { +void load(Archive& ar, coal::kIOS& bv, const unsigned int /*version*/) { ar >> make_nvp("num_spheres", bv.num_spheres); - std::array centers; - std::array radii; + std::array centers; + std::array radii; ar >> make_nvp("centers", make_array(centers.data(), centers.size())); ar >> make_nvp("radii", make_array(radii.data(), radii.size())); - for (std::size_t i = 0; i < hpp::fcl::kIOS::max_num_spheres; ++i) { + for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { bv.spheres[i].o = centers[i]; bv.spheres[i].r = radii[i]; } @@ -55,4 +54,4 @@ void load(Archive& ar, hpp::fcl::kIOS& bv, const unsigned int /*version*/) { } // namespace serialization } // namespace boost -#endif // HPP_FCL_SERIALIZATION_kIOS_H +#endif // COAL_SERIALIZATION_kIOS_H diff --git a/include/hpp/fcl/serialization/memory.h b/include/hpp/fcl/serialization/memory.h index e1e3d320f..ee278bced 100644 --- a/include/hpp/fcl/serialization/memory.h +++ b/include/hpp/fcl/serialization/memory.h @@ -2,11 +2,10 @@ // Copyright (c) 2021 INRIA // -#ifndef HPP_FCL_SERIALIZATION_MEMORY_H -#define HPP_FCL_SERIALIZATION_MEMORY_H +#ifndef COAL_SERIALIZATION_MEMORY_H +#define COAL_SERIALIZATION_MEMORY_H -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template @@ -26,7 +25,6 @@ size_t computeMemoryFootprint(const T &object) { return internal::memory_footprint_evaluator::run(object); } -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_SERIALIZATION_MEMORY_H +#endif // ifndef COAL_SERIALIZATION_MEMORY_H diff --git a/include/hpp/fcl/serialization/octree.h b/include/hpp/fcl/serialization/octree.h index 8ea430022..42a8afcdc 100644 --- a/include/hpp/fcl/serialization/octree.h +++ b/include/hpp/fcl/serialization/octree.h @@ -2,8 +2,8 @@ // Copyright (c) 2023-2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_OCTREE_H -#define HPP_FCL_SERIALIZATION_OCTREE_H +#ifndef COAL_SERIALIZATION_OCTREE_H +#define COAL_SERIALIZATION_OCTREE_H #include #include @@ -17,8 +17,8 @@ namespace boost { namespace serialization { namespace internal { -struct OcTreeAccessor : hpp::fcl::OcTree { - typedef hpp::fcl::OcTree Base; +struct OcTreeAccessor : coal::OcTree { + typedef coal::OcTree Base; using Base::default_occupancy; using Base::free_threshold; using Base::occupancy_threshold; @@ -27,14 +27,14 @@ struct OcTreeAccessor : hpp::fcl::OcTree { } // namespace internal template -void save_construct_data(Archive &ar, const hpp::fcl::OcTree *octree_ptr, +void save_construct_data(Archive &ar, const coal::OcTree *octree_ptr, const unsigned int /*version*/) { const double resolution = octree_ptr->getResolution(); ar << make_nvp("resolution", resolution); } template -void save(Archive &ar, const hpp::fcl::OcTree &octree, +void save(Archive &ar, const coal::OcTree &octree, const unsigned int /*version*/) { typedef internal::OcTreeAccessor Accessor; const Accessor &access = reinterpret_cast(octree); @@ -49,23 +49,22 @@ void save(Archive &ar, const hpp::fcl::OcTree &octree, ar << make_nvp("tree_data", make_array(stream_str.c_str(), stream_str.size())); - ar << make_nvp("base", base_object(octree)); + ar << make_nvp("base", base_object(octree)); ar << make_nvp("default_occupancy", access.default_occupancy); ar << make_nvp("occupancy_threshold", access.occupancy_threshold); ar << make_nvp("free_threshold", access.free_threshold); } template -void load_construct_data(Archive &ar, hpp::fcl::OcTree *octree_ptr, +void load_construct_data(Archive &ar, coal::OcTree *octree_ptr, const unsigned int /*version*/) { double resolution; ar >> make_nvp("resolution", resolution); - new (octree_ptr) hpp::fcl::OcTree(resolution); + new (octree_ptr) coal::OcTree(resolution); } template -void load(Archive &ar, hpp::fcl::OcTree &octree, - const unsigned int /*version*/) { +void load(Archive &ar, coal::OcTree &octree, const unsigned int /*version*/) { typedef internal::OcTreeAccessor Accessor; Accessor &access = reinterpret_cast(octree); @@ -83,21 +82,20 @@ void load(Archive &ar, hpp::fcl::OcTree &octree, access.tree = std::shared_ptr( dynamic_cast(new_tree)); - ar >> make_nvp("base", base_object(octree)); + ar >> make_nvp("base", base_object(octree)); ar >> make_nvp("default_occupancy", access.default_occupancy); ar >> make_nvp("occupancy_threshold", access.occupancy_threshold); ar >> make_nvp("free_threshold", access.free_threshold); } template -void serialize(Archive &ar, hpp::fcl::OcTree &octree, - const unsigned int version) { +void serialize(Archive &ar, coal::OcTree &octree, const unsigned int version) { split_free(ar, octree, version); } } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::hpp::fcl::OcTree) +HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree) -#endif // ifndef HPP_FCL_SERIALIZATION_OCTREE_H +#endif // ifndef COAL_SERIALIZATION_OCTREE_H diff --git a/include/hpp/fcl/serialization/quadrilateral.h b/include/hpp/fcl/serialization/quadrilateral.h index 3694607ea..eb2792e98 100644 --- a/include/hpp/fcl/serialization/quadrilateral.h +++ b/include/hpp/fcl/serialization/quadrilateral.h @@ -2,8 +2,8 @@ // Copyright (c) 2022 INRIA // -#ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H -#define HPP_FCL_SERIALIZATION_QUADRILATERAL_H +#ifndef COAL_SERIALIZATION_QUADRILATERAL_H +#define COAL_SERIALIZATION_QUADRILATERAL_H #include "hpp/fcl/data_types.h" #include "hpp/fcl/serialization/fwd.h" @@ -12,7 +12,7 @@ namespace boost { namespace serialization { template -void serialize(Archive &ar, hpp::fcl::Quadrilateral &quadrilateral, +void serialize(Archive &ar, coal::Quadrilateral &quadrilateral, const unsigned int /*version*/) { ar &make_nvp("p0", quadrilateral[0]); ar &make_nvp("p1", quadrilateral[1]); @@ -23,4 +23,4 @@ void serialize(Archive &ar, hpp::fcl::Quadrilateral &quadrilateral, } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_QUADRILATERAL_H +#endif // ifndef COAL_SERIALIZATION_QUADRILATERAL_H diff --git a/include/hpp/fcl/serialization/serializer.h b/include/hpp/fcl/serialization/serializer.h index d0d33cd83..8990cc3fa 100644 --- a/include/hpp/fcl/serialization/serializer.h +++ b/include/hpp/fcl/serialization/serializer.h @@ -2,93 +2,91 @@ // Copyright (c) 2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_SERIALIZER_H -#define HPP_FCL_SERIALIZATION_SERIALIZER_H +#ifndef COAL_SERIALIZATION_SERIALIZER_H +#define COAL_SERIALIZATION_SERIALIZER_H #include "hpp/fcl/serialization/archive.h" -namespace hpp { -namespace fcl { +namespace coal { namespace serialization { struct Serializer { /// \brief Loads an object from a text file. template static void loadFromText(T& object, const std::string& filename) { - ::hpp::fcl::serialization::loadFromText(object, filename); + ::coal::serialization::loadFromText(object, filename); } /// \brief Saves an object as a text file. template static void saveToText(const T& object, const std::string& filename) { - ::hpp::fcl::serialization::saveToText(object, filename); + ::coal::serialization::saveToText(object, filename); } /// \brief Loads an object from a stream string. template static void loadFromStringStream(T& object, std::istringstream& is) { - ::hpp::fcl::serialization::loadFromStringStream(object, is); + ::coal::serialization::loadFromStringStream(object, is); } /// \brief Saves an object to a string stream. template static void saveToStringStream(const T& object, std::stringstream& ss) { - ::hpp::fcl::serialization::saveToStringStream(object, ss); + ::coal::serialization::saveToStringStream(object, ss); } /// \brief Loads an object from a string. template static void loadFromString(T& object, const std::string& str) { - ::hpp::fcl::serialization::loadFromString(object, str); + ::coal::serialization::loadFromString(object, str); } /// \brief Saves a Derived object to a string. template static std::string saveToString(const T& object) { - return ::hpp::fcl::serialization::saveToString(object); + return ::coal::serialization::saveToString(object); } /// \brief Loads an object from an XML file. template static void loadFromXML(T& object, const std::string& filename, const std::string& tag_name) { - ::hpp::fcl::serialization::loadFromXML(object, filename, tag_name); + ::coal::serialization::loadFromXML(object, filename, tag_name); } /// \brief Saves an object as an XML file. template static void saveToXML(const T& object, const std::string& filename, const std::string& tag_name) { - ::hpp::fcl::serialization::saveToXML(object, filename, tag_name); + ::coal::serialization::saveToXML(object, filename, tag_name); } /// \brief Loads a Derived object from an binary file. template static void loadFromBinary(T& object, const std::string& filename) { - ::hpp::fcl::serialization::loadFromBinary(object, filename); + ::coal::serialization::loadFromBinary(object, filename); } /// \brief Saves a Derived object as an binary file. template static void saveToBinary(const T& object, const std::string& filename) { - ::hpp::fcl::serialization::saveToBinary(object, filename); + ::coal::serialization::saveToBinary(object, filename); } /// \brief Loads an object from a binary buffer. template static void loadFromBuffer(T& object, boost::asio::streambuf& container) { - ::hpp::fcl::serialization::loadFromBuffer(object, container); + ::coal::serialization::loadFromBuffer(object, container); } /// \brief Saves an object as a binary buffer. template static void saveToBuffer(const T& object, boost::asio::streambuf& container) { - ::hpp::fcl::serialization::saveToBuffer(object, container); + ::coal::serialization::saveToBuffer(object, container); } }; } // namespace serialization -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_SERIALIZATION_SERIALIZER_H +#endif // ifndef COAL_SERIALIZATION_SERIALIZER_H diff --git a/include/hpp/fcl/serialization/transform.h b/include/hpp/fcl/serialization/transform.h index 4c251c447..0f5c11e1f 100644 --- a/include/hpp/fcl/serialization/transform.h +++ b/include/hpp/fcl/serialization/transform.h @@ -2,8 +2,8 @@ // Copyright (c) 2024 INRIA // -#ifndef HPP_FCL_SERIALIZATION_TRANSFORM_H -#define HPP_FCL_SERIALIZATION_TRANSFORM_H +#ifndef COAL_SERIALIZATION_TRANSFORM_H +#define COAL_SERIALIZATION_TRANSFORM_H #include "hpp/fcl/math/transform.h" #include "hpp/fcl/serialization/fwd.h" @@ -12,7 +12,7 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, hpp::fcl::Transform3f& tf, +void serialize(Archive& ar, coal::Transform3f& tf, const unsigned int /*version*/) { ar& make_nvp("R", tf.rotation()); ar& make_nvp("T", tf.translation()); @@ -21,4 +21,4 @@ void serialize(Archive& ar, hpp::fcl::Transform3f& tf, } // namespace serialization } // namespace boost -#endif // HPP_FCL_SERIALIZATION_TRANSFORM_H +#endif // COAL_SERIALIZATION_TRANSFORM_H diff --git a/include/hpp/fcl/serialization/triangle.h b/include/hpp/fcl/serialization/triangle.h index 44c532e72..2ca478543 100644 --- a/include/hpp/fcl/serialization/triangle.h +++ b/include/hpp/fcl/serialization/triangle.h @@ -2,8 +2,8 @@ // Copyright (c) 2021-2022 INRIA // -#ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H -#define HPP_FCL_SERIALIZATION_TRIANGLE_H +#ifndef COAL_SERIALIZATION_TRIANGLE_H +#define COAL_SERIALIZATION_TRIANGLE_H #include "hpp/fcl/data_types.h" #include "hpp/fcl/serialization/fwd.h" @@ -12,7 +12,7 @@ namespace boost { namespace serialization { template -void serialize(Archive &ar, hpp::fcl::Triangle &triangle, +void serialize(Archive &ar, coal::Triangle &triangle, const unsigned int /*version*/) { ar &make_nvp("p0", triangle[0]); ar &make_nvp("p1", triangle[1]); @@ -22,4 +22,4 @@ void serialize(Archive &ar, hpp::fcl::Triangle &triangle, } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_TRIANGLE_H +#endif // ifndef COAL_SERIALIZATION_TRIANGLE_H diff --git a/include/hpp/fcl/shape/convex.h b/include/hpp/fcl/shape/convex.h index a69d5c657..fc2f24814 100644 --- a/include/hpp/fcl/shape/convex.h +++ b/include/hpp/fcl/shape/convex.h @@ -35,13 +35,12 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_SHAPE_CONVEX_H -#define HPP_FCL_SHAPE_CONVEX_H +#ifndef COAL_SHAPE_CONVEX_H +#define COAL_SHAPE_CONVEX_H #include "hpp/fcl/shape/geometric_shapes.h" -namespace hpp { -namespace fcl { +namespace coal { /// @brief Convex polytope /// @tparam PolygonT the polygon class. It must have method \c size() and @@ -106,9 +105,7 @@ class Convex : public ConvexBase { void fillNeighbors(); }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #include "hpp/fcl/shape/details/convex.hxx" diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx index 373921e6f..6dd170be5 100644 --- a/include/hpp/fcl/shape/details/convex.hxx +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -34,15 +34,14 @@ /** \author Joseph Mirabel */ -#ifndef HPP_FCL_SHAPE_CONVEX_HXX -#define HPP_FCL_SHAPE_CONVEX_HXX +#ifndef COAL_SHAPE_CONVEX_HXX +#define COAL_SHAPE_CONVEX_HXX #include #include #include -namespace hpp { -namespace fcl { +namespace coal { template Convex::Convex(std::shared_ptr> points_, @@ -279,8 +278,6 @@ void Convex::fillNeighbors() { assert(p_nneighbors == nneighbors_->data() + c_nneighbors); } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h index dec758e54..c6d51bc7a 100644 --- a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h +++ b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h @@ -35,15 +35,14 @@ /** \author Jia Pan */ -#ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H -#define GEOMETRIC_SHAPE_TO_BVH_MODEL_H +#ifndef COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H +#define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H #include #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Generate BVH model from box template @@ -350,8 +349,6 @@ void generateBVHModel(BVHModel& model, const Cone& shape, generateBVHModel(model, shape, pose, tot, h_num); } -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index fada3e07a..b0c1481f9 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_GEOMETRIC_SHAPES_H -#define HPP_FCL_GEOMETRIC_SHAPES_H +#ifndef COAL_GEOMETRIC_SHAPES_H +#define COAL_GEOMETRIC_SHAPES_H #include #include @@ -52,8 +52,7 @@ class Qhull; } #endif -namespace hpp { -namespace fcl { +namespace coal { /// @brief Base class for all basic geometric shapes class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { @@ -1047,8 +1046,6 @@ class HPP_FCL_DLLAPI Plane : public ShapeBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h index e4997d09a..899711315 100644 --- a/include/hpp/fcl/shape/geometric_shapes_traits.h +++ b/include/hpp/fcl/shape/geometric_shapes_traits.h @@ -34,13 +34,12 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H -#define HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H +#ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H +#define COAL_GEOMETRIC_SHAPES_TRAITS_H #include -namespace hpp { -namespace fcl { +namespace coal { struct shape_traits_base { enum { @@ -154,7 +153,6 @@ struct shape_traits : shape_traits_base { }; }; -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H +#endif // ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H diff --git a/include/hpp/fcl/shape/geometric_shapes_utility.h b/include/hpp/fcl/shape/geometric_shapes_utility.h index f46ce042e..05ce509c1 100644 --- a/include/hpp/fcl/shape/geometric_shapes_utility.h +++ b/include/hpp/fcl/shape/geometric_shapes_utility.h @@ -35,16 +35,15 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H -#define HPP_FCL_GEOMETRIC_SHAPES_UTILITY_H +#ifndef COAL_GEOMETRIC_SHAPES_UTILITY_H +#define COAL_GEOMETRIC_SHAPES_UTILITY_H #include #include #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @cond IGNORE namespace details { @@ -258,8 +257,6 @@ HPP_FCL_DLLAPI Plane transform(const Plane& a, const Transform3f& tf); HPP_FCL_DLLAPI std::array transformToHalfspaces( const Plane& a, const Transform3f& tf); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif diff --git a/include/hpp/fcl/timings.h b/include/hpp/fcl/timings.h index 961a26752..5316a832a 100644 --- a/include/hpp/fcl/timings.h +++ b/include/hpp/fcl/timings.h @@ -2,8 +2,8 @@ // Copyright (c) 2021-2023 INRIA // -#ifndef HPP_FCL_TIMINGS_FWD_H -#define HPP_FCL_TIMINGS_FWD_H +#ifndef COAL_TIMINGS_FWD_H +#define COAL_TIMINGS_FWD_H #include "hpp/fcl/fwd.hh" @@ -11,8 +11,7 @@ #include #endif -namespace hpp { -namespace fcl { +namespace coal { struct CPUTimes { double wall; @@ -108,7 +107,6 @@ struct HPP_FCL_DLLAPI Timer { #endif }; -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_TIMINGS_FWD_H +#endif // ifndef COAL_TIMINGS_FWD_H diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index f6f9592ae..dd99421ad 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -63,7 +63,7 @@ HPP_FCL_COMPILER_DIAGNOSTIC_POP #include "broadphase_callbacks.hh" #include "broadphase_collision_manager.hh" -using namespace hpp::fcl; +using namespace coal; HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index b6ed07d7b..c5d64f37f 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -32,8 +32,8 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH -#define HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH +#ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH +#define COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH #include @@ -47,8 +47,7 @@ #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_callbacks.h" #endif -namespace hpp { -namespace fcl { +namespace coal { struct CollisionCallBackBaseWrapper : CollisionCallBackBase, bp::wrapper { @@ -108,7 +107,6 @@ struct DistanceCallBackBaseWrapper : DistanceCallBackBase, } }; // DistanceCallBackBaseWrapper -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH +#endif // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_CALLBACKS_HH diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index 796dbd475..70578b439 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -32,8 +32,8 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH -#define HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH +#ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH +#define COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH #include @@ -51,8 +51,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { struct BroadPhaseCollisionManagerWrapper : BroadPhaseCollisionManager, @@ -228,7 +227,6 @@ struct BroadPhaseCollisionManagerWrapper }; // BroadPhaseCollisionManagerWrapper -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH +#endif // ifndef COAL_PYTHON_BROADPHASE_BROADPHASE_COLLISION_MANAGER_HH diff --git a/python/broadphase/fwd.hh b/python/broadphase/fwd.hh index 2f8e6bc57..c2cc67082 100644 --- a/python/broadphase/fwd.hh +++ b/python/broadphase/fwd.hh @@ -2,19 +2,17 @@ // Copyright (c) 2022 INRIA // -#ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH -#define HPP_FCL_PYTHON_BROADPHASE_FWD_HH +#ifndef COAL_PYTHON_BROADPHASE_FWD_HH +#define COAL_PYTHON_BROADPHASE_FWD_HH #include "hppfcl/fwd.hh" -namespace hpp { -namespace fcl { +namespace coal { namespace python { void exposeBroadPhase(); } -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_BROADPHASE_FWD_HH +#endif // ifndef COAL_PYTHON_BROADPHASE_FWD_HH diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 57ebb1046..cec7ea32b 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -68,8 +68,8 @@ #endif using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; namespace bp = boost::python; @@ -320,8 +320,7 @@ void exposeShapes() { bp::return_internal_reference<>()) .def("points", &ConvexBaseWrapper::point, bp::args("self", "index"), "Retrieve the point given by its index.", - ::hpp::fcl::python::deprecated_member< - bp::return_internal_reference<>>()) + ::coal::python::deprecated_member>()) .def("points", &ConvexBaseWrapper::points, bp::args("self"), "Retrieve all the points.", bp::with_custodian_and_ward_postcall<0, 1>()) @@ -644,8 +643,7 @@ void exposeCollisionGeometries() { bp::return_internal_reference<>()) .def("vertices", &BVHModelBaseWrapper::vertex, bp::args("self", "index"), "Retrieve the vertex given by its index.", - ::hpp::fcl::python::deprecated_member< - bp::return_internal_reference<>>()) + ::coal::python::deprecated_member>()) .def("vertices", &BVHModelBaseWrapper::vertices, bp::args("self"), "Retrieve all the vertices.", bp::with_custodian_and_ward_postcall<0, 1>()) diff --git a/python/collision.cc b/python/collision.cc index a759b193a..a3a47deb4 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -54,8 +54,8 @@ HPP_FCL_COMPILER_DIAGNOSTIC_POP #include "../doc/python/doxygen-boost.hh" using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; diff --git a/python/contact_patch.cc b/python/contact_patch.cc index 66099b3b3..d09c9f246 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -51,8 +51,8 @@ #include "../doc/python/doxygen-boost.hh" using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; diff --git a/python/deprecation.hh b/python/deprecation.hh index 0c5e5adeb..2bc9a20ff 100644 --- a/python/deprecation.hh +++ b/python/deprecation.hh @@ -2,15 +2,14 @@ // Copyright (c) 2020-2021 INRIA // -#ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H -#define HPP_FCL_PYTHON_UTILS_DEPRECATION_H +#ifndef COAL_PYTHON_UTILS_DEPRECATION_H +#define COAL_PYTHON_UTILS_DEPRECATION_H #include #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace python { template @@ -48,7 +47,6 @@ struct deprecated_function : deprecated_warning_policy { }; } // namespace python -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_UTILS_DEPRECATION_H +#endif // ifndef COAL_PYTHON_UTILS_DEPRECATION_H diff --git a/python/distance.cc b/python/distance.cc index 0231814da..21209eae4 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -52,8 +52,8 @@ HPP_FCL_COMPILER_DIAGNOSTIC_POP #endif using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; diff --git a/python/fcl.cc b/python/fcl.cc index 956ac8715..d0fea53b8 100644 --- a/python/fcl.cc +++ b/python/fcl.cc @@ -51,7 +51,7 @@ #include "../doc/python/doxygen.hh" #include "../doc/python/doxygen-boost.hh" -using namespace hpp::fcl; +using namespace coal; namespace dv = doxygen::visitor; #pragma GCC diagnostic push diff --git a/python/fwd.hh b/python/fwd.hh index 45e24bcbf..6d0cbff2f 100644 --- a/python/fwd.hh +++ b/python/fwd.hh @@ -2,8 +2,8 @@ // Copyright (c) 2022 CNRS INRIA // -#ifndef HPP_FCL_PYTHON_FWD_HH -#define HPP_FCL_PYTHON_FWD_HH +#ifndef COAL_PYTHON_FWD_HH +#define COAL_PYTHON_FWD_HH #include #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC @@ -37,4 +37,4 @@ namespace dv = doxygen::visitor; #define DEF_CLASS_FUNC2(CLASS, ATTRIB, policy) \ def(#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB), policy) -#endif // ifndef HPP_FCL_PYTHON_FWD_HH +#endif // ifndef COAL_PYTHON_FWD_HH diff --git a/python/gjk.cc b/python/gjk.cc index c88e9b826..1b4f7765a 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -45,11 +45,11 @@ #endif using namespace boost::python; -using namespace hpp::fcl; -using hpp::fcl::details::EPA; -using hpp::fcl::details::GJK; -using hpp::fcl::details::MinkowskiDiff; -using hpp::fcl::details::SupportOptions; +using namespace coal; +using coal::details::EPA; +using coal::details::GJK; +using coal::details::MinkowskiDiff; +using coal::details::SupportOptions; struct MinkowskiDiffWrapper { static void support0(MinkowskiDiff& self, const Vec3f& dir, int& hint, diff --git a/python/math.cc b/python/math.cc index 30c20d4ce..5e1c4c698 100644 --- a/python/math.cc +++ b/python/math.cc @@ -48,8 +48,8 @@ #endif using namespace boost::python; -using namespace hpp::fcl; -using namespace hpp::fcl::python; +using namespace coal; +using namespace coal::python; namespace dv = doxygen::visitor; @@ -57,12 +57,12 @@ struct TriangleWrapper { static Triangle::index_type getitem(const Triangle& t, int i) { if (i >= 3 || i <= -3) PyErr_SetString(PyExc_IndexError, "Index out of range"); - return t[static_cast(i % 3)]; + return t[static_cast(i % 3)]; } static void setitem(Triangle& t, int i, Triangle::index_type v) { if (i >= 3 || i <= -3) PyErr_SetString(PyExc_IndexError, "Index out of range"); - t[static_cast(i % 3)] = v; + t[static_cast(i % 3)] = v; } }; diff --git a/python/octree.cc b/python/octree.cc index 76ebc8b7f..b6bdc376a 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -23,13 +23,13 @@ bp::object toPyBytes(std::vector& bytes) { #endif } -bp::object tobytes(const hpp::fcl::OcTree& self) { +bp::object tobytes(const coal::OcTree& self) { std::vector bytes = self.tobytes(); return toPyBytes(bytes); } void exposeOctree() { - using namespace hpp::fcl; + using namespace coal; namespace bp = boost::python; namespace dv = doxygen::visitor; diff --git a/python/pickle.hh b/python/pickle.hh index 3e640cf0d..7e2358582 100644 --- a/python/pickle.hh +++ b/python/pickle.hh @@ -2,8 +2,8 @@ // Copyright (c) 2022 INRIA // -#ifndef HPP_FCL_PYTHON_PICKLE_H -#define HPP_FCL_PYTHON_PICKLE_H +#ifndef COAL_PYTHON_PICKLE_H +#define COAL_PYTHON_PICKLE_H #include #include @@ -13,7 +13,7 @@ #include using namespace boost::python; -using namespace hpp::fcl; +using namespace coal; // template struct PickleObject : boost::python::pickle_suite { @@ -54,4 +54,4 @@ struct PickleObject : boost::python::pickle_suite { static bool getstate_manages_dict() { return false; } }; -#endif // ifndef HPP_FCL_PYTHON_PICKLE_H +#endif // ifndef COAL_PYTHON_PICKLE_H diff --git a/python/serializable.hh b/python/serializable.hh index 36950f9f6..87c1690fe 100644 --- a/python/serializable.hh +++ b/python/serializable.hh @@ -4,19 +4,18 @@ // https://github.com/stack-of-tasks/pinocchio // -#ifndef HPP_FCL_PYTHON_SERIALIZABLE_H -#define HPP_FCL_PYTHON_SERIALIZABLE_H +#ifndef COAL_PYTHON_SERIALIZABLE_H +#define COAL_PYTHON_SERIALIZABLE_H #include #include "hpp/fcl/serialization/archive.h" #include "hpp/fcl/serialization/serializer.h" -namespace hpp { -namespace fcl { +namespace coal { namespace python { -using Serializer = ::hpp::fcl::serialization::Serializer; +using Serializer = ::coal::serialization::Serializer; namespace bp = boost::python; @@ -54,7 +53,6 @@ struct SerializableVisitor } }; } // namespace python -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // ifndef HPP_FCL_PYTHON_SERIALIZABLE_H +#endif // ifndef COAL_PYTHON_SERIALIZABLE_H diff --git a/python/utils/std-pair.hh b/python/utils/std-pair.hh index 809467201..8af3520a0 100644 --- a/python/utils/std-pair.hh +++ b/python/utils/std-pair.hh @@ -2,8 +2,8 @@ // Copyright (c) 2023 INRIA // -#ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H -#define HPP_FCL_PYTHON_UTILS_STD_PAIR_H +#ifndef COAL_PYTHON_UTILS_STD_PAIR_H +#define COAL_PYTHON_UTILS_STD_PAIR_H #include #include @@ -62,4 +62,4 @@ struct StdPairConverter { } }; -#endif // ifndef HPP_FCL_PYTHON_UTILS_STD_PAIR_H +#endif // ifndef COAL_PYTHON_UTILS_STD_PAIR_H diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 97f1b0723..40a352375 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -41,8 +41,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { AABB::AABB() : min_(Vec3f::Constant((std::numeric_limits::max)())), @@ -192,6 +191,4 @@ bool AABB::overlap(const Halfspace& hs) const { return (hs.signedDistance(support) < 0); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 69b740034..44036f7f3 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -44,8 +44,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Compute the 8 vertices of a OBB inline void computeVertices(const OBB& b, Vec3f vertices[8]) { @@ -489,6 +488,4 @@ OBB translate(const OBB& bv, const Vec3f& t) { return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/OBB.h b/src/BV/OBB.h index 24d6ef877..051a9af71 100644 --- a/src/BV/OBB.h +++ b/src/BV/OBB.h @@ -33,11 +33,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef HPP_FCL_SRC_OBB_H -#define HPP_FCL_SRC_OBB_H +#ifndef COAL_SRC_OBB_H +#define COAL_SRC_OBB_H -namespace hpp { -namespace fcl { +namespace coal { bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, @@ -46,8 +45,6 @@ bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b); -} // namespace fcl +} // namespace coal -} // namespace hpp - -#endif // HPP_FCL_SRC_OBB_H +#endif // COAL_SRC_OBB_H diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp index 87a8494e8..d74381f9c 100644 --- a/src/BV/OBBRSS.cpp +++ b/src/BV/OBBRSS.cpp @@ -37,8 +37,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { OBBRSS translate(const OBBRSS& bv, const Vec3f& t) { OBBRSS res(bv); @@ -47,6 +46,4 @@ OBBRSS translate(const OBBRSS& bv, const Vec3f& t) { return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index 5082775af..f793d2fad 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -42,8 +42,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Clip value between a and b void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { @@ -1010,6 +1009,4 @@ RSS translate(const RSS& bv, const Vec3f& t) { return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp index 3e92cb30d..6d9b4c9f6 100644 --- a/src/BV/kDOP.cpp +++ b/src/BV/kDOP.cpp @@ -41,8 +41,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { /// @brief Find the smaller and larger one of two values inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) { @@ -245,6 +244,4 @@ template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&); template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&); template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&); -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp index 72ba3523d..48bb08024 100644 --- a/src/BV/kIOS.cpp +++ b/src/BV/kIOS.cpp @@ -42,8 +42,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { bool kIOS::overlap(const kIOS& other) const { for (unsigned int i = 0; i < num_spheres; ++i) { @@ -198,6 +197,4 @@ kIOS translate(const kIOS& bv, const Vec3f& t) { return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp index 3327fd071..30271906f 100644 --- a/src/BVH/BVH_model.cpp +++ b/src/BVH/BVH_model.cpp @@ -48,8 +48,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { BVHModelBase::BVHModelBase() : num_tris(0), @@ -1177,6 +1176,4 @@ template class BVHModel; template class BVHModel; template class BVHModel; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp index 6f644e307..638acf3e3 100644 --- a/src/BVH/BVH_utility.cpp +++ b/src/BVH/BVH_utility.cpp @@ -40,8 +40,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace details { template @@ -665,6 +664,4 @@ FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, return maximumDistance_pointcloud(ps, ps2, indices, n, query); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp index 730447a38..86b0bc3fa 100644 --- a/src/BVH/BV_fitter.cpp +++ b/src/BVH/BV_fitter.cpp @@ -40,8 +40,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { static const double kIOS_RATIO = 1.5; static const double invSinA = 2; @@ -679,6 +678,4 @@ AABB BVFitter::fit(unsigned int* primitive_indices, return bv; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp index 64dc26733..7e4dbbc17 100644 --- a/src/BVH/BV_splitter.cpp +++ b/src/BVH/BV_splitter.cpp @@ -37,8 +37,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { template void computeSplitVector(const BV& bv, Vec3f& split_vector) { @@ -283,6 +282,4 @@ template class BVSplitter; template class BVSplitter; template class BVSplitter; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index bc51940cf..71000e45d 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -37,8 +37,7 @@ #include "hpp/fcl/broadphase/broadphase_SSaP.h" -namespace hpp { -namespace fcl { +namespace coal { /** @brief Functor sorting objects according to the AABB lower x bound */ struct SortByXLow { @@ -517,5 +516,4 @@ bool SSaPCollisionManager::empty() const { return objs_x.empty(); } //============================================================================== size_t SSaPCollisionManager::size() const { return objs_x.size(); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 4da0c995a..59871347e 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -37,8 +37,7 @@ #include "hpp/fcl/broadphase/broadphase_SaP.h" -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== void SaPCollisionManager::unregisterObject(CollisionObject* obj) { @@ -849,6 +848,4 @@ bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) { return (pair.obj1 == obj1) && (pair.obj2 == obj2); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index 17e50dda1..0eb81752f 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -40,8 +40,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== NaiveCollisionManager::NaiveCollisionManager() { @@ -199,5 +198,4 @@ bool NaiveCollisionManager::empty() const { return objs.empty(); } //============================================================================== size_t NaiveCollisionManager::size() const { return objs.size(); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 90f03249c..2c5c340a2 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -37,8 +37,7 @@ #include "hpp/fcl/broadphase/broadphase_collision_manager.h" -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== BroadPhaseCollisionManager::BroadPhaseCollisionManager() @@ -90,5 +89,4 @@ void BroadPhaseCollisionManager::insertTestedSet(CollisionObject* a, tested_set.insert(std::make_pair(b, a)); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 608d4628f..7fceeb57c 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -46,8 +46,7 @@ #include "hpp/fcl/BV/BV.h" #include "hpp/fcl/shape/geometric_shapes_utility.h" -namespace hpp { -namespace fcl { +namespace coal { namespace detail { namespace dynamic_AABB_tree { @@ -771,5 +770,4 @@ detail::HierarchyTree& DynamicAABBTreeCollisionManager::getTree() { return dtree; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 492140837..ced67ae1a 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -40,8 +40,7 @@ #ifdef HPP_FCL_HAVE_OCTOMAP #include "hpp/fcl/octree.h" #endif -namespace hpp { -namespace fcl { +namespace coal { namespace detail { namespace dynamic_AABB_tree_array { @@ -725,5 +724,4 @@ DynamicAABBTreeArrayCollisionManager::getTree() const { return dtree; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index aff1c6d25..b9cc6f9db 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -37,8 +37,7 @@ #include "hpp/fcl/broadphase/broadphase_interval_tree.h" -namespace hpp { -namespace fcl { +namespace coal { //============================================================================== void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) { @@ -645,5 +644,4 @@ IntervalTreeCollisionManager::SAPInterval::SAPInterval(FCL_REAL low_, obj = obj_; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/default_broadphase_callbacks.cpp b/src/broadphase/default_broadphase_callbacks.cpp index ea88687d4..55db074da 100644 --- a/src/broadphase/default_broadphase_callbacks.cpp +++ b/src/broadphase/default_broadphase_callbacks.cpp @@ -37,8 +37,7 @@ #include "hpp/fcl/broadphase/default_broadphase_callbacks.h" #include -namespace hpp { -namespace fcl { +namespace coal { bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* data) { @@ -122,5 +121,4 @@ bool CollisionCallBackCollect::exist(const CollisionPair& pair) const { collision_pairs.end(); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp index 567195988..88b1119cc 100644 --- a/src/broadphase/detail/interval_tree.cpp +++ b/src/broadphase/detail/interval_tree.cpp @@ -35,15 +35,14 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_INTERVAL_TREE_INL_H -#define HPP_FCL_INTERVAL_TREE_INL_H +#ifndef COAL_INTERVAL_TREE_INL_H +#define COAL_INTERVAL_TREE_INL_H #include "hpp/fcl/broadphase/detail/interval_tree.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -463,7 +462,6 @@ std::deque IntervalTree::query(FCL_REAL low, FCL_REAL high) { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/interval_tree_node.cpp b/src/broadphase/detail/interval_tree_node.cpp index beb9e28d6..afb55734e 100644 --- a/src/broadphase/detail/interval_tree_node.cpp +++ b/src/broadphase/detail/interval_tree_node.cpp @@ -35,16 +35,15 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H +#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #include "hpp/fcl/broadphase/detail/interval_tree_node.h" #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -90,7 +89,6 @@ void IntervalTreeNode::print(IntervalTreeNode* invalid_node, } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/morton-inl.h b/src/broadphase/detail/morton-inl.h index 53d7d90f7..80354a76d 100644 --- a/src/broadphase/detail/morton-inl.h +++ b/src/broadphase/detail/morton-inl.h @@ -36,13 +36,12 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_MORTON_INL_H -#define HPP_FCL_MORTON_INL_H +#ifndef COAL_MORTON_INL_H +#define COAL_MORTON_INL_H #include "hpp/fcl/broadphase/detail/morton.h" -namespace hpp { -namespace fcl { /// @cond IGNORE +namespace coal { /// @cond IGNORE namespace detail { //============================================================================== @@ -147,7 +146,6 @@ constexpr size_t morton_functor>::bits() { } // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/morton.cpp b/src/broadphase/detail/morton.cpp index 54beaa104..83fc06ced 100644 --- a/src/broadphase/detail/morton.cpp +++ b/src/broadphase/detail/morton.cpp @@ -38,8 +38,7 @@ #include "hpp/fcl/broadphase/detail/morton-inl.h" -namespace hpp { -namespace fcl { +namespace coal { /// @cond IGNORE namespace detail { @@ -79,5 +78,4 @@ uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z) { } // namespace detail /// @endcond -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/broadphase/detail/simple_interval.cpp b/src/broadphase/detail/simple_interval.cpp index 78719156c..4625c31e6 100644 --- a/src/broadphase/detail/simple_interval.cpp +++ b/src/broadphase/detail/simple_interval.cpp @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H -#define HPP_FCL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H +#ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H +#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #include "hpp/fcl/broadphase/detail/simple_interval.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -56,7 +55,6 @@ void SimpleInterval::print() { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index 07f7b6631..46afc7d32 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -35,14 +35,13 @@ /** @author Jia Pan */ -#ifndef HPP_FCL_BROADPHASE_SPATIALHASH_INL_H -#define HPP_FCL_BROADPHASE_SPATIALHASH_INL_H +#ifndef COAL_BROADPHASE_SPATIALHASH_INL_H +#define COAL_BROADPHASE_SPATIALHASH_INL_H #include "hpp/fcl/broadphase/detail/spatial_hash.h" #include -namespace hpp { -namespace fcl { +namespace coal { namespace detail { //============================================================================== @@ -85,7 +84,6 @@ std::vector SpatialHash::operator()(const AABB& aabb) const { } } // namespace detail -} // namespace fcl -} // namespace hpp +} // namespace coal #endif diff --git a/src/collision.cpp b/src/collision.cpp index 9db29dd7b..3b2f5e41b 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -40,8 +40,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { CollisionFunctionMatrix& getCollisionFunctionLookTable() { static CollisionFunctionMatrix table; @@ -203,5 +202,4 @@ std::size_t ComputeCollision::operator()(const Transform3f& tf1, return res; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/collision_data.cpp b/src/collision_data.cpp index 2572014df..51f0083a6 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -38,8 +38,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { bool CollisionRequest::isSatisfied(const CollisionResult& result) const { return result.isCollision() && (num_max_contacts <= result.numContacts()); @@ -49,5 +48,4 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const { return (result.min_distance <= 0); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 6f0dc8dc9..11347a048 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -44,8 +44,7 @@ #include #include <../src/traits_traversal.h> -namespace hpp { -namespace fcl { +namespace coal { #ifdef HPP_FCL_HAS_OCTOMAP @@ -136,7 +135,7 @@ struct HPP_FCL_LOCAL BVHShapeCollider { const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); delete obj1_tmp; return result.numContacts(); @@ -155,7 +154,7 @@ struct HPP_FCL_LOCAL BVHShapeCollider { const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); return result.numContacts(); } }; @@ -183,7 +182,7 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider { HeightFieldShapeCollisionTraversalNode node(request); initialize(node, height_field, tf1, shape, tf2, nsolver, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); return result.numContacts(); } }; @@ -233,7 +232,7 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, Transform3f tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result); - fcl::collide(&node, request, result); + coal::collide(&node, request, result); delete obj1_tmp; delete obj2_tmp; @@ -733,6 +732,4 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { #endif } // template struct CollisionFunctionMatrix; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/collision_node.cpp b/src/collision_node.cpp index 4a016916c..a5e196533 100644 --- a/src/collision_node.cpp +++ b/src/collision_node.cpp @@ -38,8 +38,7 @@ #include <../src/collision_node.h> #include -namespace hpp { -namespace fcl { +namespace coal { void checkResultLowerBound(const CollisionResult& result, FCL_REAL sqrDistLowerBound) { @@ -90,6 +89,4 @@ void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, node->postprocess(); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/collision_node.h b/src/collision_node.h index 8a72be9cf..9c48e8a7b 100644 --- a/src/collision_node.h +++ b/src/collision_node.h @@ -47,8 +47,7 @@ /// @brief collision and distance function on traversal nodes. these functions /// provide a higher level abstraction for collision functions provided in /// collision_func_matrix -namespace hpp { -namespace fcl { +namespace coal { /// collision on collision traversal node /// @@ -69,9 +68,7 @@ HPP_FCL_DLLAPI void collide(CollisionTraversalNodeBase* node, HPP_FCL_DLLAPI void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, unsigned int qsize = 2); -} // namespace fcl - -} // namespace hpp +} // namespace coal /// @endcond diff --git a/src/collision_object.cpp b/src/collision_object.cpp index 1b0ec1b71..f335f7fce 100644 --- a/src/collision_object.cpp +++ b/src/collision_object.cpp @@ -37,11 +37,8 @@ #include -namespace hpp { -namespace fcl { +namespace coal { bool CollisionGeometry::isUncertain() const { return !isOccupied() && !isFree(); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp index 05fd42969..932a6ecd3 100644 --- a/src/collision_utility.cpp +++ b/src/collision_utility.cpp @@ -18,8 +18,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { namespace details { template @@ -78,6 +77,4 @@ CollisionGeometry* extract(const CollisionGeometry* model, std::runtime_error); } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/contact_patch.cpp b/src/contact_patch.cpp index de46983ef..c5870e190 100644 --- a/src/contact_patch.cpp +++ b/src/contact_patch.cpp @@ -37,8 +37,7 @@ #include "hpp/fcl/contact_patch.h" #include "hpp/fcl/collision_utility.h" -namespace hpp { -namespace fcl { +namespace coal { ContactPatchFunctionMatrix& getContactPatchFunctionLookTable() { static ContactPatchFunctionMatrix table; @@ -169,5 +168,4 @@ void ComputeContactPatch::operator()(const Transform3f& tf1, this->run(tf1, tf2, collision_result, request, result); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp index 9593c5cd5..d6b07bdcf 100644 --- a/src/contact_patch/contact_patch_solver.cpp +++ b/src/contact_patch/contact_patch_solver.cpp @@ -36,8 +36,7 @@ #include "hpp/fcl/contact_patch/contact_patch_solver.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { @@ -103,5 +102,4 @@ ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape, } } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp index 8373baa1f..0ab71ea5d 100644 --- a/src/contact_patch_func_matrix.cpp +++ b/src/contact_patch_func_matrix.cpp @@ -40,8 +40,7 @@ #include "hpp/fcl/BV/BV.h" -namespace hpp { -namespace fcl { +namespace coal { template struct BVHShapeComputeContactPatch { @@ -407,5 +406,4 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { // clang-format on } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance.cpp b/src/distance.cpp index a234b5ff5..b22063028 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -42,8 +42,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { DistanceFunctionMatrix& getDistanceFunctionLookTable() { static DistanceFunctionMatrix table; @@ -173,5 +172,4 @@ FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, return res; } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp index 642bd3cb3..8dffb32e4 100644 --- a/src/distance/box_halfspace.cpp +++ b/src/distance/box_halfspace.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -73,5 +72,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp index fbbffbcf0..159739a8b 100644 --- a/src/distance/box_plane.cpp +++ b/src/distance/box_plane.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -75,5 +74,4 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp index cdc8e9495..e3a8bb462 100644 --- a/src/distance/box_sphere.cpp +++ b/src/distance/box_sphere.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -75,5 +74,4 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp index 635a0c4c7..58ac989ba 100644 --- a/src/distance/capsule_capsule.cpp +++ b/src/distance/capsule_capsule.cpp @@ -43,8 +43,7 @@ // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -88,8 +87,8 @@ FCL_REAL ShapeShapeDistance( FCL_REAL EPSILON = std::numeric_limits::epsilon() * 100; // We assume that capsules are centered at the origin. - const fcl::Vec3f& c1 = tf1.getTranslation(); - const fcl::Vec3f& c2 = tf2.getTranslation(); + const coal::Vec3f& c1 = tf1.getTranslation(); + const coal::Vec3f& c2 = tf2.getTranslation(); // We assume that capsules are oriented along z-axis. FCL_REAL halfLength1 = capsule1->halfLength; FCL_REAL halfLength2 = capsule2->halfLength; @@ -97,14 +96,14 @@ FCL_REAL ShapeShapeDistance( FCL_REAL radius2 = (capsule2->radius + capsule2->getSweptSphereRadius()); // direction of capsules // ||d1|| = 2 * halfLength1 - const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); - const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2); + const coal::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); + const coal::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2); // Starting point of the segments // p1 + d1 is the end point of the segment - const fcl::Vec3f p1 = c1 - d1 / 2; - const fcl::Vec3f p2 = c2 - d2 / 2; - const fcl::Vec3f r = p1 - p2; + const coal::Vec3f p1 = c1 - d1 / 2; + const coal::Vec3f p2 = c2 - d2 / 2; + const coal::Vec3f r = p1 - p2; FCL_REAL a = d1.dot(d1); FCL_REAL b = d1.dot(d2); FCL_REAL c = d1.dot(r); @@ -167,5 +166,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp index 7e32dcdb5..294fe7041 100644 --- a/src/distance/capsule_halfspace.cpp +++ b/src/distance/capsule_halfspace.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,5 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp index 3d20410ec..24ac3e321 100644 --- a/src/distance/capsule_plane.cpp +++ b/src/distance/capsule_plane.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,5 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp index b201ce9af..10356c4eb 100644 --- a/src/distance/cone_halfspace.cpp +++ b/src/distance/cone_halfspace.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,5 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp index 83f61b5ee..0b95a1576 100644 --- a/src/distance/cone_plane.cpp +++ b/src/distance/cone_plane.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -75,5 +74,4 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 5a25fd2ed..552b41dd8 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> @@ -67,5 +66,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp index e3acc9e2d..da1752c53 100644 --- a/src/distance/convex_plane.cpp +++ b/src/distance/convex_plane.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> @@ -67,5 +66,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp index 3d998e8f5..58850b15e 100644 --- a/src/distance/cylinder_halfspace.cpp +++ b/src/distance/cylinder_halfspace.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,5 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp index 1147d1bda..975d8389f 100644 --- a/src/distance/cylinder_plane.cpp +++ b/src/distance/cylinder_plane.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,5 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp index 0cd5c09a8..edd376f9f 100644 --- a/src/distance/ellipsoid_halfspace.cpp +++ b/src/distance/ellipsoid_halfspace.cpp @@ -40,8 +40,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -69,5 +68,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp index 797d4b424..f37a03a2f 100644 --- a/src/distance/ellipsoid_plane.cpp +++ b/src/distance/ellipsoid_plane.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp index 1abdb6fd9..eb39873e9 100644 --- a/src/distance/halfspace_halfspace.cpp +++ b/src/distance/halfspace_halfspace.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -55,5 +54,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp index 1608b9af1..ed595868f 100644 --- a/src/distance/halfspace_plane.cpp +++ b/src/distance/halfspace_plane.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp index 0d32413ae..bab49f799 100644 --- a/src/distance/plane_plane.cpp +++ b/src/distance/plane_plane.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -57,5 +56,4 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp index c205ad34e..05a15c39d 100644 --- a/src/distance/sphere_capsule.cpp +++ b/src/distance/sphere_capsule.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -69,5 +68,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp index e050a3421..24bed10f1 100644 --- a/src/distance/sphere_cylinder.cpp +++ b/src/distance/sphere_cylinder.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,6 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp index 2983ebd43..6f09b4fe8 100644 --- a/src/distance/sphere_halfspace.cpp +++ b/src/distance/sphere_halfspace.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,6 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp index 7c95f2312..5aa1ec725 100644 --- a/src/distance/sphere_plane.cpp +++ b/src/distance/sphere_plane.cpp @@ -42,8 +42,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -71,5 +70,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index 5f62d317d..d7403e4bd 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -48,8 +48,7 @@ // // One solution would be to make narrow phase solvers derive from an abstract // class and specialize the template for this abstract class. -namespace hpp { -namespace fcl { +namespace coal { struct GJKSolver; namespace internal { @@ -64,5 +63,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index b260cccc2..5925376cd 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp index b7eeb14f5..c77ddd648 100644 --- a/src/distance/triangle_plane.cpp +++ b/src/distance/triangle_plane.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp index e7af8bfca..f0d670ea7 100644 --- a/src/distance/triangle_sphere.cpp +++ b/src/distance/triangle_sphere.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> @@ -68,5 +67,4 @@ FCL_REAL ShapeShapeDistance( } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp index 2f7302ebc..91ac7c20b 100644 --- a/src/distance/triangle_triangle.cpp +++ b/src/distance/triangle_triangle.cpp @@ -39,8 +39,7 @@ #include #include "../narrowphase/details.h" -namespace hpp { -namespace fcl { +namespace coal { namespace internal { template <> @@ -61,7 +60,7 @@ FCL_REAL ShapeShapeDistance( // We don't need to take into account swept-sphere radius in GJK iterations; // the result will be corrected after GJK terminates. solver->minkowski_difference - .set<::hpp::fcl::details::SupportOptions::NoSweptSphere>(&t1, &t2); + .set<::coal::details::SupportOptions::NoSweptSphere>(&t1, &t2); solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance); // Get GJK initial guess @@ -105,5 +104,4 @@ FCL_REAL ShapeShapeDistance( } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index 7f154e25d..e9ccc50e1 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -43,8 +43,7 @@ #include #include <../src/traits_traversal.h> -namespace hpp { -namespace fcl { +namespace coal { #ifdef HPP_FCL_HAS_OCTOMAP @@ -98,7 +97,7 @@ struct HPP_FCL_LOCAL BVHShapeDistancer { const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); - fcl::distance(&node); + ::coal::distance(&node); delete obj1_tmp; return result.min_distance; @@ -122,7 +121,7 @@ FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - fcl::distance(&node); + ::coal::distance(&node); return result.min_distance; } @@ -656,6 +655,4 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { #endif } // template struct DistanceFunctionMatrix; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/hfield.cpp b/src/hfield.cpp index ba771ec65..2a44f01d2 100644 --- a/src/hfield.cpp +++ b/src/hfield.cpp @@ -45,8 +45,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { template <> NODE_TYPE HeightField::getNodeType() const { @@ -97,6 +96,4 @@ template class HeightField; // template class HeightField; template class HeightField; -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/intersect.cpp b/src/intersect.cpp index ca886a1d8..876b9fd7f 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -42,8 +42,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t) { @@ -704,6 +703,4 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, return res; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/math/transform.cpp b/src/math/transform.cpp index ee6237c17..480039ab0 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -37,8 +37,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { void relativeTransform(const Transform3f& tf1, const Transform3f& tf2, Transform3f& tf) { @@ -51,6 +50,4 @@ void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2, tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation()); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp index 495ace602..31bb61fa7 100644 --- a/src/mesh_loader/assimp.cpp +++ b/src/mesh_loader/assimp.cpp @@ -54,8 +54,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace internal { @@ -107,7 +106,7 @@ void Loader::load(const std::string& resource_path) { * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, +unsigned recurseBuildMesh(const coal::Vec3f& scale, const aiScene* scene, const aiNode* node, unsigned vertices_offset, TriangleAndVertices& tv) { if (!node) return 0; @@ -132,7 +131,7 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, aiVector3D p = input_mesh->mVertices[j]; p *= transform; tv.vertices_.push_back( - fcl::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2])); + coal::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2])); } // add the indices @@ -140,9 +139,9 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, aiFace& face = input_mesh->mFaces[j]; assert(face.mNumIndices == 3 && "The size of the face is not valid."); tv.triangles_.push_back( - fcl::Triangle(vertices_offset + face.mIndices[0], - vertices_offset + face.mIndices[1], - vertices_offset + face.mIndices[2])); + coal::Triangle(vertices_offset + face.mIndices[0], + vertices_offset + face.mIndices[1], + vertices_offset + face.mIndices[2])); } nbVertices += input_mesh->mNumVertices; @@ -156,11 +155,10 @@ unsigned recurseBuildMesh(const fcl::Vec3f& scale, const aiScene* scene, return nbVertices; } -void buildMesh(const fcl::Vec3f& scale, const aiScene* scene, +void buildMesh(const coal::Vec3f& scale, const aiScene* scene, unsigned vertices_offset, TriangleAndVertices& tv) { recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv); } } // namespace internal -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp index cc5543e1e..992b7bd16 100644 --- a/src/mesh_loader/loader.cpp +++ b/src/mesh_loader/loader.cpp @@ -45,8 +45,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const { const CachedMeshLoader::Key& a = *this; for (int i = 0; i < 3; ++i) { @@ -93,7 +92,7 @@ BVHModelPtr_t MeshLoader::load(const std::string& filename, CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) { #ifdef HPP_FCL_HAS_OCTOMAP shared_ptr octree(new octomap::OcTree(filename)); - return CollisionGeometryPtr_t(new hpp::fcl::OcTree(octree)); + return CollisionGeometryPtr_t(new coal::OcTree(octree)); #else HPP_FCL_THROW_PRETTY( "hpp-fcl compiled without OctoMap. Cannot create OcTrees.", @@ -125,6 +124,4 @@ BVHModelPtr_t CachedMeshLoader::load(const std::string& filename, cache_[key] = val; return geom; } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index 17d505058..14cba9f77 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -36,14 +36,13 @@ */ /** \author Jia Pan, Florent Lamiraux */ -#ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H -#define HPP_FCL_SRC_NARROWPHASE_DETAILS_H +#ifndef COAL_SRC_NARROWPHASE_DETAILS_H +#define COAL_SRC_NARROWPHASE_DETAILS_H #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace details { // Compute the point on a line segment that is the closest point on the // segment to to another point. The code is inspired by the explanation @@ -215,8 +214,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, inline FCL_REAL sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, Vec3f& p1, Vec3f& p2, Vec3f& normal) { - const fcl::Vec3f& center1 = tf1.getTranslation(); - const fcl::Vec3f& center2 = tf2.getTranslation(); + const coal::Vec3f& center1 = tf1.getTranslation(); + const coal::Vec3f& center2 = tf2.getTranslation(); FCL_REAL r1 = (s1.radius + s1.getSweptSphereRadius()); FCL_REAL r2 = (s2.radius + s2.getSweptSphereRadius()); @@ -731,7 +730,6 @@ inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, } } // namespace details -} // namespace fcl -} // namespace hpp +} // namespace coal -#endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H +#endif // COAL_SRC_NARROWPHASE_DETAILS_H diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 3b03a9ff8..ab0541488 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -43,8 +43,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace details { @@ -1483,8 +1482,8 @@ void ConvexBase::buildSupportWarmStart() { axiis(i) = 1; { Vec3f support; - hpp::fcl::details::getShapeSupport(this, axiis, support, - support_hint, support_data); + coal::details::getShapeSupport(this, axiis, support, support_hint, + support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } @@ -1492,8 +1491,8 @@ void ConvexBase::buildSupportWarmStart() { axiis(i) = -1; { Vec3f support; - hpp::fcl::details::getShapeSupport(this, axiis, support, - support_hint, support_data); + coal::details::getShapeSupport(this, axiis, support, support_hint, + support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } @@ -1509,16 +1508,16 @@ void ConvexBase::buildSupportWarmStart() { for (size_t ei_index = 0; ei_index < 4; ++ei_index) { { Vec3f support; - hpp::fcl::details::getShapeSupport(this, eis[ei_index], support, - support_hint, support_data); + coal::details::getShapeSupport(this, eis[ei_index], support, + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } { Vec3f support; - hpp::fcl::details::getShapeSupport(this, -eis[ei_index], support, - support_hint, support_data); + coal::details::getShapeSupport(this, -eis[ei_index], support, + support_hint, support_data); this->support_warm_starts.points.emplace_back(support); this->support_warm_starts.indices.emplace_back(support_hint); } @@ -1533,6 +1532,4 @@ void ConvexBase::buildSupportWarmStart() { } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index 4db64acd1..5edd3dda1 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -39,8 +39,7 @@ #include "hpp/fcl/narrowphase/minkowski_difference.h" #include "hpp/fcl/shape/geometric_shapes_traits.h" -namespace hpp { -namespace fcl { +namespace coal { namespace details { // ============================================================================ @@ -321,5 +320,4 @@ template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1 -namespace hpp { -namespace fcl { +namespace coal { namespace details { // ============================================================================ @@ -1109,5 +1108,4 @@ getShapeSupportSetTplInstantiation(LargeConvex) } } // namespace details -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/octree.cpp b/src/octree.cpp index 7e0160596..7fc5aacec 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -35,8 +35,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace internal { struct Neighbors { char value; @@ -200,5 +199,4 @@ OcTreePtr_t makeOctree( return OcTreePtr_t(new OcTree(octree)); } -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp index af6763b31..c4fe900fe 100644 --- a/src/serialization/serialization.cpp +++ b/src/serialization/serialization.cpp @@ -36,7 +36,7 @@ #include "hpp/fcl/serialization/fwd.h" -using namespace hpp::fcl; +using namespace coal; #include "hpp/fcl/serialization/transform.h" #include "hpp/fcl/serialization/collision_data.h" diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index 74fb1d6aa..69be54056 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -17,8 +17,7 @@ using orgQhull::QhullVertexList; using orgQhull::QhullVertexSet; #endif -namespace hpp { -namespace fcl { +namespace coal { // Reorders `tri` such that the dot product between the normal of triangle and // the vector `triangle barycentre - convex_tri.center` is positive. @@ -248,5 +247,4 @@ void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) { } #endif -} // namespace fcl -} // namespace hpp +} // namespace coal diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index 4c8af2a1f..87b5d739a 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -38,8 +38,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { void ConvexBase::initialize(std::shared_ptr> points_, unsigned int num_points_) { @@ -252,6 +251,4 @@ void TriangleP::computeLocalAABB() { aabb_radius = (aabb_local.min_ - aabb_center).norm(); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index d8cd7fc08..b01d11472 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -39,8 +39,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { namespace details { @@ -1097,6 +1096,4 @@ void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, tf = tf_bv * Transform3f(bv.center()); } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/traits_traversal.h b/src/traits_traversal.h index 2df829ef7..c3ae4883a 100644 --- a/src/traits_traversal.h +++ b/src/traits_traversal.h @@ -13,8 +13,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { // TraversalTraitsCollision for collision_func_matrix.cpp @@ -94,6 +93,4 @@ struct HPP_FCL_LOCAL TraversalTraitsDistance, OcTree> { #endif -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp index fc3020e3d..96b755545 100644 --- a/src/traversal/traversal_recurse.cpp +++ b/src/traversal/traversal_recurse.cpp @@ -39,8 +39,7 @@ #include -namespace hpp { -namespace fcl { +namespace coal { void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound) { @@ -360,6 +359,4 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp index 67d430d2f..98160e78a 100644 --- a/test/accelerated_gjk.cpp +++ b/test/accelerated_gjk.cpp @@ -44,22 +44,22 @@ #include "utility.h" -using hpp::fcl::Box; -using hpp::fcl::Capsule; -using hpp::fcl::constructPolytopeFromEllipsoid; -using hpp::fcl::Convex; -using hpp::fcl::Ellipsoid; -using hpp::fcl::FCL_REAL; -using hpp::fcl::GJKSolver; -using hpp::fcl::GJKVariant; -using hpp::fcl::ShapeBase; -using hpp::fcl::support_func_guess_t; -using hpp::fcl::Transform3f; -using hpp::fcl::Triangle; -using hpp::fcl::Vec3f; -using hpp::fcl::details::GJK; -using hpp::fcl::details::MinkowskiDiff; -using hpp::fcl::details::SupportOptions; +using coal::Box; +using coal::Capsule; +using coal::constructPolytopeFromEllipsoid; +using coal::Convex; +using coal::Ellipsoid; +using coal::FCL_REAL; +using coal::GJKSolver; +using coal::GJKVariant; +using coal::ShapeBase; +using coal::support_func_guess_t; +using coal::Transform3f; +using coal::Triangle; +using coal::Vec3f; +using coal::details::GJK; +using coal::details::MinkowskiDiff; +using coal::details::SupportOptions; using std::size_t; BOOST_AUTO_TEST_CASE(set_gjk_variant) { diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 54a22e5b3..49afac16a 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -27,7 +27,7 @@ #define RUN_CASE(BV, tf, models, split) \ run(tf, models, split, #BV " - " #split ":\t") -using namespace hpp::fcl; +using namespace coal; bool verbose = false; FCL_REAL DELTA = 0.001; diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp index b3276a18d..cfa19eff6 100644 --- a/test/box_box_collision.cpp +++ b/test/box_box_collision.cpp @@ -8,14 +8,14 @@ #include "utility.h" -using hpp::fcl::Box; -using hpp::fcl::collide; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::ComputeCollision; -using hpp::fcl::FCL_REAL; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using coal::Box; +using coal::collide; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::ComputeCollision; +using coal::FCL_REAL; +using coal::Transform3f; +using coal::Vec3f; BOOST_AUTO_TEST_CASE(box_box_collision) { // Define boxes diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp index d0c272cd4..7d21bd487 100644 --- a/test/box_box_distance.cpp +++ b/test/box_box_distance.cpp @@ -52,16 +52,16 @@ #include "utility.h" -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::Transform3f; +using coal::Vec3f; BOOST_AUTO_TEST_CASE(distance_box_box_1) { - CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); - CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); + CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); + CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); Transform3f tf1; Transform3f tf2(Vec3f(25, 20, 5)); @@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) { DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -101,14 +101,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) { } BOOST_AUTO_TEST_CASE(distance_box_box_2) { - CollisionGeometryPtr_t s1(new hpp::fcl::Box(6, 10, 2)); - CollisionGeometryPtr_t s2(new hpp::fcl::Box(2, 2, 2)); + CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); + CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); static double pi = M_PI; Transform3f tf1; - Transform3f tf2( - hpp::fcl::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), - sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), - Vec3f(0, 0, 10)); + Transform3f tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), + sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), + Vec3f(0, 0, 10)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -117,7 +116,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { DistanceRequest distanceRequest(true, true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -142,12 +141,12 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { } BOOST_AUTO_TEST_CASE(distance_box_box_3) { - CollisionGeometryPtr_t s1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t s2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t s1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1)); static double pi = M_PI; - Transform3f tf1(hpp::fcl::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), + Transform3f tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), Vec3f(-2, 1, .5)); - Transform3f tf2(hpp::fcl::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), + Transform3f tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), Vec3f(2, .5, .5)); CollisionObject o1(s1, tf1); @@ -157,7 +156,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { DistanceRequest distanceRequest(true, 0, 0); DistanceResult distanceResult; - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -183,8 +182,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4); // Apply the same global transform to both objects and recompute - Transform3f tf3(hpp::fcl::makeQuat(0.435952844074, -0.718287018243, - 0.310622451066, 0.444435113443), + Transform3f tf3(coal::makeQuat(0.435952844074, -0.718287018243, + 0.310622451066, 0.444435113443), Vec3f(4, 5, 6)); tf1 = tf3 * tf1; tf2 = tf3 * tf2; @@ -192,7 +191,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { o2 = CollisionObject(s2, tf2); distanceResult.clear(); - hpp::fcl::distance(&o1, &o2, distanceRequest, distanceResult); + coal::distance(&o1, &o2, distanceRequest, distanceResult); std::cerr << "Applied transformations on two boxes" << std::endl; std::cerr << " T1 = " << tf1.getTranslation() << std::endl @@ -216,8 +215,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { } BOOST_AUTO_TEST_CASE(distance_box_box_4) { - hpp::fcl::Box s1(1, 1, 1); - hpp::fcl::Box s2(1, 1, 1); + coal::Box s1(1, 1, 1); + coal::Box s2(1, 1, 1); // Enable computation of nearest points DistanceRequest distanceRequest(true, true, 0, 0); @@ -226,28 +225,28 @@ BOOST_AUTO_TEST_CASE(distance_box_box_4) { Transform3f tf1(Vec3f(2, 0, 0)); Transform3f tf2; - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 1.; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); tf1.setTranslation(Vec3f(1.01, 0, 0)); distanceResult.clear(); - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); tf1.setTranslation(Vec3f(0.99, 0, 0)); distanceResult.clear(); - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); tf1.setTranslation(Vec3f(0, 0, 0)); distanceResult.clear(); - hpp::fcl::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); + coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -1; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); diff --git a/test/broadphase.cpp b/test/broadphase.cpp index 65b96cff6..d2efdc82a 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -54,8 +54,8 @@ #include #include -using namespace hpp::fcl; -using namespace hpp::fcl::detail; +using namespace coal; +using namespace coal::detail; /// @brief Generate environment with 3 * n objects for self distance, so we try /// to make sure none of them collide with each other. diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp index 4798e66d0..9fdc98f60 100644 --- a/test/broadphase_collision_1.cpp +++ b/test/broadphase_collision_1.cpp @@ -61,7 +61,7 @@ #include #include -using namespace hpp::fcl; +using namespace coal; /// @brief make sure if broadphase algorithms doesn't check twice for the same /// collision object pair diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp index a6a667465..859206194 100644 --- a/test/broadphase_collision_2.cpp +++ b/test/broadphase_collision_2.cpp @@ -59,7 +59,7 @@ #include #include -using namespace hpp::fcl; +using namespace coal; /// @brief test for broad phase collision and self collision void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, diff --git a/test/broadphase_dynamic_AABB_tree.cpp b/test/broadphase_dynamic_AABB_tree.cpp index c16b09d15..df225687f 100644 --- a/test/broadphase_dynamic_AABB_tree.cpp +++ b/test/broadphase_dynamic_AABB_tree.cpp @@ -46,7 +46,7 @@ #include "hpp/fcl/shape/geometric_shapes.h" #include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -using namespace hpp::fcl; +using namespace coal; // Pack the data for callback function. struct CallBackData { diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 10c54a39a..92a715e41 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -52,7 +52,7 @@ #include "utility.h" #include -using namespace hpp::fcl; +using namespace coal; template void testBVHModelPointCloud() { @@ -373,7 +373,7 @@ BOOST_AUTO_TEST_CASE(load_illformated_mesh) { } BOOST_AUTO_TEST_CASE(test_convex) { - Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp index e86e5328f..f34a20faa 100644 --- a/test/capsule_box_1.cpp +++ b/test/capsule_box_1.cpp @@ -49,27 +49,27 @@ #include "utility.h" BOOST_AUTO_TEST_CASE(distance_capsule_box) { - using hpp::fcl::CollisionGeometryPtr_t; + using coal::CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.)); + CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.)); // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.)); + CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.)); // Enable computation of nearest points - hpp::fcl::DistanceRequest distanceRequest(true, 0, 0); - hpp::fcl::DistanceResult distanceResult; + coal::DistanceRequest distanceRequest(true, 0, 0); + coal::DistanceResult distanceResult; - hpp::fcl::Transform3f tf1(hpp::fcl::Vec3f(3., 0, 0)); - hpp::fcl::Transform3f tf2; - hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1); - hpp::fcl::CollisionObject box(boxGeometry, tf2); + coal::Transform3f tf1(coal::Vec3f(3., 0, 0)); + coal::Transform3f tf2; + coal::CollisionObject capsule(capsuleGeometry, tf1); + coal::CollisionObject box(boxGeometry, tf2); // test distance - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + coal::distance(&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule - hpp::fcl::Vec3f o1(distanceResult.nearest_points[0]); + coal::Vec3f o1(distanceResult.nearest_points[0]); // Nearest point on box - hpp::fcl::Vec3f o2(distanceResult.nearest_points[1]); + coal::Vec3f o2(distanceResult.nearest_points[1]); BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1); BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1); CHECK_CLOSE_TO_0(o1[1], 1e-1); @@ -77,12 +77,12 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { CHECK_CLOSE_TO_0(o2[1], 1e-1); // Move capsule above box - tf1 = hpp::fcl::Transform3f(hpp::fcl::Vec3f(0., 0., 8.)); + tf1 = coal::Transform3f(coal::Vec3f(0., 0., 8.)); capsule.setTransform(tf1); // test distance distanceResult.clear(); - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + coal::distance(&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points[0]; o2 = distanceResult.nearest_points[1]; @@ -96,13 +96,13 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1); // Rotate capsule around y axis by pi/2 and move it behind box - tf1.setTranslation(hpp::fcl::Vec3f(-10., 0., 0.)); - tf1.setQuatRotation(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0)); + tf1.setTranslation(coal::Vec3f(-10., 0., 0.)); + tf1.setQuatRotation(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0)); capsule.setTransform(tf1); // test distance distanceResult.clear(); - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); + coal::distance(&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points[0]; o2 = distanceResult.nearest_points[1]; diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp index add71efd6..6af75b959 100644 --- a/test/capsule_box_2.cpp +++ b/test/capsule_box_2.cpp @@ -49,29 +49,28 @@ #include BOOST_AUTO_TEST_CASE(distance_capsule_box) { - typedef hpp::fcl::shared_ptr - CollisionGeometryPtr_t; + typedef coal::shared_ptr CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 - CollisionGeometryPtr_t capsuleGeometry(new hpp::fcl::Capsule(2., 4.)); + CollisionGeometryPtr_t capsuleGeometry(new coal::Capsule(2., 4.)); // Box of size 1 by 2 by 4 - CollisionGeometryPtr_t boxGeometry(new hpp::fcl::Box(1., 2., 4.)); + CollisionGeometryPtr_t boxGeometry(new coal::Box(1., 2., 4.)); // Enable computation of nearest points - hpp::fcl::DistanceRequest distanceRequest(true, 0, 0); - hpp::fcl::DistanceResult distanceResult; + coal::DistanceRequest distanceRequest(true, 0, 0); + coal::DistanceResult distanceResult; // Rotate capsule around y axis by pi/2 and move it behind box - hpp::fcl::Transform3f tf1(hpp::fcl::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), - hpp::fcl::Vec3f(-10., 0.8, 1.5)); - hpp::fcl::Transform3f tf2; - hpp::fcl::CollisionObject capsule(capsuleGeometry, tf1); - hpp::fcl::CollisionObject box(boxGeometry, tf2); + coal::Transform3f tf1(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), + coal::Vec3f(-10., 0.8, 1.5)); + coal::Transform3f tf2; + coal::CollisionObject capsule(capsuleGeometry, tf1); + coal::CollisionObject box(boxGeometry, tf2); // test distance distanceResult.clear(); - hpp::fcl::distance(&capsule, &box, distanceRequest, distanceResult); - hpp::fcl::Vec3f o1 = distanceResult.nearest_points[0]; - hpp::fcl::Vec3f o2 = distanceResult.nearest_points[1]; + coal::distance(&capsule, &box, distanceRequest, distanceResult); + coal::Vec3f o1 = distanceResult.nearest_points[0]; + coal::Vec3f o2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-2); BOOST_CHECK_CLOSE(o1[0], -6, 1e-2); diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index 25d95455d..593950fd9 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -51,7 +51,7 @@ #include "utility.h" -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) { const double radius = 1.; diff --git a/test/collision.cpp b/test/collision.cpp index 5b70aae4b..115e50bbc 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -67,7 +67,7 @@ HPP_FCL_COMPILER_DIAGNOSTIC_POP #include "utility.h" #include "fcl_resources/config.h" -using namespace hpp::fcl; +using namespace coal; namespace utf = boost::unit_test::framework; int num_max_contacts = (std::numeric_limits::max)(); diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp index f5b6de08b..fb8b57f62 100644 --- a/test/collision_node_asserts.cpp +++ b/test/collision_node_asserts.cpp @@ -5,7 +5,7 @@ #include #include -using namespace hpp::fcl; +using namespace coal; constexpr FCL_REAL pi = boost::math::constants::pi(); diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp index 4bebaaea9..457f8be5a 100644 --- a/test/contact_patch.cpp +++ b/test/contact_patch.cpp @@ -41,7 +41,7 @@ #include "utility.h" -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(box_box_no_collision) { const FCL_REAL halfside = 0.5; @@ -59,14 +59,14 @@ BOOST_AUTO_TEST_CASE(box_box_no_collision) { num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(!col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 0); } @@ -86,14 +86,14 @@ BOOST_AUTO_TEST_CASE(box_sphere) { num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box, tf1, &sphere, tf2, col_req, col_res); + coal::collide(&box, tf1, &sphere, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&box, tf1, &sphere, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); @@ -124,17 +124,17 @@ BOOST_AUTO_TEST_CASE(box_box) { num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res1); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res2); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res1); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); @@ -182,17 +182,17 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &box, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &box, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, - patch_res1); - hpp::fcl::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, - patch_res2); + coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, + patch_res1); + coal::computeContactPatch(&hspace, tf1, &box, tf2, col_res, patch_req, + patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); @@ -241,15 +241,15 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { @@ -277,11 +277,11 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); @@ -309,11 +309,11 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { tf2.rotation().col(2) << -1, 0, 0; tf2.translation() << 0, 0, capsule.radius - offset; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &capsule, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &capsule, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); @@ -353,15 +353,15 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { @@ -396,11 +396,11 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); @@ -432,11 +432,11 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { tf2.rotation().col(2) << -1, 0, 0; tf2.translation() << 0, 0, cone.radius - offset; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cone, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cone, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, - patch_res); + coal::computeContactPatch(&hspace, tf1, &cone, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); @@ -478,7 +478,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); if (col_res.isCollision()) { @@ -505,8 +505,8 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { BOOST_CHECK(patch_req.getNumSamplesCurvedShapes() == ContactPatch::default_preallocated_size); ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -522,11 +522,11 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { tf2.rotation().col(1) << 0, 1, 0; tf2.rotation().col(2) << 0, 0, -1; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); @@ -544,13 +544,13 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { tf2.translation() << 0, 0, cylinder.radius - offset; col_res.clear(); - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (col_res.isCollision() && patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); @@ -588,17 +588,17 @@ BOOST_AUTO_TEST_CASE(convex_convex) { num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&box1, tf1, &box2, tf2, col_req, col_res); + coal::collide(&box1, tf1, &box2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); const ContactPatchRequest patch_req; ContactPatchResult patch_res1(patch_req); ContactPatchResult patch_res2(patch_req); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res1); - hpp::fcl::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, - patch_res2); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res1); + coal::computeContactPatch(&box1, tf1, &box2, tf2, col_res, patch_req, + patch_res2); BOOST_CHECK(patch_res1.numContactPatches() == 1); BOOST_CHECK(patch_res2.numContactPatches() == 1); @@ -673,11 +673,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -724,11 +724,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -773,11 +773,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -839,11 +839,11 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -887,11 +887,11 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -935,11 +935,11 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { @@ -1000,11 +1000,11 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { Convex tetra2(pts2, 4, tris2, 4); col_res.clear(); - hpp::fcl::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); + coal::collide(&tetra1, tf1, &tetra2, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); patch_res.clear(); - hpp::fcl::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&tetra1, tf1, &tetra2, tf2, col_res, patch_req, + patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0) { diff --git a/test/convex.cpp b/test/convex.cpp index 744902b97..ad6522029 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -43,7 +43,7 @@ #include "utility.h" -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(convex) { FCL_REAL l = 1, w = 1, d = 1; diff --git a/test/distance.cpp b/test/distance.cpp index ed365417a..567e2109b 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -49,7 +49,7 @@ #include "utility.h" #include "fcl_resources/config.h" -using namespace hpp::fcl; +using namespace coal; namespace utf = boost::unit_test::framework; bool verbose = false; diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index d14825dd1..7442280e8 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -46,19 +46,19 @@ #include "utility.h" #include "fcl_resources/config.h" -using hpp::fcl::BVHModel; -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::FCL_REAL; -using hpp::fcl::OBBRSS; -using hpp::fcl::shared_ptr; -using hpp::fcl::Transform3f; -using hpp::fcl::Triangle; -using hpp::fcl::Vec3f; +using coal::BVHModel; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::FCL_REAL; +using coal::OBBRSS; +using coal::shared_ptr; +using coal::Transform3f; +using coal::Triangle; +using coal::Vec3f; bool testDistanceLowerBound(const Transform3f& tf, const CollisionGeometryPtr_t& m1, @@ -72,7 +72,7 @@ bool testDistanceLowerBound(const Transform3f& tf, CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); - hpp::fcl::collide(&co1, &co2, request, result); + coal::collide(&co1, &co2, request, result); distance = result.distance_lower_bound; return result.isCollision(); @@ -82,14 +82,14 @@ bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2) { Transform3f pose1(tf), pose2; - CollisionRequest request(hpp::fcl::NO_REQUEST, 1); + CollisionRequest request(coal::NO_REQUEST, 1); request.enable_distance_lower_bound = false; CollisionResult result; CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); - hpp::fcl::collide(&co1, &co2, request, result); + coal::collide(&co1, &co2, request, result); return result.isCollision(); } @@ -102,7 +102,7 @@ bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1, CollisionObject co1(m1, pose1); CollisionObject co2(m2, pose2); - hpp::fcl::distance(&co1, &co2, request, result); + coal::distance(&co1, &co2, request, result); distance = result.min_distance; if (result.min_distance <= 0) { @@ -158,8 +158,8 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { } BOOST_AUTO_TEST_CASE(box_sphere) { - shared_ptr sphere(new hpp::fcl::Sphere(0.5)); - shared_ptr box(new hpp::fcl::Box(1., 1., 1.)); + shared_ptr sphere(new coal::Sphere(0.5)); + shared_ptr box(new coal::Box(1., 1., 1.)); Transform3f M1; M1.setIdentity(); @@ -195,8 +195,8 @@ BOOST_AUTO_TEST_CASE(box_sphere) { } BOOST_AUTO_TEST_CASE(sphere_sphere) { - shared_ptr sphere1(new hpp::fcl::Sphere(0.5)); - shared_ptr sphere2(new hpp::fcl::Sphere(1.)); + shared_ptr sphere1(new coal::Sphere(0.5)); + shared_ptr sphere2(new coal::Sphere(1.)); Transform3f M1; M1.setIdentity(); @@ -239,7 +239,7 @@ BOOST_AUTO_TEST_CASE(box_mesh) { loadOBJFile((path / "env.obj").string().c_str(), p1, t1); shared_ptr > m1(new BVHModel); - shared_ptr m2(new hpp::fcl::Box(500, 200, 150)); + shared_ptr m2(new coal::Box(500, 200, 150)); m1->beginModel(); m1->addSubModel(p1, t1); diff --git a/test/frontlist.cpp b/test/frontlist.cpp index 724144faa..f90634d5e 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -47,7 +47,7 @@ #include "fcl_resources/config.h" #include -using namespace hpp::fcl; +using namespace coal; namespace utf = boost::unit_test::framework; template diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index ec7425248..ba5114c3f 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -48,7 +48,7 @@ #include #include -using namespace hpp::fcl; +using namespace coal; FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; @@ -66,8 +66,7 @@ int line; << "].") #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) -namespace hpp { -namespace fcl { +namespace coal { std::ostream& operator<<(std::ostream& os, const ShapeBase&) { return os << "a_shape"; } @@ -75,8 +74,7 @@ std::ostream& operator<<(std::ostream& os, const ShapeBase&) { std::ostream& operator<<(std::ostream& os, const Box& b) { return os << "Box(" << 2 * b.halfSide.transpose() << ')'; } -} // namespace fcl -} // namespace hpp +} // namespace coal template void printComparisonError(const std::string& comparison_type, const S1& s1, @@ -505,18 +503,18 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { } BOOST_AUTO_TEST_CASE(distance_spherebox) { - hpp::fcl::Matrix3f rotSphere; + coal::Matrix3f rotSphere; rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - hpp::fcl::Vec3f trSphere(0.0, 0.0, 0.0); + coal::Vec3f trSphere(0.0, 0.0, 0.0); - hpp::fcl::Matrix3f rotBox; + coal::Matrix3f rotBox; rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - hpp::fcl::Vec3f trBox(0.0, 5.0, 3.0); + coal::Vec3f trBox(0.0, 5.0, 3.0); - hpp::fcl::Sphere sphere(1); - hpp::fcl::Box box(10, 2, 10); + coal::Sphere sphere(1); + coal::Box box(10, 2, 10); - hpp::fcl::DistanceResult result; + coal::DistanceResult result; distance(&sphere, Transform3f(rotSphere, trSphere), &box, Transform3f(rotBox, trBox), DistanceRequest(true), result); diff --git a/test/gjk.cpp b/test/gjk.cpp index d1877f475..1631a2777 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -46,14 +46,14 @@ #include "utility.h" -using hpp::fcl::FCL_REAL; -using hpp::fcl::GJKSolver; -using hpp::fcl::GJKVariant; -using hpp::fcl::Matrix3f; -using hpp::fcl::Quatf; -using hpp::fcl::Transform3f; -using hpp::fcl::TriangleP; -using hpp::fcl::Vec3f; +using coal::FCL_REAL; +using coal::GJKSolver; +using coal::GJKVariant; +using coal::Matrix3f; +using coal::Quatf; +using coal::Transform3f; +using coal::TriangleP; +using coal::Vec3f; typedef Eigen::Matrix vector_t; typedef Eigen::Matrix vector6_t; @@ -138,14 +138,14 @@ void test_gjk_distance_triangle_triangle( TriangleP tri2(Q1_loc, Q2_loc, Q3_loc); Vec3f normal; const bool compute_penetration = true; - hpp::fcl::DistanceRequest request(compute_penetration, compute_penetration); - hpp::fcl::DistanceResult result; + coal::DistanceRequest request(compute_penetration, compute_penetration); + coal::DistanceResult result; start = clock(); // The specialized function TriangleP-TriangleP calls GJK to check for // collision and compute the witness points but it does not use EPA to // compute the penetration depth. - distance = hpp::fcl::ShapeShapeDistance( + distance = coal::ShapeShapeDistance( &tri1, tf1, &tri2, tf2, &solver, request, result); end = clock(); p1 = result.nearest_points[0]; @@ -163,7 +163,7 @@ void test_gjk_distance_triangle_triangle( assert(penetration_depth >= 0); tf2.setTranslation((penetration_depth + 10 - 4) * normal); result.clear(); - distance = hpp::fcl::ShapeShapeDistance( + distance = coal::ShapeShapeDistance( &tri1, tf1, &tri2, tf2, &solver, request, result); c1 = result.nearest_points[0]; c2 = result.nearest_points[1]; @@ -337,7 +337,7 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) { void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, double swept_sphere_radius, bool use_gjk_nesterov_acceleration) { - using namespace hpp::fcl; + using namespace coal; const FCL_REAL r = 1.0; Sphere sphere(r); sphere.setSweptSphereRadius(swept_sphere_radius); @@ -416,7 +416,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, bool use_gjk_nesterov_acceleration, Vec3f w0_expected, Vec3f w1_expected) { - using namespace hpp::fcl; + using namespace coal; Capsule capsule(1., 2.); // Radius 1 and length 2 TriangleP triangle(Vec3f(0., 0., 0.), Vec3f(1., 0., 0.), Vec3f(1., 1., 0.)); diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp index b06ee09a7..afe6ae5de 100644 --- a/test/gjk_asserts.cpp +++ b/test/gjk_asserts.cpp @@ -5,7 +5,7 @@ #include #include -using namespace hpp::fcl; +using namespace coal; constexpr FCL_REAL pi = boost::math::constants::pi(); diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index f2ac0bdbc..bc331a7a7 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -46,17 +46,17 @@ #include "utility.h" -using hpp::fcl::Box; -using hpp::fcl::FCL_REAL; -using hpp::fcl::GJKConvergenceCriterion; -using hpp::fcl::GJKConvergenceCriterionType; -using hpp::fcl::GJKSolver; -using hpp::fcl::ShapeBase; -using hpp::fcl::support_func_guess_t; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; -using hpp::fcl::details::GJK; -using hpp::fcl::details::MinkowskiDiff; +using coal::Box; +using coal::FCL_REAL; +using coal::GJKConvergenceCriterion; +using coal::GJKConvergenceCriterionType; +using coal::GJKSolver; +using coal::ShapeBase; +using coal::support_func_guess_t; +using coal::Transform3f; +using coal::Vec3f; +using coal::details::GJK; +using coal::details::MinkowskiDiff; using std::size_t; BOOST_AUTO_TEST_CASE(set_cv_criterion) { diff --git a/test/hfields.cpp b/test/hfields.cpp index ca34db96d..a28ea8052 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -53,7 +53,7 @@ #include "utility.h" #include -using namespace hpp::fcl; +using namespace coal; template void test_constant_hfields(const Eigen::DenseIndex nx, diff --git a/test/math.cpp b/test/math.cpp index 3a5be3c9e..8a4edf638 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -45,7 +45,7 @@ #include #include -using namespace hpp::fcl; +using namespace coal; BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { Vec3f v1(1.0, 2.0, 3.0); diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp index a3ef2a696..41ac105ee 100644 --- a/test/normal_and_nearest_points.cpp +++ b/test/normal_and_nearest_points.cpp @@ -46,7 +46,7 @@ #include "utility.h" -using namespace hpp::fcl; +using namespace coal; typedef Eigen::Vector2d Vec2d; // This test suite is designed to operate on any pair of primitive shapes: @@ -679,9 +679,9 @@ void test_normal_and_nearest_points(const Halfspace& o1, } BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) { - Box* box_ptr = new hpp::fcl::Box(1, 1, 1); - hpp::fcl::CollisionGeometryPtr_t b1(box_ptr); - BVHModel o1 = BVHModel(); + Box* box_ptr = new coal::Box(1, 1, 1); + coal::CollisionGeometryPtr_t b1(box_ptr); + BVHModel o1 = BVHModel(); generateBVHModel(o1, *box_ptr, Transform3f()); o1.buildConvexRepresentation(false); diff --git a/test/obb.cpp b/test/obb.cpp index 117696bb7..bd94b94ed 100644 --- a/test/obb.cpp +++ b/test/obb.cpp @@ -46,7 +46,7 @@ #include #include "utility.h" -using namespace hpp::fcl; +using namespace coal; void randomOBBs(Vec3f& a, Vec3f& b, FCL_REAL extentNorm) { // Extent norm is between 0 and extentNorm on each axis diff --git a/test/octree.cpp b/test/octree.cpp index 566fb6dd5..5b1100de5 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -53,11 +53,11 @@ namespace utf = boost::unit_test::framework; -using namespace hpp::fcl; +using namespace coal; void makeMesh(const std::vector& vertices, const std::vector& triangles, BVHModel& model) { - hpp::fcl::SplitMethodType split_method(hpp::fcl::SPLIT_METHOD_MEAN); + coal::SplitMethodType split_method(coal::SPLIT_METHOD_MEAN); model.bv_splitter.reset(new BVSplitter(split_method)); model.bv_splitter.reset(new BVSplitter(split_method)); @@ -66,13 +66,12 @@ void makeMesh(const std::vector& vertices, model.endModel(); } -hpp::fcl::OcTree makeOctree(const BVHModel& mesh, - const FCL_REAL& resolution) { +coal::OcTree makeOctree(const BVHModel& mesh, + const FCL_REAL& resolution) { Vec3f m(mesh.aabb_local.min_); Vec3f M(mesh.aabb_local.max_); - hpp::fcl::Box box(resolution, resolution, resolution); - CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, - 1); + coal::Box box(resolution, resolution, resolution); + CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); CollisionResult result; Transform3f tfBox; octomap::OcTreePtr_t octree(new octomap::OcTree(resolution)); @@ -86,7 +85,7 @@ hpp::fcl::OcTree makeOctree(const BVHModel& mesh, Vec3f center(x + .5 * resolution, y + .5 * resolution, z + .5 * resolution); tfBox.setTranslation(center); - hpp::fcl::collide(&box, tfBox, &mesh, Transform3f(), request, result); + coal::collide(&box, tfBox, &mesh, Transform3f(), request, result); if (result.isCollision()) { octomap::point3d p((float)center[0], (float)center[1], (float)center[2]); @@ -120,7 +119,7 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { envMesh.computeLocalAABB(); // Load octree built from envMesh by makeOctree(envMesh, resolution) OcTree envOctree( - hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution)); + coal::loadOctreeFile((path / "env.octree").string(), resolution)); std::cout << "Finished loading octree." << std::endl; @@ -147,13 +146,12 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { #else std::size_t N = 10000; #endif - N = hpp::fcl::getNbRun(utf::master_test_suite().argc, - utf::master_test_suite().argv, N); + N = coal::getNbRun(utf::master_test_suite().argc, + utf::master_test_suite().argv, N); generateRandomTransforms(extents, transforms, 2 * N); - CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, - 1); + CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); for (std::size_t i = 0; i < N; ++i) { CollisionResult resultMesh; CollisionResult resultOctree; @@ -161,16 +159,16 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { Transform3f tf2(transforms[2 * i + 1]); ; // Test collision between meshes with random transform for robot. - hpp::fcl::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh); + coal::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh); // Test collision between mesh and octree for the same transform. - hpp::fcl::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree); + coal::collide(&robMesh, tf1, &envOctree, tf2, request, resultOctree); bool resMesh(resultMesh.isCollision()); bool resOctree(resultOctree.isCollision()); BOOST_CHECK(!resMesh || resOctree); if (!resMesh && resOctree) { - hpp::fcl::DistanceRequest dreq; - hpp::fcl::DistanceResult dres; - hpp::fcl::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres); + coal::DistanceRequest dreq; + coal::DistanceResult dres; + coal::distance(&robMesh, tf1, &envMesh, tf2, dreq, dres); std::cout << "distance mesh mesh: " << dres.min_distance << std::endl; BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution); } @@ -193,7 +191,7 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { envMesh.computeLocalAABB(); // Load octree built from envMesh by makeOctree(envMesh, resolution) OcTree envOctree( - hpp::fcl::loadOctreeFile((path / "env.octree").string(), resolution)); + coal::loadOctreeFile((path / "env.octree").string(), resolution)); std::cout << "Finished loading octree." << std::endl; @@ -213,13 +211,12 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { #else std::size_t N = 100000; #endif - N = hpp::fcl::getNbRun(utf::master_test_suite().argc, - utf::master_test_suite().argv, N); + N = coal::getNbRun(utf::master_test_suite().argc, + utf::master_test_suite().argv, N); generateRandomTransforms(extents, transforms, 2 * N); - CollisionRequest request(hpp::fcl::CONTACT | hpp::fcl::DISTANCE_LOWER_BOUND, - 1); + CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); for (std::size_t i = 0; i < N; ++i) { CollisionResult resultBox; CollisionResult resultHfield1, resultHfield2; @@ -231,19 +228,19 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { constructBox(hfield.aabb_local, tf2, box, box_tf); // Test collision between octree and equivalent box. - hpp::fcl::collide(&envOctree, tf1, &box, box_tf, request, resultBox); + coal::collide(&envOctree, tf1, &box, box_tf, request, resultBox); // Test collision between octree and hfield. - hpp::fcl::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1); - hpp::fcl::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2); + coal::collide(&envOctree, tf1, &hfield, tf2, request, resultHfield1); + coal::collide(&hfield, tf2, &envOctree, tf1, request, resultHfield2); bool resBox(resultBox.isCollision()); bool resHfield(resultHfield1.isCollision()); BOOST_CHECK(resBox == resHfield); BOOST_CHECK(resultHfield1.isCollision() == resultHfield2.isCollision()); if (!resBox && resHfield) { - hpp::fcl::DistanceRequest dreq; - hpp::fcl::DistanceResult dres; - hpp::fcl::distance(&envMesh, tf1, &box, box_tf, dreq, dres); + coal::DistanceRequest dreq; + coal::DistanceResult dres; + coal::distance(&envMesh, tf1, &box, box_tf, dreq, dres); std::cout << "distance mesh box: " << dres.min_distance << std::endl; BOOST_CHECK(dres.min_distance < sqrt(2.) * resolution); } diff --git a/test/profiling.cpp b/test/profiling.cpp index 6c11dac7d..2462cdf0b 100644 --- a/test/profiling.cpp +++ b/test/profiling.cpp @@ -27,7 +27,7 @@ #include "utility.h" #include "fcl_resources/config.h" -using namespace hpp::fcl; +using namespace coal; CollisionFunctionMatrix lookupTable; bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) { @@ -190,10 +190,10 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { iarg += 3; if (iarg < argc && strcmp(argv[iarg], "crop") == 0) { CHECK_PARAM_NB(6, Crop); - hpp::fcl::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]), - atof(argv[iarg + 3])), - Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]), - atof(argv[iarg + 6]))); + coal::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]), + atof(argv[iarg + 3])), + Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]), + atof(argv[iarg + 6]))); OUT("Cropping " << aabb.min_.transpose() << " ---- " << aabb.max_.transpose() << " ..."); o->computeLocalAABB(); diff --git a/test/security_margin.cpp b/test/security_margin.cpp index aee545577..9fe867be3 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -47,26 +47,26 @@ #include "utility.h" -using namespace hpp::fcl; -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using namespace coal; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::Transform3f; +using coal::Vec3f; #define MATH_SQUARED(x) (x * x) BOOST_AUTO_TEST_CASE(aabb_aabb) { - CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3f tf1; const Transform3f tf2_collision(Vec3f(0, 1, 1)); - hpp::fcl::Box s1(1, 1, 1); - hpp::fcl::Box s2(1, 1, 1); + coal::Box s1(1, 1, 1); + coal::Box s2(1, 1, 1); const double tol = 1e-8; AABB bv1, bv2; @@ -150,13 +150,13 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { } BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { - CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3f tf1; const Transform3f tf2_collision(Vec3f(0, 0, 0)); - hpp::fcl::Box s1(1, 1, 1); - hpp::fcl::Box s2(1, 1, 1); + coal::Box s1(1, 1, 1); + coal::Box s2(1, 1, 1); AABB bv1, bv2; computeBV(s1, Transform3f(), bv1); @@ -180,8 +180,8 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { } BOOST_AUTO_TEST_CASE(sphere_sphere) { - CollisionGeometryPtr_t s1(new hpp::fcl::Sphere(1)); - CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(2)); + CollisionGeometryPtr_t s1(new coal::Sphere(1)); + CollisionGeometryPtr_t s2(new coal::Sphere(2)); const Transform3f tf1; const Transform3f tf2_collision(Vec3f(0, 0, 3)); @@ -258,8 +258,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { } BOOST_AUTO_TEST_CASE(capsule_capsule) { - CollisionGeometryPtr_t c1(new hpp::fcl::Capsule(0.5, 1.)); - CollisionGeometryPtr_t c2(new hpp::fcl::Capsule(0.5, 1.)); + CollisionGeometryPtr_t c1(new coal::Capsule(0.5, 1.)); + CollisionGeometryPtr_t c2(new coal::Capsule(0.5, 1.)); const Transform3f tf1; const Transform3f tf2_collision(Vec3f(0, 1., 0)); @@ -332,8 +332,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { } BOOST_AUTO_TEST_CASE(box_box) { - CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); - CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3f tf1; const Transform3f tf2_collision(Vec3f(0, 1, 1)); @@ -488,13 +488,13 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, } BOOST_AUTO_TEST_CASE(sphere_box) { - Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); box_bvh_model.buildConvexRepresentation(false); ConvexBase& box_convex = *box_bvh_model.convex.get(); - CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(0.5)); + CollisionGeometryPtr_t s2(new coal::Sphere(0.5)); const Transform3f tf1; const Transform3f tf2_collision(Vec3f(0, 0, 1)); diff --git a/test/serialization.cpp b/test/serialization.cpp index 46177408c..4f2bee37d 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -70,7 +70,7 @@ HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS namespace utf = boost::unit_test::framework; -using namespace hpp::fcl; +using namespace coal; template bool check(const T& value, const T& other) { @@ -158,31 +158,31 @@ void test_serialization(const T& value, T& other_value, // -- TXT { const std::string filename = txt_filename.string(); - hpp::fcl::serialization::saveToText(value, filename); + coal::serialization::saveToText(value, filename); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromText(other_value, filename); + coal::serialization::loadFromText(other_value, filename); BOOST_CHECK(check(value, other_value)); } // -- String stream (TXT format) { std::stringstream ss_out; - hpp::fcl::serialization::saveToStringStream(value, ss_out); + coal::serialization::saveToStringStream(value, ss_out); BOOST_CHECK(check(value, value)); std::istringstream ss_in(ss_out.str()); - hpp::fcl::serialization::loadFromStringStream(other_value, ss_in); + coal::serialization::loadFromStringStream(other_value, ss_in); BOOST_CHECK(check(value, other_value)); } // -- String { - const std::string str_out = hpp::fcl::serialization::saveToString(value); + const std::string str_out = coal::serialization::saveToString(value); BOOST_CHECK(check(value, value)); const std::string str_in(str_out); - hpp::fcl::serialization::loadFromString(other_value, str_in); + coal::serialization::loadFromString(other_value, str_in); BOOST_CHECK(check(value, other_value)); } } @@ -192,10 +192,10 @@ void test_serialization(const T& value, T& other_value, { const std::string filename = xml_filename.string(); const std::string xml_tag = "value"; - hpp::fcl::serialization::saveToXML(value, filename, xml_tag); + coal::serialization::saveToXML(value, filename, xml_tag); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromXML(other_value, filename, xml_tag); + coal::serialization::loadFromXML(other_value, filename, xml_tag); BOOST_CHECK(check(value, other_value)); } } @@ -204,10 +204,10 @@ void test_serialization(const T& value, T& other_value, if (mode & 0x4) { { const std::string filename = bin_filename.string(); - hpp::fcl::serialization::saveToBinary(value, filename); + coal::serialization::saveToBinary(value, filename); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromBinary(other_value, filename); + coal::serialization::loadFromBinary(other_value, filename); BOOST_CHECK(check(value, other_value)); } } @@ -216,10 +216,10 @@ void test_serialization(const T& value, T& other_value, if (mode & 0x8) { { boost::asio::streambuf buffer; - hpp::fcl::serialization::saveToBuffer(value, buffer); + coal::serialization::saveToBuffer(value, buffer); BOOST_CHECK(check(value, value)); - hpp::fcl::serialization::loadFromBuffer(other_value, buffer); + coal::serialization::loadFromBuffer(other_value, buffer); BOOST_CHECK(check(value, other_value)); } } @@ -230,11 +230,11 @@ void test_serialization(const T& value, T& other_value, std::shared_ptr ptr = std::make_shared(value); const std::string filename = txt_ptr_filename.string(); - hpp::fcl::serialization::saveToText(ptr, filename); + coal::serialization::saveToText(ptr, filename); BOOST_CHECK(check_ptr(ptr.get(), ptr.get())); std::shared_ptr other_ptr = nullptr; - hpp::fcl::serialization::loadFromText(other_ptr, filename); + coal::serialization::loadFromText(other_ptr, filename); BOOST_CHECK(check_ptr(ptr.get(), other_ptr.get())); } @@ -295,13 +295,13 @@ BOOST_AUTO_TEST_CASE(test_collision_data) { const CollisionRequest col_req(CollisionRequestFlag::CONTACT, num_max_contact); CollisionResult col_res; - hpp::fcl::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); + coal::collide(&hspace, tf1, &cylinder, tf2, col_req, col_res); BOOST_CHECK(col_res.isCollision()); if (col_res.isCollision()) { ContactPatchRequest patch_req; ContactPatchResult patch_res(patch_req); - hpp::fcl::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, - patch_req, patch_res); + coal::computeContactPatch(&hspace, tf1, &cylinder, tf2, col_res, + patch_req, patch_res); BOOST_CHECK(patch_res.numContactPatches() == 1); // Serialize patch request, result and the patch itself @@ -403,12 +403,12 @@ BOOST_AUTO_TEST_CASE(test_Convex) { BOOST_CHECK(ptr.get()); const std::string filename = xml_filename.string(); const std::string tag_name = "CollisionGeometry"; - hpp::fcl::serialization::saveToXML(ptr, filename, tag_name); + coal::serialization::saveToXML(ptr, filename, tag_name); BOOST_CHECK(check(*reinterpret_cast*>(ptr.get()), convex)); std::shared_ptr other_ptr = nullptr; BOOST_CHECK(!other_ptr.get()); - hpp::fcl::serialization::loadFromXML(other_ptr, filename, tag_name); + coal::serialization::loadFromXML(other_ptr, filename, tag_name); BOOST_CHECK( check(convex, *reinterpret_cast*>(other_ptr.get()))); } diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index e305906bd..323dc90bb 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -46,15 +46,15 @@ #include "utility.h" -using namespace hpp::fcl; -using hpp::fcl::CollisionGeometryPtr_t; -using hpp::fcl::CollisionObject; -using hpp::fcl::CollisionRequest; -using hpp::fcl::CollisionResult; -using hpp::fcl::DistanceRequest; -using hpp::fcl::DistanceResult; -using hpp::fcl::Transform3f; -using hpp::fcl::Vec3f; +using namespace coal; +using coal::CollisionGeometryPtr_t; +using coal::CollisionObject; +using coal::CollisionRequest; +using coal::CollisionResult; +using coal::DistanceRequest; +using coal::DistanceResult; +using coal::Transform3f; +using coal::Vec3f; #define MATH_SQUARED(x) (x * x) @@ -164,35 +164,35 @@ void test_no_throw(const Shape &shape, const FCL_REAL inflation) { } BOOST_AUTO_TEST_CASE(test_inflate) { - const hpp::fcl::Sphere sphere(1); + const coal::Sphere sphere(1); test(sphere, 0.01, 1e-8); test_throw(sphere, -1.1); test_no_throw(sphere, 1.); - const hpp::fcl::Box box(1, 1, 1); + const coal::Box box(1, 1, 1); test(box, 0.01, 1e-8); test_throw(box, -0.6); - const hpp::fcl::Ellipsoid ellipsoid(1, 2, 3); + const coal::Ellipsoid ellipsoid(1, 2, 3); test(ellipsoid, 0.01, 1e-8); test_throw(ellipsoid, -1.1); - const hpp::fcl::Capsule capsule(1., 2.); + const coal::Capsule capsule(1., 2.); test(capsule, 0.01, 1e-8); test_throw(capsule, -1.1); - const hpp::fcl::Cylinder cylinder(1., 2.); + const coal::Cylinder cylinder(1., 2.); test(cylinder, 0.01, 1e-8); test_throw(cylinder, -1.1); - const hpp::fcl::Cone cone(1., 4.); + const coal::Cone cone(1., 4.); test(cone, 0.01, 1e-8); test_throw(cone, -1.1); - const hpp::fcl::Halfspace halfspace(Vec3f::UnitZ(), 0.); + const coal::Halfspace halfspace(Vec3f::UnitZ(), 0.); test(halfspace, 0.01, 1e-8); - // const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), + // const coal::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), // Vec3f::UnitZ()); // test(triangle, 0.01, 1e-8); } diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index 0c0e7c907..e84f179fc 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -44,7 +44,7 @@ #include #include "utility.h" -using namespace hpp::fcl; +using namespace coal; #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index 3bf8f538e..b829fd7b9 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -47,7 +47,7 @@ #include "utility.h" -using namespace hpp::fcl; +using namespace coal; NODE_TYPE node1_type; NODE_TYPE node2_type; @@ -315,12 +315,12 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { const FCL_REAL ssr2 = shape2.getSweptSphereRadius(); shape1.setSweptSphereRadius(0.); shape2.setSweptSphereRadius(0.); - hpp::fcl::collide(&shape1, tf1, &shape2, tf2, request, result[0]); + coal::collide(&shape1, tf1, &shape2, tf2, request, result[0]); // With swept sphere radius shape1.setSweptSphereRadius(ssr1); shape2.setSweptSphereRadius(ssr2); - hpp::fcl::collide(&shape1, tf1, &shape2, tf2, request, result[1]); + coal::collide(&shape1, tf1, &shape2, tf2, request, result[1]); BOOST_CHECK(result[0].isCollision()); BOOST_CHECK(result[1].isCollision()); diff --git a/test/utility.cpp b/test/utility.cpp index cf20cdd14..a17831eb9 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -14,8 +14,7 @@ #include #include -namespace hpp { -namespace fcl { +namespace coal { BenchTimer::BenchTimer() { #ifdef _WIN32 @@ -191,7 +190,7 @@ OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) { << " and not " << resolution; throw std::invalid_argument(oss.str()); } - return hpp::fcl::OcTree(octree); + return coal::OcTree(octree); } #endif @@ -649,6 +648,4 @@ std::shared_ptr makeRandomGeometry(NODE_TYPE node_type) { } } -} // namespace fcl - -} // namespace hpp +} // namespace coal diff --git a/test/utility.h b/test/utility.h index b81723ab9..96b09c26d 100644 --- a/test/utility.h +++ b/test/utility.h @@ -79,11 +79,10 @@ namespace octomap { #ifdef HPP_FCL_HAS_OCTOMAP -typedef hpp::fcl::shared_ptr OcTreePtr_t; +typedef coal::shared_ptr OcTreePtr_t; #endif } // namespace octomap -namespace hpp { -namespace fcl { +namespace coal { class BenchTimer { public: @@ -139,8 +138,8 @@ void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); #ifdef HPP_FCL_HAS_OCTOMAP -fcl::OcTree loadOctreeFile(const std::string& filename, - const FCL_REAL& resolution); +coal::OcTree loadOctreeFile(const std::string& filename, + const FCL_REAL& resolution); #endif /// @brief Generate one random transform whose translation is constrained by @@ -234,8 +233,6 @@ Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size); std::shared_ptr makeRandomGeometry(NODE_TYPE node_type); -} // namespace fcl - -} // namespace hpp +} // namespace coal #endif From fb4d468d764461cfb0ea83721bc4aa2772753b24 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 18 Jun 2024 17:23:51 +0200 Subject: [PATCH 02/57] format: apply pre-commit format to `internal/intersect.h` --- include/hpp/fcl/internal/intersect.h | 377 +++++++++++++-------------- 1 file changed, 187 insertions(+), 190 deletions(-) diff --git a/include/hpp/fcl/internal/intersect.h b/include/hpp/fcl/internal/intersect.h index 1166bc74f..d6dd5f518 100644 --- a/include/hpp/fcl/internal/intersect.h +++ b/include/hpp/fcl/internal/intersect.h @@ -1,190 +1,187 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2015, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Jia Pan */ - -#ifndef HPP_FCL_INTERSECT_H -#define HPP_FCL_INTERSECT_H - -/// @cond INTERNAL - -#include - -namespace hpp { -namespace fcl { - -/// @brief CCD intersect kernel among primitives -class HPP_FCL_DLLAPI Intersect { - public: - static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, - const Vec3f& v3, Vec3f* n, FCL_REAL* t); -}; // class Intersect - -/// @brief Project functions -class HPP_FCL_DLLAPI Project { - public: - struct HPP_FCL_DLLAPI ProjectResult { - /// @brief Parameterization of the projected point (based on the simplex to - /// be projected, use 2 or 3 or 4 of the array) - FCL_REAL parameterization[4]; - - /// @brief square distance from the query point to the projected simplex - FCL_REAL sqr_distance; - - /// @brief the code of the projection type - unsigned int encode; - - ProjectResult() : sqr_distance(-1), encode(0) {} - }; - - /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, - const Vec3f& p); - - /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& p); - - /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& d, - const Vec3f& p); - - /// @brief Project origin (0) onto line a-b - static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b); - - /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, - const Vec3f& c); - - /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& d); -}; - -/// @brief Triangle distance functions -class HPP_FCL_DLLAPI TriangleDistance { - public: - /// @brief Returns closest points between an segment pair. - /// The first segment is P + t * A - /// The second segment is Q + t * B - /// X, Y are the closest points on the two segments - /// VEC is the vector between X and Y - static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, - const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y); - - /// Compute squared distance between triangles - /// @param S and T are two triangles - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, - Vec3f& Q); - - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, Vec3f& P, - Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S and T are two triangles - /// @param R, Tl, rotation and translation applied to T, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S and T are two triangles - /// @param tf, rotation and translation applied to T, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, Vec3f& P, Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices - /// @param R, Tl, rotation and translation applied to T1, T2, T3, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); - - /// Compute squared distance between triangles - /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices - /// @param tf, rotation and translation applied to T1, T2, T3, - /// @retval P, Q closest points if triangles do not intersect. - /// @return squared distance if triangles do not intersect, 0 otherwise. - /// If the triangles are disjoint, P and Q give the closet points of - /// S and T respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points - /// from the triangles, not coincident points on the intersection of the - /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, Vec3f& P, Vec3f& Q); -}; - -} // namespace fcl - -} // namespace hpp - -/// @endcond - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jia Pan */ + +#ifndef COAL_INTERSECT_H +#define COAL_INTERSECT_H + +/// @cond INTERNAL + +#include + +namespace coal { + +/// @brief CCD intersect kernel among primitives +class HPP_FCL_DLLAPI Intersect { + public: + static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, + const Vec3f& v3, Vec3f* n, FCL_REAL* t); +}; // class Intersect + +/// @brief Project functions +class HPP_FCL_DLLAPI Project { + public: + struct HPP_FCL_DLLAPI ProjectResult { + /// @brief Parameterization of the projected point (based on the simplex to + /// be projected, use 2 or 3 or 4 of the array) + FCL_REAL parameterization[4]; + + /// @brief square distance from the query point to the projected simplex + FCL_REAL sqr_distance; + + /// @brief the code of the projection type + unsigned int encode; + + ProjectResult() : sqr_distance(-1), encode(0) {} + }; + + /// @brief Project point p onto line a-b + static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, + const Vec3f& p); + + /// @brief Project point p onto triangle a-b-c + static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, + const Vec3f& c, const Vec3f& p); + + /// @brief Project point p onto tetrahedra a-b-c-d + static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, + const Vec3f& c, const Vec3f& d, + const Vec3f& p); + + /// @brief Project origin (0) onto line a-b + static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b); + + /// @brief Project origin (0) onto triangle a-b-c + static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, + const Vec3f& c); + + /// @brief Project origin (0) onto tetrahedran a-b-c-d + static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, + const Vec3f& c, const Vec3f& d); +}; + +/// @brief Triangle distance functions +class HPP_FCL_DLLAPI TriangleDistance { + public: + /// @brief Returns closest points between an segment pair. + /// The first segment is P + t * A + /// The second segment is Q + t * B + /// X, Y are the closest points on the two segments + /// VEC is the vector between X and Y + static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, + const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y); + + /// Compute squared distance between triangles + /// @param S and T are two triangles + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, + Vec3f& Q); + + static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, Vec3f& P, + Vec3f& Q); + + /// Compute squared distance between triangles + /// @param S and T are two triangles + /// @param R, Tl, rotation and translation applied to T, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Matrix3f& R, const Vec3f& Tl, Vec3f& P, + Vec3f& Q); + + /// Compute squared distance between triangles + /// @param S and T are two triangles + /// @param tf, rotation and translation applied to T, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Transform3f& tf, Vec3f& P, Vec3f& Q); + + /// Compute squared distance between triangles + /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices + /// @param R, Tl, rotation and translation applied to T1, T2, T3, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Matrix3f& R, const Vec3f& Tl, Vec3f& P, + Vec3f& Q); + + /// Compute squared distance between triangles + /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices + /// @param tf, rotation and translation applied to T1, T2, T3, + /// @retval P, Q closest points if triangles do not intersect. + /// @return squared distance if triangles do not intersect, 0 otherwise. + /// If the triangles are disjoint, P and Q give the closet points of + /// S and T respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points + /// from the triangles, not coincident points on the intersection of the + /// triangles, as might be expected. + static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Transform3f& tf, Vec3f& P, Vec3f& Q); +}; + +} // namespace coal + +/// @endcond + +#endif From 52f406fdc80a44bde6fe35c8622533a491f1d1cb Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 18 Jun 2024 17:24:08 +0200 Subject: [PATCH 03/57] format: apply pre-commit format to `test/simple.cpp` --- test/simple.cpp | 488 ++++++++++++++++++++++++------------------------ 1 file changed, 244 insertions(+), 244 deletions(-) diff --git a/test/simple.cpp b/test/simple.cpp index 99a7ee521..b1582ef3a 100644 --- a/test/simple.cpp +++ b/test/simple.cpp @@ -1,244 +1,244 @@ -#define BOOST_TEST_MODULE FCL_SIMPLE -#include - -#include -#include -#include -#include "fcl_resources/config.h" -#include - -using namespace hpp::fcl; - -static FCL_REAL epsilon = 1e-6; - -static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; } - -BOOST_AUTO_TEST_CASE(projection_test_line) { - Vec3f v1(0, 0, 0); - Vec3f v2(2, 0, 0); - - Vec3f p(1, 0, 0); - Project::ProjectResult res = Project::projectLine(v1, v2, p); - BOOST_CHECK(res.encode == 3); - BOOST_CHECK(approx(res.sqr_distance, 0)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - - p = Vec3f(-1, 0, 0); - res = Project::projectLine(v1, v2, p); - BOOST_CHECK(res.encode == 1); - BOOST_CHECK(approx(res.sqr_distance, 1)); - BOOST_CHECK(approx(res.parameterization[0], 1)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - - p = Vec3f(3, 0, 0); - res = Project::projectLine(v1, v2, p); - BOOST_CHECK(res.encode == 2); - BOOST_CHECK(approx(res.sqr_distance, 1)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1)); -} - -BOOST_AUTO_TEST_CASE(projection_test_triangle) { - Vec3f v1(0, 0, 1); - Vec3f v2(0, 1, 0); - Vec3f v3(1, 0, 0); - - Vec3f p(1, 1, 1); - Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 7); - BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - - p = Vec3f(0, 0, 1.5); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 1); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 1)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - - p = Vec3f(1.5, 0, 0); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 4); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 1)); - - p = Vec3f(0, 1.5, 0); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 2); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - - p = Vec3f(1, 1, 0); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 6); - BOOST_CHECK(approx(res.sqr_distance, 0.5)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - - p = Vec3f(1, 0, 1); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 5); - BOOST_CHECK(approx(res.sqr_distance, 0.5)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - - p = Vec3f(0, 1, 1); - res = Project::projectTriangle(v1, v2, v3, p); - BOOST_CHECK(res.encode == 3); - BOOST_CHECK(approx(res.sqr_distance, 0.5)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0)); -} - -BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { - Vec3f v1(0, 0, 1); - Vec3f v2(0, 1, 0); - Vec3f v3(1, 0, 0); - Vec3f v4(1, 1, 1); - - Vec3f p(0.5, 0.5, 0.5); - Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 15); - BOOST_CHECK(approx(res.sqr_distance, 0)); - BOOST_CHECK(approx(res.parameterization[0], 0.25)); - BOOST_CHECK(approx(res.parameterization[1], 0.25)); - BOOST_CHECK(approx(res.parameterization[2], 0.25)); - BOOST_CHECK(approx(res.parameterization[3], 0.25)); - - p = Vec3f(0, 0, 0); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 7); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0, 1, 1); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 11); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - - p = Vec3f(1, 1, 0); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 14); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - - p = Vec3f(1, 0, 1); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 13); - BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - - p = Vec3f(1.5, 1.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 8); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 1)); - - p = Vec3f(1.5, -0.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 4); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 1)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(-0.5, -0.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 1); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 1)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(-0.5, 1.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 2); - BOOST_CHECK(approx(res.sqr_distance, 0.75)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 1)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0.5, -0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 5); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0.5, 1.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 10); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0.5)); - - p = Vec3f(1.5, 0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 12); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - BOOST_CHECK(approx(res.parameterization[3], 0.5)); - - p = Vec3f(-0.5, 0.5, 0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 3); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0)); - - p = Vec3f(0.5, 0.5, 1.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 9); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0.5)); - BOOST_CHECK(approx(res.parameterization[1], 0)); - BOOST_CHECK(approx(res.parameterization[2], 0)); - BOOST_CHECK(approx(res.parameterization[3], 0.5)); - - p = Vec3f(0.5, 0.5, -0.5); - res = Project::projectTetrahedra(v1, v2, v3, v4, p); - BOOST_CHECK(res.encode == 6); - BOOST_CHECK(approx(res.sqr_distance, 0.25)); - BOOST_CHECK(approx(res.parameterization[0], 0)); - BOOST_CHECK(approx(res.parameterization[1], 0.5)); - BOOST_CHECK(approx(res.parameterization[2], 0.5)); - BOOST_CHECK(approx(res.parameterization[3], 0)); -} +#define BOOST_TEST_MODULE FCL_SIMPLE +#include + +#include +#include +#include +#include "fcl_resources/config.h" +#include + +using namespace coal; + +static FCL_REAL epsilon = 1e-6; + +static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; } + +BOOST_AUTO_TEST_CASE(projection_test_line) { + Vec3f v1(0, 0, 0); + Vec3f v2(2, 0, 0); + + Vec3f p(1, 0, 0); + Project::ProjectResult res = Project::projectLine(v1, v2, p); + BOOST_CHECK(res.encode == 3); + BOOST_CHECK(approx(res.sqr_distance, 0)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + + p = Vec3f(-1, 0, 0); + res = Project::projectLine(v1, v2, p); + BOOST_CHECK(res.encode == 1); + BOOST_CHECK(approx(res.sqr_distance, 1)); + BOOST_CHECK(approx(res.parameterization[0], 1)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + + p = Vec3f(3, 0, 0); + res = Project::projectLine(v1, v2, p); + BOOST_CHECK(res.encode == 2); + BOOST_CHECK(approx(res.sqr_distance, 1)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1)); +} + +BOOST_AUTO_TEST_CASE(projection_test_triangle) { + Vec3f v1(0, 0, 1); + Vec3f v2(0, 1, 0); + Vec3f v3(1, 0, 0); + + Vec3f p(1, 1, 1); + Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 7); + BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + + p = Vec3f(0, 0, 1.5); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 1); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 1)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + + p = Vec3f(1.5, 0, 0); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 4); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 1)); + + p = Vec3f(0, 1.5, 0); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 2); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + + p = Vec3f(1, 1, 0); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 6); + BOOST_CHECK(approx(res.sqr_distance, 0.5)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + + p = Vec3f(1, 0, 1); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 5); + BOOST_CHECK(approx(res.sqr_distance, 0.5)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + + p = Vec3f(0, 1, 1); + res = Project::projectTriangle(v1, v2, v3, p); + BOOST_CHECK(res.encode == 3); + BOOST_CHECK(approx(res.sqr_distance, 0.5)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0)); +} + +BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { + Vec3f v1(0, 0, 1); + Vec3f v2(0, 1, 0); + Vec3f v3(1, 0, 0); + Vec3f v4(1, 1, 1); + + Vec3f p(0.5, 0.5, 0.5); + Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 15); + BOOST_CHECK(approx(res.sqr_distance, 0)); + BOOST_CHECK(approx(res.parameterization[0], 0.25)); + BOOST_CHECK(approx(res.parameterization[1], 0.25)); + BOOST_CHECK(approx(res.parameterization[2], 0.25)); + BOOST_CHECK(approx(res.parameterization[3], 0.25)); + + p = Vec3f(0, 0, 0); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 7); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3f(0, 1, 1); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 11); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); + + p = Vec3f(1, 1, 0); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 14); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); + + p = Vec3f(1, 0, 1); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 13); + BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[0], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); + BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); + + p = Vec3f(1.5, 1.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 8); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 1)); + + p = Vec3f(1.5, -0.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 4); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 1)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3f(-0.5, -0.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 1); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 1)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3f(-0.5, 1.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 2); + BOOST_CHECK(approx(res.sqr_distance, 0.75)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 1)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3f(0.5, -0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 5); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3f(0.5, 1.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 10); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0.5)); + + p = Vec3f(1.5, 0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 12); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + BOOST_CHECK(approx(res.parameterization[3], 0.5)); + + p = Vec3f(-0.5, 0.5, 0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 3); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0)); + + p = Vec3f(0.5, 0.5, 1.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 9); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0.5)); + BOOST_CHECK(approx(res.parameterization[1], 0)); + BOOST_CHECK(approx(res.parameterization[2], 0)); + BOOST_CHECK(approx(res.parameterization[3], 0.5)); + + p = Vec3f(0.5, 0.5, -0.5); + res = Project::projectTetrahedra(v1, v2, v3, v4, p); + BOOST_CHECK(res.encode == 6); + BOOST_CHECK(approx(res.sqr_distance, 0.25)); + BOOST_CHECK(approx(res.parameterization[0], 0)); + BOOST_CHECK(approx(res.parameterization[1], 0.5)); + BOOST_CHECK(approx(res.parameterization[2], 0.5)); + BOOST_CHECK(approx(res.parameterization[3], 0)); +} From 0e9342cd91f34d83df6405a171b1d8a080e6494a Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 18 Jun 2024 17:27:31 +0200 Subject: [PATCH 04/57] all: create `include/coal` folder and move headers there --- include/{hpp/fcl => coal}/BV/AABB.h | 0 include/{hpp/fcl => coal}/BV/BV.h | 0 include/{hpp/fcl => coal}/BV/BV_node.h | 0 include/{hpp/fcl => coal}/BV/OBB.h | 0 include/{hpp/fcl => coal}/BV/OBBRSS.h | 0 include/{hpp/fcl => coal}/BV/RSS.h | 0 include/{hpp/fcl => coal}/BV/kDOP.h | 0 include/{hpp/fcl => coal}/BV/kIOS.h | 0 include/{hpp/fcl => coal}/BVH/BVH_front.h | 0 include/{hpp/fcl => coal}/BVH/BVH_internal.h | 0 include/{hpp/fcl => coal}/BVH/BVH_model.h | 0 include/{hpp/fcl => coal}/BVH/BVH_utility.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase_SSaP.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase_SaP.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase_bruteforce.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase_callbacks.h | 0 .../{hpp/fcl => coal}/broadphase/broadphase_collision_manager.h | 0 .../broadphase/broadphase_continuous_collision_manager-inl.h | 0 .../broadphase/broadphase_continuous_collision_manager.h | 0 .../fcl => coal}/broadphase/broadphase_dynamic_AABB_tree-inl.h | 0 .../{hpp/fcl => coal}/broadphase/broadphase_dynamic_AABB_tree.h | 0 .../broadphase/broadphase_dynamic_AABB_tree_array-inl.h | 0 .../fcl => coal}/broadphase/broadphase_dynamic_AABB_tree_array.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase_interval_tree.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase_spatialhash-inl.h | 0 include/{hpp/fcl => coal}/broadphase/broadphase_spatialhash.h | 0 .../{hpp/fcl => coal}/broadphase/default_broadphase_callbacks.h | 0 include/{hpp/fcl => coal}/broadphase/detail/hierarchy_tree-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/hierarchy_tree.h | 0 .../fcl => coal}/broadphase/detail/hierarchy_tree_array-inl.h | 0 .../{hpp/fcl => coal}/broadphase/detail/hierarchy_tree_array.h | 0 include/{hpp/fcl => coal}/broadphase/detail/interval_tree.h | 0 .../{hpp/fcl => coal}/broadphase/detail/interval_tree_node-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/interval_tree_node.h | 0 include/{hpp/fcl => coal}/broadphase/detail/morton-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/morton.h | 0 include/{hpp/fcl => coal}/broadphase/detail/node_base-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/node_base.h | 0 include/{hpp/fcl => coal}/broadphase/detail/node_base_array-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/node_base_array.h | 0 .../{hpp/fcl => coal}/broadphase/detail/simple_hash_table-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/simple_hash_table.h | 0 include/{hpp/fcl => coal}/broadphase/detail/simple_interval-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/simple_interval.h | 0 .../{hpp/fcl => coal}/broadphase/detail/sparse_hash_table-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/sparse_hash_table.h | 0 include/{hpp/fcl => coal}/broadphase/detail/spatial_hash-inl.h | 0 include/{hpp/fcl => coal}/broadphase/detail/spatial_hash.h | 0 include/{hpp/fcl => coal}/collision.h | 0 include/{hpp/fcl => coal}/collision_data.h | 0 include/{hpp/fcl => coal}/collision_func_matrix.h | 0 include/{hpp/fcl => coal}/collision_object.h | 0 include/{hpp/fcl => coal}/collision_utility.h | 0 include/{hpp/fcl => coal}/contact_patch.h | 0 include/{hpp/fcl => coal}/contact_patch/contact_patch_solver.h | 0 include/{hpp/fcl => coal}/contact_patch/contact_patch_solver.hxx | 0 include/{hpp/fcl => coal}/contact_patch_func_matrix.h | 0 include/{hpp/fcl => coal}/data_types.h | 0 include/{hpp/fcl => coal}/distance.h | 0 include/{hpp/fcl => coal}/distance_func_matrix.h | 0 include/{hpp/fcl => coal}/doc.hh | 0 include/{hpp/fcl => coal}/fwd.hh | 0 include/{hpp/fcl => coal}/hfield.h | 0 include/{hpp/fcl => coal}/internal/BV_fitter.h | 0 include/{hpp/fcl => coal}/internal/BV_splitter.h | 0 include/{hpp/fcl => coal}/internal/intersect.h | 0 .../{hpp/fcl => coal}/internal/shape_shape_contact_patch_func.h | 0 include/{hpp/fcl => coal}/internal/shape_shape_func.h | 0 include/{hpp/fcl => coal}/internal/tools.h | 0 include/{hpp/fcl => coal}/internal/traversal.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_base.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_bvh_hfield.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_bvh_shape.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_bvhs.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_hfield_shape.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_octree.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_setup.h | 0 include/{hpp/fcl => coal}/internal/traversal_node_shapes.h | 0 include/{hpp/fcl => coal}/internal/traversal_recurse.h | 0 include/{hpp/fcl => coal}/logging.h | 0 include/{hpp/fcl => coal}/math/matrix_3f.h | 0 include/{hpp/fcl => coal}/math/transform.h | 0 include/{hpp/fcl => coal}/math/types.h | 0 include/{hpp/fcl => coal}/math/vec_3f.h | 0 include/{hpp/fcl => coal}/mesh_loader/assimp.h | 0 include/{hpp/fcl => coal}/mesh_loader/loader.h | 0 include/{hpp/fcl => coal}/narrowphase/gjk.h | 0 include/{hpp/fcl => coal}/narrowphase/minkowski_difference.h | 0 include/{hpp/fcl => coal}/narrowphase/narrowphase.h | 0 include/{hpp/fcl => coal}/narrowphase/narrowphase_defaults.h | 0 include/{hpp/fcl => coal}/narrowphase/support_functions.h | 0 include/{hpp/fcl => coal}/octree.h | 0 include/{hpp/fcl => coal}/serialization/AABB.h | 0 include/{hpp/fcl => coal}/serialization/BVH_model.h | 0 include/{hpp/fcl => coal}/serialization/BV_node.h | 0 include/{hpp/fcl => coal}/serialization/BV_splitter.h | 0 include/{hpp/fcl => coal}/serialization/OBB.h | 0 include/{hpp/fcl => coal}/serialization/OBBRSS.h | 0 include/{hpp/fcl => coal}/serialization/RSS.h | 0 include/{hpp/fcl => coal}/serialization/archive.h | 0 include/{hpp/fcl => coal}/serialization/collision_data.h | 0 include/{hpp/fcl => coal}/serialization/collision_object.h | 0 include/{hpp/fcl => coal}/serialization/contact_patch.h | 0 include/{hpp/fcl => coal}/serialization/convex.h | 0 include/{hpp/fcl => coal}/serialization/eigen.h | 0 include/{hpp/fcl => coal}/serialization/fwd.h | 0 include/{hpp/fcl => coal}/serialization/geometric_shapes.h | 0 include/{hpp/fcl => coal}/serialization/hfield.h | 0 include/{hpp/fcl => coal}/serialization/kDOP.h | 0 include/{hpp/fcl => coal}/serialization/kIOS.h | 0 include/{hpp/fcl => coal}/serialization/memory.h | 0 include/{hpp/fcl => coal}/serialization/octree.h | 0 include/{hpp/fcl => coal}/serialization/quadrilateral.h | 0 include/{hpp/fcl => coal}/serialization/serializer.h | 0 include/{hpp/fcl => coal}/serialization/transform.h | 0 include/{hpp/fcl => coal}/serialization/triangle.h | 0 include/{hpp/fcl => coal}/shape/convex.h | 0 include/{hpp/fcl => coal}/shape/details/convex.hxx | 0 include/{hpp/fcl => coal}/shape/geometric_shape_to_BVH_model.h | 0 include/{hpp/fcl => coal}/shape/geometric_shapes.h | 0 include/{hpp/fcl => coal}/shape/geometric_shapes_traits.h | 0 include/{hpp/fcl => coal}/shape/geometric_shapes_utility.h | 0 include/{hpp/fcl => coal}/timings.h | 0 124 files changed, 0 insertions(+), 0 deletions(-) rename include/{hpp/fcl => coal}/BV/AABB.h (100%) rename include/{hpp/fcl => coal}/BV/BV.h (100%) rename include/{hpp/fcl => coal}/BV/BV_node.h (100%) rename include/{hpp/fcl => coal}/BV/OBB.h (100%) rename include/{hpp/fcl => coal}/BV/OBBRSS.h (100%) rename include/{hpp/fcl => coal}/BV/RSS.h (100%) rename include/{hpp/fcl => coal}/BV/kDOP.h (100%) rename include/{hpp/fcl => coal}/BV/kIOS.h (100%) rename include/{hpp/fcl => coal}/BVH/BVH_front.h (100%) rename include/{hpp/fcl => coal}/BVH/BVH_internal.h (100%) rename include/{hpp/fcl => coal}/BVH/BVH_model.h (100%) rename include/{hpp/fcl => coal}/BVH/BVH_utility.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_SSaP.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_SaP.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_bruteforce.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_callbacks.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_collision_manager.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_continuous_collision_manager-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_continuous_collision_manager.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_dynamic_AABB_tree-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_dynamic_AABB_tree.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_dynamic_AABB_tree_array-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_dynamic_AABB_tree_array.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_interval_tree.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_spatialhash-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/broadphase_spatialhash.h (100%) rename include/{hpp/fcl => coal}/broadphase/default_broadphase_callbacks.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/hierarchy_tree-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/hierarchy_tree.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/hierarchy_tree_array-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/hierarchy_tree_array.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/interval_tree.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/interval_tree_node-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/interval_tree_node.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/morton-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/morton.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/node_base-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/node_base.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/node_base_array-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/node_base_array.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/simple_hash_table-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/simple_hash_table.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/simple_interval-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/simple_interval.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/sparse_hash_table-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/sparse_hash_table.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/spatial_hash-inl.h (100%) rename include/{hpp/fcl => coal}/broadphase/detail/spatial_hash.h (100%) rename include/{hpp/fcl => coal}/collision.h (100%) rename include/{hpp/fcl => coal}/collision_data.h (100%) rename include/{hpp/fcl => coal}/collision_func_matrix.h (100%) rename include/{hpp/fcl => coal}/collision_object.h (100%) rename include/{hpp/fcl => coal}/collision_utility.h (100%) rename include/{hpp/fcl => coal}/contact_patch.h (100%) rename include/{hpp/fcl => coal}/contact_patch/contact_patch_solver.h (100%) rename include/{hpp/fcl => coal}/contact_patch/contact_patch_solver.hxx (100%) rename include/{hpp/fcl => coal}/contact_patch_func_matrix.h (100%) rename include/{hpp/fcl => coal}/data_types.h (100%) rename include/{hpp/fcl => coal}/distance.h (100%) rename include/{hpp/fcl => coal}/distance_func_matrix.h (100%) rename include/{hpp/fcl => coal}/doc.hh (100%) rename include/{hpp/fcl => coal}/fwd.hh (100%) rename include/{hpp/fcl => coal}/hfield.h (100%) rename include/{hpp/fcl => coal}/internal/BV_fitter.h (100%) rename include/{hpp/fcl => coal}/internal/BV_splitter.h (100%) rename include/{hpp/fcl => coal}/internal/intersect.h (100%) rename include/{hpp/fcl => coal}/internal/shape_shape_contact_patch_func.h (100%) rename include/{hpp/fcl => coal}/internal/shape_shape_func.h (100%) rename include/{hpp/fcl => coal}/internal/tools.h (100%) rename include/{hpp/fcl => coal}/internal/traversal.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_base.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_bvh_hfield.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_bvh_shape.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_bvhs.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_hfield_shape.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_octree.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_setup.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_node_shapes.h (100%) rename include/{hpp/fcl => coal}/internal/traversal_recurse.h (100%) rename include/{hpp/fcl => coal}/logging.h (100%) rename include/{hpp/fcl => coal}/math/matrix_3f.h (100%) rename include/{hpp/fcl => coal}/math/transform.h (100%) rename include/{hpp/fcl => coal}/math/types.h (100%) rename include/{hpp/fcl => coal}/math/vec_3f.h (100%) rename include/{hpp/fcl => coal}/mesh_loader/assimp.h (100%) rename include/{hpp/fcl => coal}/mesh_loader/loader.h (100%) rename include/{hpp/fcl => coal}/narrowphase/gjk.h (100%) rename include/{hpp/fcl => coal}/narrowphase/minkowski_difference.h (100%) rename include/{hpp/fcl => coal}/narrowphase/narrowphase.h (100%) rename include/{hpp/fcl => coal}/narrowphase/narrowphase_defaults.h (100%) rename include/{hpp/fcl => coal}/narrowphase/support_functions.h (100%) rename include/{hpp/fcl => coal}/octree.h (100%) rename include/{hpp/fcl => coal}/serialization/AABB.h (100%) rename include/{hpp/fcl => coal}/serialization/BVH_model.h (100%) rename include/{hpp/fcl => coal}/serialization/BV_node.h (100%) rename include/{hpp/fcl => coal}/serialization/BV_splitter.h (100%) rename include/{hpp/fcl => coal}/serialization/OBB.h (100%) rename include/{hpp/fcl => coal}/serialization/OBBRSS.h (100%) rename include/{hpp/fcl => coal}/serialization/RSS.h (100%) rename include/{hpp/fcl => coal}/serialization/archive.h (100%) rename include/{hpp/fcl => coal}/serialization/collision_data.h (100%) rename include/{hpp/fcl => coal}/serialization/collision_object.h (100%) rename include/{hpp/fcl => coal}/serialization/contact_patch.h (100%) rename include/{hpp/fcl => coal}/serialization/convex.h (100%) rename include/{hpp/fcl => coal}/serialization/eigen.h (100%) rename include/{hpp/fcl => coal}/serialization/fwd.h (100%) rename include/{hpp/fcl => coal}/serialization/geometric_shapes.h (100%) rename include/{hpp/fcl => coal}/serialization/hfield.h (100%) rename include/{hpp/fcl => coal}/serialization/kDOP.h (100%) rename include/{hpp/fcl => coal}/serialization/kIOS.h (100%) rename include/{hpp/fcl => coal}/serialization/memory.h (100%) rename include/{hpp/fcl => coal}/serialization/octree.h (100%) rename include/{hpp/fcl => coal}/serialization/quadrilateral.h (100%) rename include/{hpp/fcl => coal}/serialization/serializer.h (100%) rename include/{hpp/fcl => coal}/serialization/transform.h (100%) rename include/{hpp/fcl => coal}/serialization/triangle.h (100%) rename include/{hpp/fcl => coal}/shape/convex.h (100%) rename include/{hpp/fcl => coal}/shape/details/convex.hxx (100%) rename include/{hpp/fcl => coal}/shape/geometric_shape_to_BVH_model.h (100%) rename include/{hpp/fcl => coal}/shape/geometric_shapes.h (100%) rename include/{hpp/fcl => coal}/shape/geometric_shapes_traits.h (100%) rename include/{hpp/fcl => coal}/shape/geometric_shapes_utility.h (100%) rename include/{hpp/fcl => coal}/timings.h (100%) diff --git a/include/hpp/fcl/BV/AABB.h b/include/coal/BV/AABB.h similarity index 100% rename from include/hpp/fcl/BV/AABB.h rename to include/coal/BV/AABB.h diff --git a/include/hpp/fcl/BV/BV.h b/include/coal/BV/BV.h similarity index 100% rename from include/hpp/fcl/BV/BV.h rename to include/coal/BV/BV.h diff --git a/include/hpp/fcl/BV/BV_node.h b/include/coal/BV/BV_node.h similarity index 100% rename from include/hpp/fcl/BV/BV_node.h rename to include/coal/BV/BV_node.h diff --git a/include/hpp/fcl/BV/OBB.h b/include/coal/BV/OBB.h similarity index 100% rename from include/hpp/fcl/BV/OBB.h rename to include/coal/BV/OBB.h diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/coal/BV/OBBRSS.h similarity index 100% rename from include/hpp/fcl/BV/OBBRSS.h rename to include/coal/BV/OBBRSS.h diff --git a/include/hpp/fcl/BV/RSS.h b/include/coal/BV/RSS.h similarity index 100% rename from include/hpp/fcl/BV/RSS.h rename to include/coal/BV/RSS.h diff --git a/include/hpp/fcl/BV/kDOP.h b/include/coal/BV/kDOP.h similarity index 100% rename from include/hpp/fcl/BV/kDOP.h rename to include/coal/BV/kDOP.h diff --git a/include/hpp/fcl/BV/kIOS.h b/include/coal/BV/kIOS.h similarity index 100% rename from include/hpp/fcl/BV/kIOS.h rename to include/coal/BV/kIOS.h diff --git a/include/hpp/fcl/BVH/BVH_front.h b/include/coal/BVH/BVH_front.h similarity index 100% rename from include/hpp/fcl/BVH/BVH_front.h rename to include/coal/BVH/BVH_front.h diff --git a/include/hpp/fcl/BVH/BVH_internal.h b/include/coal/BVH/BVH_internal.h similarity index 100% rename from include/hpp/fcl/BVH/BVH_internal.h rename to include/coal/BVH/BVH_internal.h diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/coal/BVH/BVH_model.h similarity index 100% rename from include/hpp/fcl/BVH/BVH_model.h rename to include/coal/BVH/BVH_model.h diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h similarity index 100% rename from include/hpp/fcl/BVH/BVH_utility.h rename to include/coal/BVH/BVH_utility.h diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/coal/broadphase/broadphase.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase.h rename to include/coal/broadphase/broadphase.h diff --git a/include/hpp/fcl/broadphase/broadphase_SSaP.h b/include/coal/broadphase/broadphase_SSaP.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_SSaP.h rename to include/coal/broadphase/broadphase_SSaP.h diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_SaP.h rename to include/coal/broadphase/broadphase_SaP.h diff --git a/include/hpp/fcl/broadphase/broadphase_bruteforce.h b/include/coal/broadphase/broadphase_bruteforce.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_bruteforce.h rename to include/coal/broadphase/broadphase_bruteforce.h diff --git a/include/hpp/fcl/broadphase/broadphase_callbacks.h b/include/coal/broadphase/broadphase_callbacks.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_callbacks.h rename to include/coal/broadphase/broadphase_callbacks.h diff --git a/include/hpp/fcl/broadphase/broadphase_collision_manager.h b/include/coal/broadphase/broadphase_collision_manager.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_collision_manager.h rename to include/coal/broadphase/broadphase_collision_manager.h diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h rename to include/coal/broadphase/broadphase_continuous_collision_manager-inl.h diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/coal/broadphase/broadphase_continuous_collision_manager.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h rename to include/coal/broadphase/broadphase_continuous_collision_manager.h diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h rename to include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h rename to include/coal/broadphase/broadphase_dynamic_AABB_tree.h diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h rename to include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h rename to include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h diff --git a/include/hpp/fcl/broadphase/broadphase_interval_tree.h b/include/coal/broadphase/broadphase_interval_tree.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_interval_tree.h rename to include/coal/broadphase/broadphase_interval_tree.h diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h rename to include/coal/broadphase/broadphase_spatialhash-inl.h diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash.h b/include/coal/broadphase/broadphase_spatialhash.h similarity index 100% rename from include/hpp/fcl/broadphase/broadphase_spatialhash.h rename to include/coal/broadphase/broadphase_spatialhash.h diff --git a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h b/include/coal/broadphase/default_broadphase_callbacks.h similarity index 100% rename from include/hpp/fcl/broadphase/default_broadphase_callbacks.h rename to include/coal/broadphase/default_broadphase_callbacks.h diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h b/include/coal/broadphase/detail/hierarchy_tree-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h rename to include/coal/broadphase/detail/hierarchy_tree-inl.h diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h b/include/coal/broadphase/detail/hierarchy_tree.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/hierarchy_tree.h rename to include/coal/broadphase/detail/hierarchy_tree.h diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h rename to include/coal/broadphase/detail/hierarchy_tree_array-inl.h diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h b/include/coal/broadphase/detail/hierarchy_tree_array.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h rename to include/coal/broadphase/detail/hierarchy_tree_array.h diff --git a/include/hpp/fcl/broadphase/detail/interval_tree.h b/include/coal/broadphase/detail/interval_tree.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/interval_tree.h rename to include/coal/broadphase/detail/interval_tree.h diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h b/include/coal/broadphase/detail/interval_tree_node-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/interval_tree_node-inl.h rename to include/coal/broadphase/detail/interval_tree_node-inl.h diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node.h b/include/coal/broadphase/detail/interval_tree_node.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/interval_tree_node.h rename to include/coal/broadphase/detail/interval_tree_node.h diff --git a/include/hpp/fcl/broadphase/detail/morton-inl.h b/include/coal/broadphase/detail/morton-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/morton-inl.h rename to include/coal/broadphase/detail/morton-inl.h diff --git a/include/hpp/fcl/broadphase/detail/morton.h b/include/coal/broadphase/detail/morton.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/morton.h rename to include/coal/broadphase/detail/morton.h diff --git a/include/hpp/fcl/broadphase/detail/node_base-inl.h b/include/coal/broadphase/detail/node_base-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/node_base-inl.h rename to include/coal/broadphase/detail/node_base-inl.h diff --git a/include/hpp/fcl/broadphase/detail/node_base.h b/include/coal/broadphase/detail/node_base.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/node_base.h rename to include/coal/broadphase/detail/node_base.h diff --git a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h b/include/coal/broadphase/detail/node_base_array-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/node_base_array-inl.h rename to include/coal/broadphase/detail/node_base_array-inl.h diff --git a/include/hpp/fcl/broadphase/detail/node_base_array.h b/include/coal/broadphase/detail/node_base_array.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/node_base_array.h rename to include/coal/broadphase/detail/node_base_array.h diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/coal/broadphase/detail/simple_hash_table-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h rename to include/coal/broadphase/detail/simple_hash_table-inl.h diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/coal/broadphase/detail/simple_hash_table.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/simple_hash_table.h rename to include/coal/broadphase/detail/simple_hash_table.h diff --git a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h b/include/coal/broadphase/detail/simple_interval-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/simple_interval-inl.h rename to include/coal/broadphase/detail/simple_interval-inl.h diff --git a/include/hpp/fcl/broadphase/detail/simple_interval.h b/include/coal/broadphase/detail/simple_interval.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/simple_interval.h rename to include/coal/broadphase/detail/simple_interval.h diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h b/include/coal/broadphase/detail/sparse_hash_table-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h rename to include/coal/broadphase/detail/sparse_hash_table-inl.h diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/coal/broadphase/detail/sparse_hash_table.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/sparse_hash_table.h rename to include/coal/broadphase/detail/sparse_hash_table.h diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h b/include/coal/broadphase/detail/spatial_hash-inl.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/spatial_hash-inl.h rename to include/coal/broadphase/detail/spatial_hash-inl.h diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash.h b/include/coal/broadphase/detail/spatial_hash.h similarity index 100% rename from include/hpp/fcl/broadphase/detail/spatial_hash.h rename to include/coal/broadphase/detail/spatial_hash.h diff --git a/include/hpp/fcl/collision.h b/include/coal/collision.h similarity index 100% rename from include/hpp/fcl/collision.h rename to include/coal/collision.h diff --git a/include/hpp/fcl/collision_data.h b/include/coal/collision_data.h similarity index 100% rename from include/hpp/fcl/collision_data.h rename to include/coal/collision_data.h diff --git a/include/hpp/fcl/collision_func_matrix.h b/include/coal/collision_func_matrix.h similarity index 100% rename from include/hpp/fcl/collision_func_matrix.h rename to include/coal/collision_func_matrix.h diff --git a/include/hpp/fcl/collision_object.h b/include/coal/collision_object.h similarity index 100% rename from include/hpp/fcl/collision_object.h rename to include/coal/collision_object.h diff --git a/include/hpp/fcl/collision_utility.h b/include/coal/collision_utility.h similarity index 100% rename from include/hpp/fcl/collision_utility.h rename to include/coal/collision_utility.h diff --git a/include/hpp/fcl/contact_patch.h b/include/coal/contact_patch.h similarity index 100% rename from include/hpp/fcl/contact_patch.h rename to include/coal/contact_patch.h diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h similarity index 100% rename from include/hpp/fcl/contact_patch/contact_patch_solver.h rename to include/coal/contact_patch/contact_patch_solver.h diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx similarity index 100% rename from include/hpp/fcl/contact_patch/contact_patch_solver.hxx rename to include/coal/contact_patch/contact_patch_solver.hxx diff --git a/include/hpp/fcl/contact_patch_func_matrix.h b/include/coal/contact_patch_func_matrix.h similarity index 100% rename from include/hpp/fcl/contact_patch_func_matrix.h rename to include/coal/contact_patch_func_matrix.h diff --git a/include/hpp/fcl/data_types.h b/include/coal/data_types.h similarity index 100% rename from include/hpp/fcl/data_types.h rename to include/coal/data_types.h diff --git a/include/hpp/fcl/distance.h b/include/coal/distance.h similarity index 100% rename from include/hpp/fcl/distance.h rename to include/coal/distance.h diff --git a/include/hpp/fcl/distance_func_matrix.h b/include/coal/distance_func_matrix.h similarity index 100% rename from include/hpp/fcl/distance_func_matrix.h rename to include/coal/distance_func_matrix.h diff --git a/include/hpp/fcl/doc.hh b/include/coal/doc.hh similarity index 100% rename from include/hpp/fcl/doc.hh rename to include/coal/doc.hh diff --git a/include/hpp/fcl/fwd.hh b/include/coal/fwd.hh similarity index 100% rename from include/hpp/fcl/fwd.hh rename to include/coal/fwd.hh diff --git a/include/hpp/fcl/hfield.h b/include/coal/hfield.h similarity index 100% rename from include/hpp/fcl/hfield.h rename to include/coal/hfield.h diff --git a/include/hpp/fcl/internal/BV_fitter.h b/include/coal/internal/BV_fitter.h similarity index 100% rename from include/hpp/fcl/internal/BV_fitter.h rename to include/coal/internal/BV_fitter.h diff --git a/include/hpp/fcl/internal/BV_splitter.h b/include/coal/internal/BV_splitter.h similarity index 100% rename from include/hpp/fcl/internal/BV_splitter.h rename to include/coal/internal/BV_splitter.h diff --git a/include/hpp/fcl/internal/intersect.h b/include/coal/internal/intersect.h similarity index 100% rename from include/hpp/fcl/internal/intersect.h rename to include/coal/internal/intersect.h diff --git a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h b/include/coal/internal/shape_shape_contact_patch_func.h similarity index 100% rename from include/hpp/fcl/internal/shape_shape_contact_patch_func.h rename to include/coal/internal/shape_shape_contact_patch_func.h diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h similarity index 100% rename from include/hpp/fcl/internal/shape_shape_func.h rename to include/coal/internal/shape_shape_func.h diff --git a/include/hpp/fcl/internal/tools.h b/include/coal/internal/tools.h similarity index 100% rename from include/hpp/fcl/internal/tools.h rename to include/coal/internal/tools.h diff --git a/include/hpp/fcl/internal/traversal.h b/include/coal/internal/traversal.h similarity index 100% rename from include/hpp/fcl/internal/traversal.h rename to include/coal/internal/traversal.h diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/coal/internal/traversal_node_base.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_base.h rename to include/coal/internal/traversal_node_base.h diff --git a/include/hpp/fcl/internal/traversal_node_bvh_hfield.h b/include/coal/internal/traversal_node_bvh_hfield.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_bvh_hfield.h rename to include/coal/internal/traversal_node_bvh_hfield.h diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_bvh_shape.h rename to include/coal/internal/traversal_node_bvh_shape.h diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/coal/internal/traversal_node_bvhs.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_bvhs.h rename to include/coal/internal/traversal_node_bvhs.h diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_hfield_shape.h rename to include/coal/internal/traversal_node_hfield_shape.h diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_octree.h rename to include/coal/internal/traversal_node_octree.h diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_setup.h rename to include/coal/internal/traversal_node_setup.h diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/coal/internal/traversal_node_shapes.h similarity index 100% rename from include/hpp/fcl/internal/traversal_node_shapes.h rename to include/coal/internal/traversal_node_shapes.h diff --git a/include/hpp/fcl/internal/traversal_recurse.h b/include/coal/internal/traversal_recurse.h similarity index 100% rename from include/hpp/fcl/internal/traversal_recurse.h rename to include/coal/internal/traversal_recurse.h diff --git a/include/hpp/fcl/logging.h b/include/coal/logging.h similarity index 100% rename from include/hpp/fcl/logging.h rename to include/coal/logging.h diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/coal/math/matrix_3f.h similarity index 100% rename from include/hpp/fcl/math/matrix_3f.h rename to include/coal/math/matrix_3f.h diff --git a/include/hpp/fcl/math/transform.h b/include/coal/math/transform.h similarity index 100% rename from include/hpp/fcl/math/transform.h rename to include/coal/math/transform.h diff --git a/include/hpp/fcl/math/types.h b/include/coal/math/types.h similarity index 100% rename from include/hpp/fcl/math/types.h rename to include/coal/math/types.h diff --git a/include/hpp/fcl/math/vec_3f.h b/include/coal/math/vec_3f.h similarity index 100% rename from include/hpp/fcl/math/vec_3f.h rename to include/coal/math/vec_3f.h diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/coal/mesh_loader/assimp.h similarity index 100% rename from include/hpp/fcl/mesh_loader/assimp.h rename to include/coal/mesh_loader/assimp.h diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/coal/mesh_loader/loader.h similarity index 100% rename from include/hpp/fcl/mesh_loader/loader.h rename to include/coal/mesh_loader/loader.h diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h similarity index 100% rename from include/hpp/fcl/narrowphase/gjk.h rename to include/coal/narrowphase/gjk.h diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h similarity index 100% rename from include/hpp/fcl/narrowphase/minkowski_difference.h rename to include/coal/narrowphase/minkowski_difference.h diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h similarity index 100% rename from include/hpp/fcl/narrowphase/narrowphase.h rename to include/coal/narrowphase/narrowphase.h diff --git a/include/hpp/fcl/narrowphase/narrowphase_defaults.h b/include/coal/narrowphase/narrowphase_defaults.h similarity index 100% rename from include/hpp/fcl/narrowphase/narrowphase_defaults.h rename to include/coal/narrowphase/narrowphase_defaults.h diff --git a/include/hpp/fcl/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h similarity index 100% rename from include/hpp/fcl/narrowphase/support_functions.h rename to include/coal/narrowphase/support_functions.h diff --git a/include/hpp/fcl/octree.h b/include/coal/octree.h similarity index 100% rename from include/hpp/fcl/octree.h rename to include/coal/octree.h diff --git a/include/hpp/fcl/serialization/AABB.h b/include/coal/serialization/AABB.h similarity index 100% rename from include/hpp/fcl/serialization/AABB.h rename to include/coal/serialization/AABB.h diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/coal/serialization/BVH_model.h similarity index 100% rename from include/hpp/fcl/serialization/BVH_model.h rename to include/coal/serialization/BVH_model.h diff --git a/include/hpp/fcl/serialization/BV_node.h b/include/coal/serialization/BV_node.h similarity index 100% rename from include/hpp/fcl/serialization/BV_node.h rename to include/coal/serialization/BV_node.h diff --git a/include/hpp/fcl/serialization/BV_splitter.h b/include/coal/serialization/BV_splitter.h similarity index 100% rename from include/hpp/fcl/serialization/BV_splitter.h rename to include/coal/serialization/BV_splitter.h diff --git a/include/hpp/fcl/serialization/OBB.h b/include/coal/serialization/OBB.h similarity index 100% rename from include/hpp/fcl/serialization/OBB.h rename to include/coal/serialization/OBB.h diff --git a/include/hpp/fcl/serialization/OBBRSS.h b/include/coal/serialization/OBBRSS.h similarity index 100% rename from include/hpp/fcl/serialization/OBBRSS.h rename to include/coal/serialization/OBBRSS.h diff --git a/include/hpp/fcl/serialization/RSS.h b/include/coal/serialization/RSS.h similarity index 100% rename from include/hpp/fcl/serialization/RSS.h rename to include/coal/serialization/RSS.h diff --git a/include/hpp/fcl/serialization/archive.h b/include/coal/serialization/archive.h similarity index 100% rename from include/hpp/fcl/serialization/archive.h rename to include/coal/serialization/archive.h diff --git a/include/hpp/fcl/serialization/collision_data.h b/include/coal/serialization/collision_data.h similarity index 100% rename from include/hpp/fcl/serialization/collision_data.h rename to include/coal/serialization/collision_data.h diff --git a/include/hpp/fcl/serialization/collision_object.h b/include/coal/serialization/collision_object.h similarity index 100% rename from include/hpp/fcl/serialization/collision_object.h rename to include/coal/serialization/collision_object.h diff --git a/include/hpp/fcl/serialization/contact_patch.h b/include/coal/serialization/contact_patch.h similarity index 100% rename from include/hpp/fcl/serialization/contact_patch.h rename to include/coal/serialization/contact_patch.h diff --git a/include/hpp/fcl/serialization/convex.h b/include/coal/serialization/convex.h similarity index 100% rename from include/hpp/fcl/serialization/convex.h rename to include/coal/serialization/convex.h diff --git a/include/hpp/fcl/serialization/eigen.h b/include/coal/serialization/eigen.h similarity index 100% rename from include/hpp/fcl/serialization/eigen.h rename to include/coal/serialization/eigen.h diff --git a/include/hpp/fcl/serialization/fwd.h b/include/coal/serialization/fwd.h similarity index 100% rename from include/hpp/fcl/serialization/fwd.h rename to include/coal/serialization/fwd.h diff --git a/include/hpp/fcl/serialization/geometric_shapes.h b/include/coal/serialization/geometric_shapes.h similarity index 100% rename from include/hpp/fcl/serialization/geometric_shapes.h rename to include/coal/serialization/geometric_shapes.h diff --git a/include/hpp/fcl/serialization/hfield.h b/include/coal/serialization/hfield.h similarity index 100% rename from include/hpp/fcl/serialization/hfield.h rename to include/coal/serialization/hfield.h diff --git a/include/hpp/fcl/serialization/kDOP.h b/include/coal/serialization/kDOP.h similarity index 100% rename from include/hpp/fcl/serialization/kDOP.h rename to include/coal/serialization/kDOP.h diff --git a/include/hpp/fcl/serialization/kIOS.h b/include/coal/serialization/kIOS.h similarity index 100% rename from include/hpp/fcl/serialization/kIOS.h rename to include/coal/serialization/kIOS.h diff --git a/include/hpp/fcl/serialization/memory.h b/include/coal/serialization/memory.h similarity index 100% rename from include/hpp/fcl/serialization/memory.h rename to include/coal/serialization/memory.h diff --git a/include/hpp/fcl/serialization/octree.h b/include/coal/serialization/octree.h similarity index 100% rename from include/hpp/fcl/serialization/octree.h rename to include/coal/serialization/octree.h diff --git a/include/hpp/fcl/serialization/quadrilateral.h b/include/coal/serialization/quadrilateral.h similarity index 100% rename from include/hpp/fcl/serialization/quadrilateral.h rename to include/coal/serialization/quadrilateral.h diff --git a/include/hpp/fcl/serialization/serializer.h b/include/coal/serialization/serializer.h similarity index 100% rename from include/hpp/fcl/serialization/serializer.h rename to include/coal/serialization/serializer.h diff --git a/include/hpp/fcl/serialization/transform.h b/include/coal/serialization/transform.h similarity index 100% rename from include/hpp/fcl/serialization/transform.h rename to include/coal/serialization/transform.h diff --git a/include/hpp/fcl/serialization/triangle.h b/include/coal/serialization/triangle.h similarity index 100% rename from include/hpp/fcl/serialization/triangle.h rename to include/coal/serialization/triangle.h diff --git a/include/hpp/fcl/shape/convex.h b/include/coal/shape/convex.h similarity index 100% rename from include/hpp/fcl/shape/convex.h rename to include/coal/shape/convex.h diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/coal/shape/details/convex.hxx similarity index 100% rename from include/hpp/fcl/shape/details/convex.hxx rename to include/coal/shape/details/convex.hxx diff --git a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h similarity index 100% rename from include/hpp/fcl/shape/geometric_shape_to_BVH_model.h rename to include/coal/shape/geometric_shape_to_BVH_model.h diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h similarity index 100% rename from include/hpp/fcl/shape/geometric_shapes.h rename to include/coal/shape/geometric_shapes.h diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/coal/shape/geometric_shapes_traits.h similarity index 100% rename from include/hpp/fcl/shape/geometric_shapes_traits.h rename to include/coal/shape/geometric_shapes_traits.h diff --git a/include/hpp/fcl/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h similarity index 100% rename from include/hpp/fcl/shape/geometric_shapes_utility.h rename to include/coal/shape/geometric_shapes_utility.h diff --git a/include/hpp/fcl/timings.h b/include/coal/timings.h similarity index 100% rename from include/hpp/fcl/timings.h rename to include/coal/timings.h From 14987e91fa7adea562e113ff25c2dcf6e4db1045 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 19 Jun 2024 11:40:17 +0200 Subject: [PATCH 05/57] all: change `#include "hpp/fcl/..."` to `#include "coal/..."` --- CMakeLists.txt | 248 +++++++++--------- include/coal/BV/AABB.h | 14 +- include/coal/BV/BV.h | 14 +- include/coal/BV/BV_node.h | 8 +- include/coal/BV/OBB.h | 20 +- include/coal/BV/OBBRSS.h | 6 +- include/coal/BV/RSS.h | 20 +- include/coal/BV/kDOP.h | 8 +- include/coal/BV/kIOS.h | 24 +- include/coal/BVH/BVH_front.h | 4 +- include/coal/BVH/BVH_internal.h | 2 +- include/coal/BVH/BVH_model.h | 12 +- include/coal/BVH/BVH_utility.h | 79 +++--- include/coal/broadphase/broadphase.h | 16 +- include/coal/broadphase/broadphase_SSaP.h | 4 +- include/coal/broadphase/broadphase_SaP.h | 8 +- .../coal/broadphase/broadphase_bruteforce.h | 4 +- .../coal/broadphase/broadphase_callbacks.h | 8 +- .../broadphase/broadphase_collision_manager.h | 6 +- ...adphase_continuous_collision_manager-inl.h | 2 +- .../broadphase_continuous_collision_manager.h | 10 +- .../broadphase_dynamic_AABB_tree-inl.h | 8 +- .../broadphase/broadphase_dynamic_AABB_tree.h | 16 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 6 +- .../broadphase_dynamic_AABB_tree_array.h | 16 +- .../broadphase/broadphase_interval_tree.h | 10 +- .../broadphase/broadphase_spatialhash-inl.h | 2 +- .../coal/broadphase/broadphase_spatialhash.h | 12 +- .../broadphase/default_broadphase_callbacks.h | 22 +- .../broadphase/detail/hierarchy_tree-inl.h | 2 +- .../coal/broadphase/detail/hierarchy_tree.h | 10 +- .../detail/hierarchy_tree_array-inl.h | 2 +- .../broadphase/detail/hierarchy_tree_array.h | 10 +- .../coal/broadphase/detail/interval_tree.h | 6 +- .../detail/interval_tree_node-inl.h | 2 +- .../broadphase/detail/interval_tree_node.h | 8 +- include/coal/broadphase/detail/morton-inl.h | 2 +- include/coal/broadphase/detail/morton.h | 8 +- .../coal/broadphase/detail/node_base-inl.h | 2 +- include/coal/broadphase/detail/node_base.h | 4 +- .../broadphase/detail/node_base_array-inl.h | 2 +- .../coal/broadphase/detail/node_base_array.h | 4 +- .../broadphase/detail/simple_hash_table-inl.h | 2 +- .../broadphase/detail/simple_hash_table.h | 2 +- .../broadphase/detail/simple_interval-inl.h | 2 +- .../coal/broadphase/detail/simple_interval.h | 6 +- .../broadphase/detail/sparse_hash_table-inl.h | 2 +- .../broadphase/detail/sparse_hash_table.h | 2 +- .../coal/broadphase/detail/spatial_hash-inl.h | 2 +- include/coal/broadphase/detail/spatial_hash.h | 4 +- include/coal/collision.h | 32 +-- include/coal/collision_data.h | 38 +-- include/coal/collision_func_matrix.h | 8 +- include/coal/collision_object.h | 12 +- include/coal/collision_utility.h | 8 +- include/coal/contact_patch.h | 34 +-- .../coal/contact_patch/contact_patch_solver.h | 10 +- .../contact_patch/contact_patch_solver.hxx | 4 +- include/coal/contact_patch_func_matrix.h | 8 +- include/coal/data_types.h | 6 +- include/coal/distance.h | 30 +-- include/coal/distance_func_matrix.h | 8 +- include/coal/fwd.hh | 6 +- include/coal/hfield.h | 16 +- include/coal/internal/BV_fitter.h | 22 +- include/coal/internal/BV_splitter.h | 38 +-- include/coal/internal/intersect.h | 10 +- .../internal/shape_shape_contact_patch_func.h | 10 +- include/coal/internal/shape_shape_func.h | 20 +- include/coal/internal/tools.h | 4 +- include/coal/internal/traversal.h | 4 +- include/coal/internal/traversal_node_base.h | 6 +- .../coal/internal/traversal_node_bvh_hfield.h | 22 +- .../coal/internal/traversal_node_bvh_shape.h | 16 +- include/coal/internal/traversal_node_bvhs.h | 20 +- .../internal/traversal_node_hfield_shape.h | 20 +- include/coal/internal/traversal_node_octree.h | 44 ++-- include/coal/internal/traversal_node_setup.h | 14 +- include/coal/internal/traversal_node_shapes.h | 14 +- include/coal/internal/traversal_recurse.h | 6 +- include/coal/math/matrix_3f.h | 4 +- include/coal/math/transform.h | 8 +- include/coal/math/types.h | 4 +- include/coal/math/vec_3f.h | 4 +- include/coal/mesh_loader/assimp.h | 17 +- include/coal/mesh_loader/loader.h | 16 +- include/coal/narrowphase/gjk.h | 16 +- .../coal/narrowphase/minkowski_difference.h | 8 +- include/coal/narrowphase/narrowphase.h | 12 +- .../coal/narrowphase/narrowphase_defaults.h | 2 +- include/coal/narrowphase/support_functions.h | 12 +- include/coal/octree.h | 10 +- include/coal/serialization/AABB.h | 4 +- include/coal/serialization/BVH_model.h | 16 +- include/coal/serialization/BV_node.h | 6 +- include/coal/serialization/BV_splitter.h | 4 +- include/coal/serialization/OBB.h | 4 +- include/coal/serialization/OBBRSS.h | 8 +- include/coal/serialization/RSS.h | 4 +- include/coal/serialization/archive.h | 2 +- include/coal/serialization/collision_data.h | 4 +- include/coal/serialization/collision_object.h | 6 +- include/coal/serialization/contact_patch.h | 6 +- include/coal/serialization/convex.h | 12 +- include/coal/serialization/fwd.h | 4 +- include/coal/serialization/geometric_shapes.h | 6 +- include/coal/serialization/hfield.h | 6 +- include/coal/serialization/kDOP.h | 4 +- include/coal/serialization/kIOS.h | 6 +- include/coal/serialization/octree.h | 4 +- include/coal/serialization/quadrilateral.h | 4 +- include/coal/serialization/serializer.h | 2 +- include/coal/serialization/transform.h | 4 +- include/coal/serialization/triangle.h | 4 +- include/coal/shape/convex.h | 4 +- .../coal/shape/geometric_shape_to_BVH_model.h | 4 +- include/coal/shape/geometric_shapes.h | 30 +-- include/coal/shape/geometric_shapes_traits.h | 2 +- include/coal/shape/geometric_shapes_utility.h | 225 ++++++++-------- include/coal/timings.h | 4 +- python/broadphase/broadphase.cc | 16 +- python/broadphase/broadphase_callbacks.hh | 4 +- .../broadphase_collision_manager.hh | 6 +- python/broadphase/fwd.hh | 2 +- python/collision-geometries.cc | 28 +- python/collision.cc | 12 +- python/contact_patch.cc | 6 +- python/distance.cc | 13 +- python/fcl.cc | 10 +- python/fwd.hh | 2 +- python/gjk.cc | 4 +- python/math.cc | 6 +- python/octree.cc | 4 +- python/serializable.hh | 4 +- python/version.cc | 20 +- src/BV/AABB.cpp | 6 +- src/BV/OBB.cpp | 12 +- src/BV/OBBRSS.cpp | 2 +- src/BV/RSS.cpp | 8 +- src/BV/kDOP.cpp | 6 +- src/BV/kIOS.cpp | 6 +- src/BVH/BVH_model.cpp | 16 +- src/BVH/BVH_utility.cpp | 8 +- src/BVH/BV_fitter.cpp | 7 +- src/BVH/BV_splitter.cpp | 2 +- src/CMakeLists.txt | 6 +- src/broadphase/broadphase_SSaP.cpp | 4 +- src/broadphase/broadphase_SaP.cpp | 2 +- src/broadphase/broadphase_bruteforce.cpp | 2 +- .../broadphase_collision_manager.cpp | 2 +- .../broadphase_dynamic_AABB_tree.cpp | 12 +- .../broadphase_dynamic_AABB_tree_array.cpp | 6 +- src/broadphase/broadphase_interval_tree.cpp | 2 +- .../default_broadphase_callbacks.cpp | 2 +- src/broadphase/detail/interval_tree.cpp | 2 +- src/broadphase/detail/interval_tree_node.cpp | 2 +- src/broadphase/detail/morton-inl.h | 2 +- src/broadphase/detail/morton.cpp | 2 +- src/broadphase/detail/simple_interval.cpp | 2 +- src/broadphase/detail/spatial_hash.cpp | 2 +- src/collision.cpp | 8 +- src/collision_data.cpp | 2 +- src/collision_func_matrix.cpp | 14 +- src/collision_node.cpp | 2 +- src/collision_node.h | 26 +- src/collision_object.cpp | 2 +- src/collision_utility.cpp | 5 +- src/contact_patch.cpp | 4 +- src/contact_patch/contact_patch_solver.cpp | 2 +- src/contact_patch_func_matrix.cpp | 11 +- src/distance.cpp | 8 +- src/distance/box_halfspace.cpp | 6 +- src/distance/box_plane.cpp | 6 +- src/distance/box_sphere.cpp | 6 +- src/distance/capsule_capsule.cpp | 6 +- src/distance/capsule_halfspace.cpp | 6 +- src/distance/capsule_plane.cpp | 6 +- src/distance/cone_halfspace.cpp | 6 +- src/distance/cone_plane.cpp | 6 +- src/distance/convex_halfspace.cpp | 4 +- src/distance/convex_plane.cpp | 4 +- src/distance/cylinder_halfspace.cpp | 6 +- src/distance/cylinder_plane.cpp | 6 +- src/distance/ellipsoid_halfspace.cpp | 6 +- src/distance/ellipsoid_plane.cpp | 4 +- src/distance/halfspace_halfspace.cpp | 4 +- src/distance/halfspace_plane.cpp | 4 +- src/distance/plane_plane.cpp | 4 +- src/distance/sphere_capsule.cpp | 6 +- src/distance/sphere_cylinder.cpp | 6 +- src/distance/sphere_halfspace.cpp | 6 +- src/distance/sphere_plane.cpp | 6 +- src/distance/sphere_sphere.cpp | 8 +- src/distance/triangle_halfspace.cpp | 4 +- src/distance/triangle_plane.cpp | 4 +- src/distance/triangle_sphere.cpp | 4 +- src/distance/triangle_triangle.cpp | 4 +- src/distance_func_matrix.cpp | 20 +- src/hfield.cpp | 14 +- src/intersect.cpp | 5 +- src/math/transform.cpp | 2 +- src/mesh_loader/assimp.cpp | 2 +- src/mesh_loader/loader.cpp | 8 +- src/narrowphase/details.h | 4 +- src/narrowphase/gjk.cpp | 12 +- src/narrowphase/minkowski_difference.cpp | 20 +- src/narrowphase/support_functions.cpp | 54 ++-- src/octree.cpp | 3 +- src/serialization/serialization.cpp | 16 +- src/shape/convex.cpp | 2 +- src/shape/geometric_shapes.cpp | 4 +- src/shape/geometric_shapes_utility.cpp | 6 +- src/traits_traversal.h | 36 +-- src/traversal/traversal_recurse.cpp | 8 +- test/CMakeLists.txt | 2 +- test/accelerated_gjk.cpp | 6 +- test/benchmark.cpp | 6 +- test/box_box_collision.cpp | 6 +- test/box_box_distance.cpp | 10 +- test/broadphase.cpp | 8 +- test/broadphase_collision_1.cpp | 20 +- test/broadphase_collision_2.cpp | 20 +- test/broadphase_dynamic_AABB_tree.cpp | 12 +- test/bvh_models.cpp | 16 +- test/capsule_box_1.cpp | 10 +- test/capsule_box_2.cpp | 10 +- test/capsule_capsule.cpp | 12 +- test/collision.cpp | 26 +- test/collision_node_asserts.cpp | 4 +- test/contact_patch.cpp | 2 +- test/convex.cpp | 6 +- test/distance.cpp | 6 +- test/distance_lower_bound.cpp | 14 +- test/frontlist.cpp | 6 +- test/geometric_shapes.cpp | 12 +- test/gjk.cpp | 8 +- test/gjk_asserts.cpp | 4 +- test/gjk_convergence_criterion.cpp | 10 +- test/hfields.cpp | 18 +- test/math.cpp | 8 +- test/normal_and_nearest_points.cpp | 12 +- test/obb.cpp | 4 +- test/octree.cpp | 14 +- test/profiling.cpp | 16 +- test/security_margin.cpp | 14 +- test/serialization.cpp | 47 ++-- test/shape_inflation.cpp | 12 +- test/shape_mesh_consistency.cpp | 8 +- test/simple.cpp | 6 +- test/swept_sphere_radius.cpp | 12 +- test/utility.cpp | 16 +- test/utility.h | 12 +- 252 files changed, 1408 insertions(+), 1410 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d5031b66..c2acb4bf7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,9 +35,9 @@ cmake_minimum_required(VERSION 3.10) set(CXX_DISABLE_WERROR TRUE) -set(PROJECT_NAME hpp-fcl) +set(PROJECT_NAME coal) set(PROJECT_DESCRIPTION - "HPP fork of FCL -- The Flexible Collision Library" + "Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library" ) SET(PROJECT_USE_CMAKE_EXPORT TRUE) SET(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion) @@ -188,131 +188,131 @@ endif() FIND_PACKAGE(assimp REQUIRED) SET(${PROJECT_NAME}_HEADERS - include/hpp/fcl/collision_data.h - include/hpp/fcl/BV/kIOS.h - include/hpp/fcl/BV/BV.h - include/hpp/fcl/BV/RSS.h - include/hpp/fcl/BV/OBBRSS.h - include/hpp/fcl/BV/BV_node.h - include/hpp/fcl/BV/AABB.h - include/hpp/fcl/BV/OBB.h - include/hpp/fcl/BV/kDOP.h - include/hpp/fcl/broadphase/broadphase.h - include/hpp/fcl/broadphase/broadphase_SSaP.h - include/hpp/fcl/broadphase/broadphase_SaP.h - include/hpp/fcl/broadphase/broadphase_bruteforce.h - include/hpp/fcl/broadphase/broadphase_collision_manager.h - include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h - include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h - include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h - include/hpp/fcl/broadphase/broadphase_interval_tree.h - include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h - include/hpp/fcl/broadphase/broadphase_spatialhash.h - include/hpp/fcl/broadphase/broadphase_callbacks.h - include/hpp/fcl/broadphase/default_broadphase_callbacks.h - include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h - include/hpp/fcl/broadphase/detail/hierarchy_tree.h - include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h - include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h - include/hpp/fcl/broadphase/detail/interval_tree.h - include/hpp/fcl/broadphase/detail/interval_tree_node.h - include/hpp/fcl/broadphase/detail/morton-inl.h - include/hpp/fcl/broadphase/detail/morton.h - include/hpp/fcl/broadphase/detail/node_base-inl.h - include/hpp/fcl/broadphase/detail/node_base.h - include/hpp/fcl/broadphase/detail/node_base_array-inl.h - include/hpp/fcl/broadphase/detail/node_base_array.h - include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h - include/hpp/fcl/broadphase/detail/simple_hash_table.h - include/hpp/fcl/broadphase/detail/simple_interval-inl.h - include/hpp/fcl/broadphase/detail/simple_interval.h - include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h - include/hpp/fcl/broadphase/detail/sparse_hash_table.h - include/hpp/fcl/broadphase/detail/spatial_hash-inl.h - include/hpp/fcl/broadphase/detail/spatial_hash.h - include/hpp/fcl/narrowphase/narrowphase.h - include/hpp/fcl/narrowphase/gjk.h - include/hpp/fcl/narrowphase/narrowphase_defaults.h - include/hpp/fcl/narrowphase/minkowski_difference.h - include/hpp/fcl/narrowphase/support_functions.h - include/hpp/fcl/shape/convex.h - include/hpp/fcl/shape/details/convex.hxx - include/hpp/fcl/shape/geometric_shape_to_BVH_model.h - include/hpp/fcl/shape/geometric_shapes.h - include/hpp/fcl/shape/geometric_shapes_traits.h - include/hpp/fcl/shape/geometric_shapes_utility.h - include/hpp/fcl/distance_func_matrix.h - include/hpp/fcl/collision.h - include/hpp/fcl/collision_func_matrix.h - include/hpp/fcl/contact_patch.h - include/hpp/fcl/contact_patch_func_matrix.h - include/hpp/fcl/contact_patch/contact_patch_solver.h - include/hpp/fcl/contact_patch/contact_patch_solver.hxx - include/hpp/fcl/distance.h - include/hpp/fcl/math/matrix_3f.h - include/hpp/fcl/math/vec_3f.h - include/hpp/fcl/math/types.h - include/hpp/fcl/math/transform.h - include/hpp/fcl/data_types.h - include/hpp/fcl/BVH/BVH_internal.h - include/hpp/fcl/BVH/BVH_model.h - include/hpp/fcl/BVH/BVH_front.h - include/hpp/fcl/BVH/BVH_utility.h - include/hpp/fcl/collision_object.h - include/hpp/fcl/collision_utility.h - include/hpp/fcl/hfield.h - include/hpp/fcl/fwd.hh - include/hpp/fcl/logging.h - include/hpp/fcl/mesh_loader/assimp.h - include/hpp/fcl/mesh_loader/loader.h - include/hpp/fcl/internal/BV_fitter.h - include/hpp/fcl/internal/BV_splitter.h - include/hpp/fcl/internal/shape_shape_func.h - include/hpp/fcl/internal/shape_shape_contact_patch_func.h - include/hpp/fcl/internal/intersect.h - include/hpp/fcl/internal/tools.h - include/hpp/fcl/internal/traversal_node_base.h - include/hpp/fcl/internal/traversal_node_bvh_shape.h - include/hpp/fcl/internal/traversal_node_bvhs.h - include/hpp/fcl/internal/traversal_node_hfield_shape.h - include/hpp/fcl/internal/traversal_node_setup.h - include/hpp/fcl/internal/traversal_node_shapes.h - include/hpp/fcl/internal/traversal_recurse.h - include/hpp/fcl/internal/traversal.h - include/hpp/fcl/serialization/fwd.h - include/hpp/fcl/serialization/serializer.h - include/hpp/fcl/serialization/archive.h - include/hpp/fcl/serialization/transform.h - include/hpp/fcl/serialization/AABB.h - include/hpp/fcl/serialization/BV_node.h - include/hpp/fcl/serialization/BV_splitter.h - include/hpp/fcl/serialization/BVH_model.h - include/hpp/fcl/serialization/collision_data.h - include/hpp/fcl/serialization/contact_patch.h - include/hpp/fcl/serialization/collision_object.h - include/hpp/fcl/serialization/convex.h - include/hpp/fcl/serialization/eigen.h - include/hpp/fcl/serialization/geometric_shapes.h - include/hpp/fcl/serialization/memory.h - include/hpp/fcl/serialization/OBB.h - include/hpp/fcl/serialization/RSS.h - include/hpp/fcl/serialization/OBBRSS.h - include/hpp/fcl/serialization/kIOS.h - include/hpp/fcl/serialization/kDOP.h - include/hpp/fcl/serialization/hfield.h - include/hpp/fcl/serialization/quadrilateral.h - include/hpp/fcl/serialization/triangle.h - include/hpp/fcl/timings.h + include/coal/collision_data.h + include/coal/BV/kIOS.h + include/coal/BV/BV.h + include/coal/BV/RSS.h + include/coal/BV/OBBRSS.h + include/coal/BV/BV_node.h + include/coal/BV/AABB.h + include/coal/BV/OBB.h + include/coal/BV/kDOP.h + include/coal/broadphase/broadphase.h + include/coal/broadphase/broadphase_SSaP.h + include/coal/broadphase/broadphase_SaP.h + include/coal/broadphase/broadphase_bruteforce.h + include/coal/broadphase/broadphase_collision_manager.h + include/coal/broadphase/broadphase_continuous_collision_manager-inl.h + include/coal/broadphase/broadphase_continuous_collision_manager.h + include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h + include/coal/broadphase/broadphase_dynamic_AABB_tree.h + include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h + include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h + include/coal/broadphase/broadphase_interval_tree.h + include/coal/broadphase/broadphase_spatialhash-inl.h + include/coal/broadphase/broadphase_spatialhash.h + include/coal/broadphase/broadphase_callbacks.h + include/coal/broadphase/default_broadphase_callbacks.h + include/coal/broadphase/detail/hierarchy_tree-inl.h + include/coal/broadphase/detail/hierarchy_tree.h + include/coal/broadphase/detail/hierarchy_tree_array-inl.h + include/coal/broadphase/detail/hierarchy_tree_array.h + include/coal/broadphase/detail/interval_tree.h + include/coal/broadphase/detail/interval_tree_node.h + include/coal/broadphase/detail/morton-inl.h + include/coal/broadphase/detail/morton.h + include/coal/broadphase/detail/node_base-inl.h + include/coal/broadphase/detail/node_base.h + include/coal/broadphase/detail/node_base_array-inl.h + include/coal/broadphase/detail/node_base_array.h + include/coal/broadphase/detail/simple_hash_table-inl.h + include/coal/broadphase/detail/simple_hash_table.h + include/coal/broadphase/detail/simple_interval-inl.h + include/coal/broadphase/detail/simple_interval.h + include/coal/broadphase/detail/sparse_hash_table-inl.h + include/coal/broadphase/detail/sparse_hash_table.h + include/coal/broadphase/detail/spatial_hash-inl.h + include/coal/broadphase/detail/spatial_hash.h + include/coal/narrowphase/narrowphase.h + include/coal/narrowphase/gjk.h + include/coal/narrowphase/narrowphase_defaults.h + include/coal/narrowphase/minkowski_difference.h + include/coal/narrowphase/support_functions.h + include/coal/shape/convex.h + include/coal/shape/details/convex.hxx + include/coal/shape/geometric_shape_to_BVH_model.h + include/coal/shape/geometric_shapes.h + include/coal/shape/geometric_shapes_traits.h + include/coal/shape/geometric_shapes_utility.h + include/coal/distance_func_matrix.h + include/coal/collision.h + include/coal/collision_func_matrix.h + include/coal/contact_patch.h + include/coal/contact_patch_func_matrix.h + include/coal/contact_patch/contact_patch_solver.h + include/coal/contact_patch/contact_patch_solver.hxx + include/coal/distance.h + include/coal/math/matrix_3f.h + include/coal/math/vec_3f.h + include/coal/math/types.h + include/coal/math/transform.h + include/coal/data_types.h + include/coal/BVH/BVH_internal.h + include/coal/BVH/BVH_model.h + include/coal/BVH/BVH_front.h + include/coal/BVH/BVH_utility.h + include/coal/collision_object.h + include/coal/collision_utility.h + include/coal/hfield.h + include/coal/fwd.hh + include/coal/logging.h + include/coal/mesh_loader/assimp.h + include/coal/mesh_loader/loader.h + include/coal/internal/BV_fitter.h + include/coal/internal/BV_splitter.h + include/coal/internal/shape_shape_func.h + include/coal/internal/shape_shape_contact_patch_func.h + include/coal/internal/intersect.h + include/coal/internal/tools.h + include/coal/internal/traversal_node_base.h + include/coal/internal/traversal_node_bvh_shape.h + include/coal/internal/traversal_node_bvhs.h + include/coal/internal/traversal_node_hfield_shape.h + include/coal/internal/traversal_node_setup.h + include/coal/internal/traversal_node_shapes.h + include/coal/internal/traversal_recurse.h + include/coal/internal/traversal.h + include/coal/serialization/fwd.h + include/coal/serialization/serializer.h + include/coal/serialization/archive.h + include/coal/serialization/transform.h + include/coal/serialization/AABB.h + include/coal/serialization/BV_node.h + include/coal/serialization/BV_splitter.h + include/coal/serialization/BVH_model.h + include/coal/serialization/collision_data.h + include/coal/serialization/contact_patch.h + include/coal/serialization/collision_object.h + include/coal/serialization/convex.h + include/coal/serialization/eigen.h + include/coal/serialization/geometric_shapes.h + include/coal/serialization/memory.h + include/coal/serialization/OBB.h + include/coal/serialization/RSS.h + include/coal/serialization/OBBRSS.h + include/coal/serialization/kIOS.h + include/coal/serialization/kDOP.h + include/coal/serialization/hfield.h + include/coal/serialization/quadrilateral.h + include/coal/serialization/triangle.h + include/coal/timings.h ) IF(HPP_FCL_HAS_OCTOMAP) LIST(APPEND ${PROJECT_NAME}_HEADERS - include/hpp/fcl/octree.h - include/hpp/fcl/serialization/octree.h - include/hpp/fcl/internal/traversal_node_octree.h + include/coal/octree.h + include/coal/serialization/octree.h + include/coal/internal/traversal_node_octree.h ) ENDIF(HPP_FCL_HAS_OCTOMAP) @@ -325,7 +325,7 @@ if(BUILD_TESTING) add_subdirectory(test) endif(BUILD_TESTING) -pkg_config_append_libs("hpp-fcl") +pkg_config_append_libs("coal") IF(HPP_FCL_HAS_OCTOMAP) # FCL_HAVE_OCTOMAP kept for backward compatibility reasons. PKG_CONFIG_APPEND_CFLAGS( diff --git a/include/coal/BV/AABB.h b/include/coal/BV/AABB.h index 4f228416f..c0aa81b67 100644 --- a/include/coal/BV/AABB.h +++ b/include/coal/BV/AABB.h @@ -38,7 +38,7 @@ #ifndef COAL_AABB_H #define COAL_AABB_H -#include "hpp/fcl/data_types.h" +#include "coal/data_types.h" namespace coal { @@ -52,7 +52,7 @@ class Halfspace; /// @brief A class describing the AABB collision structure, which is a box in 3D /// space determined by two diagonal points -class HPP_FCL_DLLAPI AABB { +class COAL_DLLAPI AABB { public: /// @brief The min point in the AABB Vec3f min_; @@ -253,14 +253,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& R) { /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) /// and b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, - const AABB& b2); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, + const AABB& b2); /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) /// and b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, - const AABB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, + const AABB& b2, const CollisionRequest& request, + FCL_REAL& sqrDistLowerBound); } // namespace coal #endif diff --git a/include/coal/BV/BV.h b/include/coal/BV/BV.h index 1dad8f132..7cca20489 100644 --- a/include/coal/BV/BV.h +++ b/include/coal/BV/BV.h @@ -38,13 +38,13 @@ #ifndef COAL_BV_H #define COAL_BV_H -#include -#include -#include -#include -#include -#include -#include +#include "coal/BV/kDOP.h" +#include "coal/BV/AABB.h" +#include "coal/BV/OBB.h" +#include "coal/BV/RSS.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BV/kIOS.h" +#include "coal/math/transform.h" /** @brief Main namespace */ namespace coal { diff --git a/include/coal/BV/BV_node.h b/include/coal/BV/BV_node.h index 86cab1036..9e7adfad6 100644 --- a/include/coal/BV/BV_node.h +++ b/include/coal/BV/BV_node.h @@ -38,8 +38,8 @@ #ifndef COAL_BV_NODE_H #define COAL_BV_NODE_H -#include -#include +#include "coal/data_types.h" +#include "coal/BV/BV.h" namespace coal { @@ -48,7 +48,7 @@ namespace coal { /// @{ /// @brief BVNodeBase encodes the tree structure for BVH -struct HPP_FCL_DLLAPI BVNodeBase { +struct COAL_DLLAPI BVNodeBase { /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node /// If the value is negative, it is -(primitive index + 1) @@ -103,7 +103,7 @@ struct HPP_FCL_DLLAPI BVNodeBase { /// structure providing in BVNodeBase and also the geometry data provided in BV /// template parameter. template -struct HPP_FCL_DLLAPI BVNode : public BVNodeBase { +struct COAL_DLLAPI BVNode : public BVNodeBase { typedef BVNodeBase Base; /// @brief bounding volume storing the geometry diff --git a/include/coal/BV/OBB.h b/include/coal/BV/OBB.h index b47bef7d3..d9bd0c503 100644 --- a/include/coal/BV/OBB.h +++ b/include/coal/BV/OBB.h @@ -38,7 +38,7 @@ #ifndef COAL_OBB_H #define COAL_OBB_H -#include "hpp/fcl/data_types.h" +#include "coal/data_types.h" namespace coal { @@ -48,7 +48,7 @@ struct CollisionRequest; /// @{ /// @brief Oriented bounding box class -struct HPP_FCL_DLLAPI OBB { +struct COAL_DLLAPI OBB { EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @brief Orientation of OBB. axis[i] is the ith column of the orientation @@ -125,26 +125,26 @@ struct HPP_FCL_DLLAPI OBB { }; /// @brief Translate the OBB bv -HPP_FCL_DLLAPI OBB translate(const OBB& bv, const Vec3f& t); +COAL_DLLAPI OBB translate(const OBB& bv, const Vec3f& t); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and /// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, - const OBB& b2); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, + const OBB& b2); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and /// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, - const OBB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, + const OBB& b2, const CollisionRequest& request, + FCL_REAL& sqrDistLowerBound); /// Check collision between two boxes /// @param B, T orientation and position of first box, /// @param a half dimensions of first box, /// @param b half dimensions of second box. /// The second box is in identity configuration. -HPP_FCL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b); +COAL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, + const Vec3f& b); } // namespace coal #endif diff --git a/include/coal/BV/OBBRSS.h b/include/coal/BV/OBBRSS.h index 5529ec95b..5e1da89fa 100644 --- a/include/coal/BV/OBBRSS.h +++ b/include/coal/BV/OBBRSS.h @@ -38,8 +38,8 @@ #ifndef COAL_OBBRSS_H #define COAL_OBBRSS_H -#include "hpp/fcl/BV/OBB.h" -#include "hpp/fcl/BV/RSS.h" +#include "coal/BV/OBB.h" +#include "coal/BV/RSS.h" namespace coal { @@ -50,7 +50,7 @@ struct CollisionRequest; /// @brief Class merging the OBB and RSS, can handle collision and distance /// simultaneously -struct HPP_FCL_DLLAPI OBBRSS { +struct COAL_DLLAPI OBBRSS { EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @brief OBB member, for rotation diff --git a/include/coal/BV/RSS.h b/include/coal/BV/RSS.h index 2250dac5b..e0916fe29 100644 --- a/include/coal/BV/RSS.h +++ b/include/coal/BV/RSS.h @@ -38,7 +38,7 @@ #ifndef COAL_RSS_H #define COAL_RSS_H -#include "hpp/fcl/data_types.h" +#include "coal/data_types.h" #include @@ -50,7 +50,7 @@ struct CollisionRequest; /// @{ /// @brief A class for rectangle sphere-swept bounding volume -struct HPP_FCL_DLLAPI RSS { +struct COAL_DLLAPI RSS { EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// @brief Orientation of RSS. axis[i] is the ith column of the orientation @@ -153,20 +153,20 @@ struct HPP_FCL_DLLAPI RSS { /// not the RSS. But the direction P - Q is the correct direction for cloest /// points Notice that P and Q are both in the local frame of the first RSS (not /// global frame and not even the local frame of object 1) -HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, - const RSS& b1, const RSS& b2, Vec3f* P = NULL, - Vec3f* Q = NULL); +COAL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, + const RSS& b1, const RSS& b2, Vec3f* P = NULL, + Vec3f* Q = NULL); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, - const RSS& b2); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, + const RSS& b2); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, - const RSS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, + const RSS& b2, const CollisionRequest& request, + FCL_REAL& sqrDistLowerBound); } // namespace coal diff --git a/include/coal/BV/kDOP.h b/include/coal/BV/kDOP.h index 7186a9213..5a68e15ad 100644 --- a/include/coal/BV/kDOP.h +++ b/include/coal/BV/kDOP.h @@ -38,8 +38,8 @@ #ifndef COAL_KDOP_H #define COAL_KDOP_H -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" +#include "coal/fwd.hh" +#include "coal/data_types.h" namespace coal { @@ -88,7 +88,7 @@ struct CollisionRequest; /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 template -class HPP_FCL_DLLAPI KDOP { +class COAL_DLLAPI KDOP { protected: /// @brief Origin's distances to N KDOP planes Eigen::Array dist_; @@ -183,7 +183,7 @@ bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, /// @brief translate the KDOP BV template -HPP_FCL_DLLAPI KDOP translate(const KDOP& bv, const Vec3f& t); +COAL_DLLAPI KDOP translate(const KDOP& bv, const Vec3f& t); } // namespace coal diff --git a/include/coal/BV/kIOS.h b/include/coal/BV/kIOS.h index e96608d6e..36eaee616 100644 --- a/include/coal/BV/kIOS.h +++ b/include/coal/BV/kIOS.h @@ -38,7 +38,7 @@ #ifndef COAL_KIOS_H #define COAL_KIOS_H -#include "hpp/fcl/BV/OBB.h" +#include "coal/BV/OBB.h" namespace coal { @@ -49,9 +49,9 @@ struct CollisionRequest; /// @brief A class describing the kIOS collision structure, which is a set of /// spheres. -class HPP_FCL_DLLAPI kIOS { +class COAL_DLLAPI kIOS { /// @brief One sphere in kIOS - struct HPP_FCL_DLLAPI kIOS_Sphere { + struct COAL_DLLAPI kIOS_Sphere { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3f o; @@ -166,26 +166,26 @@ class HPP_FCL_DLLAPI kIOS { }; /// @brief Translate the kIOS BV -HPP_FCL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t); +COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t); /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) /// and b2 is in identity. /// @todo Not efficient -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, - const kIOS& b2); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2); /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) /// and b2 is in identity. /// @todo Not efficient -HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, - const kIOS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); +COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2, const CollisionRequest& request, + FCL_REAL& sqrDistLowerBound); /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation -HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, - const kIOS& b1, const kIOS& b2, - Vec3f* P = NULL, Vec3f* Q = NULL); +COAL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, + const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, + Vec3f* Q = NULL); } // namespace coal diff --git a/include/coal/BVH/BVH_front.h b/include/coal/BVH/BVH_front.h index 3ec1522d1..fe471b9b3 100644 --- a/include/coal/BVH/BVH_front.h +++ b/include/coal/BVH/BVH_front.h @@ -40,7 +40,7 @@ #include -#include +#include "coal/config.hh" namespace coal { @@ -49,7 +49,7 @@ namespace coal { /// the traversal terminates while performing a query during a given time /// instance. The front list reflects the subset of a BVTT that is traversed for /// that particular proximity query. -struct HPP_FCL_DLLAPI BVHFrontNode { +struct COAL_DLLAPI BVHFrontNode { /// @brief The nodes to start in the future, i.e. the wave front of the /// traversal tree. unsigned int left, right; diff --git a/include/coal/BVH/BVH_internal.h b/include/coal/BVH/BVH_internal.h index 892911959..49884639a 100644 --- a/include/coal/BVH/BVH_internal.h +++ b/include/coal/BVH/BVH_internal.h @@ -38,7 +38,7 @@ #ifndef COAL_BVH_INTERNAL_H #define COAL_BVH_INTERNAL_H -#include +#include "coal/data_types.h" namespace coal { diff --git a/include/coal/BVH/BVH_model.h b/include/coal/BVH/BVH_model.h index 16ae11e67..71b830188 100644 --- a/include/coal/BVH/BVH_model.h +++ b/include/coal/BVH/BVH_model.h @@ -39,10 +39,10 @@ #ifndef COAL_BVH_MODEL_H #define COAL_BVH_MODEL_H -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/BVH/BVH_internal.h" -#include "hpp/fcl/BV/BV_node.h" +#include "coal/fwd.hh" +#include "coal/collision_object.h" +#include "coal/BVH/BVH_internal.h" +#include "coal/BV/BV_node.h" #include #include @@ -62,7 +62,7 @@ class BVSplitter; /// @brief A base class describing the bounding hierarchy of a mesh model or a /// point cloud model (which is viewed as a degraded version of mesh) -class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry { +class COAL_DLLAPI BVHModelBase : public CollisionGeometry { public: /// @brief Geometry point data std::shared_ptr> vertices; @@ -311,7 +311,7 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry { /// cloud model (which is viewed as a degraded version of mesh) \tparam BV one /// of the bounding volume class in \ref Bounding_Volume. template -class HPP_FCL_DLLAPI BVHModel : public BVHModelBase { +class COAL_DLLAPI BVHModel : public BVHModelBase { typedef BVHModelBase Base; public: diff --git a/include/coal/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h index 27093fb37..21434736c 100644 --- a/include/coal/BVH/BVH_utility.h +++ b/include/coal/BVH/BVH_utility.h @@ -38,78 +38,77 @@ #ifndef COAL_BVH_UTILITY_H #define COAL_BVH_UTILITY_H -#include +#include "coal/BVH/BVH_model.h" namespace coal { /// @brief Extract the part of the BVHModel that is inside an AABB. /// A triangle in collision with the AABB is considered inside. template -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, const AABB& aabb); template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, + const Transform3f& pose, + const AABB& aabb); +template <> +COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& aabb); template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, +COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, const Transform3f& pose, const AABB& aabb); template <> -HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, - const AABB& aabb); -template <> -HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, - const AABB& aabb); +COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, + const AABB& aabb); template <> -HPP_FCL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, - const AABB& aabb); +COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, + const Transform3f& pose, + const AABB& aabb); /// @brief Compute the covariance matrix for a set or subset of points. if ts = /// null, then indices refer to points directly; otherwise refer to triangles -HPP_FCL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - Matrix3f& M); +COAL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + Matrix3f& M); /// @brief Compute the RSS bounding volume parameters: radius, rectangle size /// and the origin, given the BV axises. -HPP_FCL_DLLAPI void getRadiusAndOriginAndRectangleSize( +COAL_DLLAPI void getRadiusAndOriginAndRectangleSize( Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r); /// @brief Compute the bounding volume extent and center for a set or subset of /// points, given the BV axises. -HPP_FCL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - Matrix3f& axes, Vec3f& center, - Vec3f& extent); +COAL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + Matrix3f& axes, Vec3f& center, + Vec3f& extent); /// @brief Compute the center and radius for a triangle's circumcircle -HPP_FCL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b, - const Vec3f& c, Vec3f& center, - FCL_REAL& radius); +COAL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b, + const Vec3f& c, Vec3f& center, + FCL_REAL& radius); /// @brief Compute the maximum distance from a given center point to a point /// cloud -HPP_FCL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - const Vec3f& query); +COAL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Vec3f& query); } // namespace coal diff --git a/include/coal/broadphase/broadphase.h b/include/coal/broadphase/broadphase.h index fafd6f954..887dbb141 100644 --- a/include/coal/broadphase/broadphase.h +++ b/include/coal/broadphase/broadphase.h @@ -35,14 +35,14 @@ #ifndef COAL_BROADPHASE_BROADPHASE_H #define COAL_BROADPHASE_BROADPHASE_H -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_spatialhash.h" -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" +#include "coal/broadphase/default_broadphase_callbacks.h" #endif // ifndef COAL_BROADPHASE_BROADPHASE_H diff --git a/include/coal/broadphase/broadphase_SSaP.h b/include/coal/broadphase/broadphase_SSaP.h index d3511c5f0..fdb46cb21 100644 --- a/include/coal/broadphase/broadphase_SSaP.h +++ b/include/coal/broadphase/broadphase_SSaP.h @@ -39,12 +39,12 @@ #define COAL_BROAD_PHASE_SSAP_H #include -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/broadphase_collision_manager.h" namespace coal { /// @brief Simple SAP collision manager -class HPP_FCL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { +class COAL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; diff --git a/include/coal/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h index 8ae3f48f3..209ad6af3 100644 --- a/include/coal/broadphase/broadphase_SaP.h +++ b/include/coal/broadphase/broadphase_SaP.h @@ -41,12 +41,12 @@ #include #include -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/broadphase_collision_manager.h" namespace coal { /// @brief Rigorous SAP collision manager -class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { +class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; @@ -168,7 +168,7 @@ class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { }; /// @brief Functor to help unregister one object - class HPP_FCL_DLLAPI isUnregistered { + class COAL_DLLAPI isUnregistered { CollisionObject* obj; public: @@ -179,7 +179,7 @@ class HPP_FCL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { /// @brief Functor to help remove collision pairs no longer valid (i.e., /// should be culled away) - class HPP_FCL_DLLAPI isNotValidPair { + class COAL_DLLAPI isNotValidPair { CollisionObject* obj1; CollisionObject* obj2; diff --git a/include/coal/broadphase/broadphase_bruteforce.h b/include/coal/broadphase/broadphase_bruteforce.h index ac5dce5ee..ab1f9d564 100644 --- a/include/coal/broadphase/broadphase_bruteforce.h +++ b/include/coal/broadphase/broadphase_bruteforce.h @@ -39,12 +39,12 @@ #define COAL_BROAD_PHASE_BRUTE_FORCE_H #include -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/broadphase_collision_manager.h" namespace coal { /// @brief Brute force N-body collision manager -class HPP_FCL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { +class COAL_DLLAPI NaiveCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; using Base::getObjects; diff --git a/include/coal/broadphase/broadphase_callbacks.h b/include/coal/broadphase/broadphase_callbacks.h index d63318dbf..56fe4f1cd 100644 --- a/include/coal/broadphase/broadphase_callbacks.h +++ b/include/coal/broadphase/broadphase_callbacks.h @@ -37,8 +37,8 @@ #ifndef COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H #define COAL_BROADPHASE_BROAD_PHASE_CALLBACKS_H -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" +#include "coal/fwd.hh" +#include "coal/data_types.h" namespace coal { @@ -46,7 +46,7 @@ namespace coal { /// This class can be supersed by child classes to provide desired behaviors /// according to the application (e.g, only listing the potential /// CollisionObjects in collision). -struct HPP_FCL_DLLAPI CollisionCallBackBase { +struct COAL_DLLAPI CollisionCallBackBase { /// @brief Initialization of the callback before running the collision /// broadphase manager. virtual void init() {}; @@ -69,7 +69,7 @@ struct HPP_FCL_DLLAPI CollisionCallBackBase { /// This class can be supersed by child classes to provide desired behaviors /// according to the application (e.g, only listing the potential /// CollisionObjects in collision). -struct HPP_FCL_DLLAPI DistanceCallBackBase { +struct COAL_DLLAPI DistanceCallBackBase { /// @brief Initialization of the callback before running the collision /// broadphase manager. virtual void init() {}; diff --git a/include/coal/broadphase/broadphase_collision_manager.h b/include/coal/broadphase/broadphase_collision_manager.h index 76ad5d4b1..a394878af 100644 --- a/include/coal/broadphase/broadphase_collision_manager.h +++ b/include/coal/broadphase/broadphase_collision_manager.h @@ -42,15 +42,15 @@ #include #include -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/broadphase/broadphase_callbacks.h" +#include "coal/collision_object.h" +#include "coal/broadphase/broadphase_callbacks.h" namespace coal { /// @brief Base class for broad phase collision. It helps to accelerate the /// collision/distance between N objects. Also support self collision, self /// distance and collision/distance with another M objects. -class HPP_FCL_DLLAPI BroadPhaseCollisionManager { +class COAL_DLLAPI BroadPhaseCollisionManager { public: BroadPhaseCollisionManager(); diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h index 93c038aee..441a615bc 100644 --- a/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h +++ b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H #define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_INL_H -#include "hpp/fcl/broadphase/broadphase_continuous_collision_manager.h" +#include "coal/broadphase/broadphase_continuous_collision_manager.h" #include namespace coal { diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager.h b/include/coal/broadphase/broadphase_continuous_collision_manager.h index 4dd102db5..c2cec955e 100644 --- a/include/coal/broadphase/broadphase_continuous_collision_manager.h +++ b/include/coal/broadphase/broadphase_continuous_collision_manager.h @@ -38,9 +38,9 @@ #ifndef COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H #define COAL_BROADPHASE_BROADPHASECONTINUOUSCOLLISIONMANAGER_H -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/narrowphase/continuous_collision_object.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/collision_object.h" +#include "coal/narrowphase/continuous_collision_object.h" namespace coal { @@ -62,7 +62,7 @@ using ContinuousDistanceCallBack = bool (*)(ContinuousCollisionObject* o1, /// accelerate the continuous collision/distance between N objects. Also support /// self collision, self distance and collision/distance with another M objects. template -class HPP_FCL_DLLAPI BroadPhaseContinuousCollisionManager { +class COAL_DLLAPI BroadPhaseContinuousCollisionManager { public: BroadPhaseContinuousCollisionManager(); @@ -138,6 +138,6 @@ using BroadPhaseContinuousCollisionManagerd = } // namespace coal -#include "hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h" +#include "coal/broadphase/broadphase_continuous_collision_manager-inl.h" #endif diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h index 45a114c79..924b505ef 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -38,16 +38,16 @@ #ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_INL_H -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #include #if HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" +#include "coal/octree.h" #endif -#include "hpp/fcl/BV/BV.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes_utility.h" namespace coal { namespace detail { diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree.h index deabdf4bd..d3ecd5497 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree.h @@ -41,16 +41,16 @@ #include #include -#include "hpp/fcl/fwd.hh" -// #include "hpp/fcl/BV/utility.h" -#include "hpp/fcl/shape/geometric_shapes.h" -// #include "hpp/fcl/geometry/shape/utility.h" -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/hierarchy_tree.h" +#include "coal/fwd.hh" +// #include "coal/BV/utility.h" +#include "coal/shape/geometric_shapes.h" +// #include "coal/geometry/shape/utility.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/hierarchy_tree.h" namespace coal { -class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager +class COAL_DLLAPI DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; @@ -145,6 +145,6 @@ class HPP_FCL_DLLAPI DynamicAABBTreeCollisionManager } // namespace coal -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree-inl.h" #endif diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index b12283592..70327aa92 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -38,11 +38,11 @@ #ifndef COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H #define COAL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/shape/geometric_shapes_utility.h" #if HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" +#include "coal/octree.h" #endif namespace coal { diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h index cb1c24a91..35f17cd82 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -42,16 +42,16 @@ #include #include -#include "hpp/fcl/fwd.hh" -// #include "hpp/fcl/BV/utility.h" -#include "hpp/fcl/shape/geometric_shapes.h" -// #include "hpp/fcl/geometry/shape/utility.h" -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" +#include "coal/fwd.hh" +// #include "coal/BV/utility.h" +#include "coal/shape/geometric_shapes.h" +// #include "coal/geometry/shape/utility.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/hierarchy_tree_array.h" namespace coal { -class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager +class COAL_DLLAPI DynamicAABBTreeArrayCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; @@ -141,6 +141,6 @@ class HPP_FCL_DLLAPI DynamicAABBTreeArrayCollisionManager } // namespace coal -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" #endif diff --git a/include/coal/broadphase/broadphase_interval_tree.h b/include/coal/broadphase/broadphase_interval_tree.h index 97a1db7b8..a97799c29 100644 --- a/include/coal/broadphase/broadphase_interval_tree.h +++ b/include/coal/broadphase/broadphase_interval_tree.h @@ -41,13 +41,13 @@ #include #include -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/interval_tree.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/interval_tree.h" namespace coal { /// @brief Collision manager based on interval tree -class HPP_FCL_DLLAPI IntervalTreeCollisionManager +class COAL_DLLAPI IntervalTreeCollisionManager : public BroadPhaseCollisionManager { public: typedef BroadPhaseCollisionManager Base; @@ -114,7 +114,7 @@ class HPP_FCL_DLLAPI IntervalTreeCollisionManager protected: /// @brief SAP end point /// @brief SAP end point - struct HPP_FCL_DLLAPI EndPoint { + struct COAL_DLLAPI EndPoint { /// @brief object related with the end point CollisionObject* obj; @@ -130,7 +130,7 @@ class HPP_FCL_DLLAPI IntervalTreeCollisionManager /// @brief Extention interval tree's interval to SAP interval, adding more /// information - struct HPP_FCL_DLLAPI SAPInterval : public detail::SimpleInterval { + struct COAL_DLLAPI SAPInterval : public detail::SimpleInterval { CollisionObject* obj; SAPInterval(FCL_REAL low_, FCL_REAL high_, CollisionObject* obj_); diff --git a/include/coal/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h index 5b0eb7df9..fd4ffa0bb 100644 --- a/include/coal/broadphase/broadphase_spatialhash-inl.h +++ b/include/coal/broadphase/broadphase_spatialhash-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H #define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_spatialhash.h" namespace coal { diff --git a/include/coal/broadphase/broadphase_spatialhash.h b/include/coal/broadphase/broadphase_spatialhash.h index 5e5b18920..17f37e052 100644 --- a/include/coal/broadphase/broadphase_spatialhash.h +++ b/include/coal/broadphase/broadphase_spatialhash.h @@ -40,11 +40,11 @@ #include #include -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" -#include "hpp/fcl/broadphase/detail/simple_hash_table.h" -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/BV/AABB.h" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/detail/simple_hash_table.h" +#include "coal/broadphase/detail/sparse_hash_table.h" +#include "coal/broadphase/detail/spatial_hash.h" namespace coal { @@ -162,6 +162,6 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { } // namespace coal -#include "hpp/fcl/broadphase/broadphase_spatialhash-inl.h" +#include "coal/broadphase/broadphase_spatialhash-inl.h" #endif diff --git a/include/coal/broadphase/default_broadphase_callbacks.h b/include/coal/broadphase/default_broadphase_callbacks.h index 09bf73eeb..a124cd8d0 100644 --- a/include/coal/broadphase/default_broadphase_callbacks.h +++ b/include/coal/broadphase/default_broadphase_callbacks.h @@ -39,14 +39,14 @@ #ifndef COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H #define COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H -#include "hpp/fcl/broadphase/broadphase_callbacks.h" -#include "hpp/fcl/collision.h" -#include "hpp/fcl/distance.h" -// #include "hpp/fcl/narrowphase/continuous_collision.h" -// #include "hpp/fcl/narrowphase/continuous_collision_request.h" -// #include "hpp/fcl/narrowphase/continuous_collision_result.h" -// #include "hpp/fcl/narrowphase/distance_request.h" -// #include "hpp/fcl/narrowphase/distance_result.h" +#include "coal/broadphase/broadphase_callbacks.h" +#include "coal/collision.h" +#include "coal/distance.h" +// #include "coal/narrowphase/continuous_collision.h" +// #include "coal/narrowphase/continuous_collision_request.h" +// #include "coal/narrowphase/continuous_collision_result.h" +// #include "coal/narrowphase/distance_request.h" +// #include "coal/narrowphase/distance_result.h" namespace coal { @@ -193,7 +193,7 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, /// @brief Default collision callback to check collision between collision /// objects. -struct HPP_FCL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { +struct COAL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { /// @brief Initialize the callback. /// Clears the collision result and sets the done boolean to false. void init() { data.clear(); } @@ -207,7 +207,7 @@ struct HPP_FCL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase { /// @brief Default distance callback to check collision between collision /// objects. -struct HPP_FCL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { +struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { /// @brief Initialize the callback. /// Clears the distance result and sets the done boolean to false. void init() { data.clear(); } @@ -220,7 +220,7 @@ struct HPP_FCL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { }; /// @brief Collision callback to collect collision pairs potentially in contacts -struct HPP_FCL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase { +struct COAL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase { typedef std::pair CollisionPair; /// @brief Default constructor. diff --git a/include/coal/broadphase/detail/hierarchy_tree-inl.h b/include/coal/broadphase/detail/hierarchy_tree-inl.h index 2b3119e35..85d0c8899 100644 --- a/include/coal/broadphase/detail/hierarchy_tree-inl.h +++ b/include/coal/broadphase/detail/hierarchy_tree-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_HIERARCHY_TREE_INL_H #define COAL_HIERARCHY_TREE_INL_H -#include "hpp/fcl/broadphase/detail/hierarchy_tree.h" +#include "coal/broadphase/detail/hierarchy_tree.h" namespace coal { diff --git a/include/coal/broadphase/detail/hierarchy_tree.h b/include/coal/broadphase/detail/hierarchy_tree.h index baaeb7b3b..8eb2f46da 100644 --- a/include/coal/broadphase/detail/hierarchy_tree.h +++ b/include/coal/broadphase/detail/hierarchy_tree.h @@ -42,10 +42,10 @@ #include #include #include -#include "hpp/fcl/warning.hh" -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/broadphase/detail/morton.h" -#include "hpp/fcl/broadphase/detail/node_base.h" +#include "coal/warning.hh" +#include "coal/BV/AABB.h" +#include "coal/broadphase/detail/morton.h" +#include "coal/broadphase/detail/node_base.h" namespace coal { @@ -287,6 +287,6 @@ size_t select(const BV& query, const NodeBase& node1, } // namespace detail } // namespace coal -#include "hpp/fcl/broadphase/detail/hierarchy_tree-inl.h" +#include "coal/broadphase/detail/hierarchy_tree-inl.h" #endif diff --git a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h index 131b82b87..d9ba6c84f 100644 --- a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h +++ b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_HIERARCHY_TREE_ARRAY_INL_H #define COAL_HIERARCHY_TREE_ARRAY_INL_H -#include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h" +#include "coal/broadphase/detail/hierarchy_tree_array.h" #include #include diff --git a/include/coal/broadphase/detail/hierarchy_tree_array.h b/include/coal/broadphase/detail/hierarchy_tree_array.h index d5d9b66ab..23e2c2bcc 100644 --- a/include/coal/broadphase/detail/hierarchy_tree_array.h +++ b/include/coal/broadphase/detail/hierarchy_tree_array.h @@ -42,10 +42,10 @@ #include #include -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/broadphase/detail/morton.h" -#include "hpp/fcl/broadphase/detail/node_base_array.h" +#include "coal/fwd.hh" +#include "coal/BV/AABB.h" +#include "coal/broadphase/detail/morton.h" +#include "coal/broadphase/detail/node_base_array.h" namespace coal { @@ -295,6 +295,6 @@ size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes); } // namespace detail } // namespace coal -#include "hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h" +#include "coal/broadphase/detail/hierarchy_tree_array-inl.h" #endif diff --git a/include/coal/broadphase/detail/interval_tree.h b/include/coal/broadphase/detail/interval_tree.h index 46001856b..b17c8b238 100644 --- a/include/coal/broadphase/detail/interval_tree.h +++ b/include/coal/broadphase/detail/interval_tree.h @@ -42,7 +42,7 @@ #include #include -#include "hpp/fcl/broadphase/detail/interval_tree_node.h" +#include "coal/broadphase/detail/interval_tree_node.h" namespace coal { namespace detail { @@ -50,7 +50,7 @@ namespace detail { /// @brief Class describes the information needed when we take the /// right branch in searching for intervals but possibly come back /// and check the left branch as well. -struct HPP_FCL_DLLAPI it_recursion_node { +struct COAL_DLLAPI it_recursion_node { public: IntervalTreeNode* start_node; @@ -60,7 +60,7 @@ struct HPP_FCL_DLLAPI it_recursion_node { }; /// @brief Interval tree -class HPP_FCL_DLLAPI IntervalTree { +class COAL_DLLAPI IntervalTree { public: IntervalTree(); diff --git a/include/coal/broadphase/detail/interval_tree_node-inl.h b/include/coal/broadphase/detail/interval_tree_node-inl.h index 58e01c3fc..1edd7e6d2 100644 --- a/include/coal/broadphase/detail/interval_tree_node-inl.h +++ b/include/coal/broadphase/detail/interval_tree_node-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#include "hpp/fcl/broadphase/detail/interval_tree_node.h" +#include "coal/broadphase/detail/interval_tree_node.h" #include #include diff --git a/include/coal/broadphase/detail/interval_tree_node.h b/include/coal/broadphase/detail/interval_tree_node.h index a6956de2b..b053a2f9c 100644 --- a/include/coal/broadphase/detail/interval_tree_node.h +++ b/include/coal/broadphase/detail/interval_tree_node.h @@ -38,17 +38,17 @@ #ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H #define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H -#include "hpp/fcl/broadphase/detail/simple_interval.h" -#include "hpp/fcl/fwd.hh" +#include "coal/broadphase/detail/simple_interval.h" +#include "coal/fwd.hh" namespace coal { namespace detail { -class HPP_FCL_DLLAPI IntervalTree; +class COAL_DLLAPI IntervalTree; /// @brief The node for interval tree -class HPP_FCL_DLLAPI IntervalTreeNode { +class COAL_DLLAPI IntervalTreeNode { public: friend class IntervalTree; diff --git a/include/coal/broadphase/detail/morton-inl.h b/include/coal/broadphase/detail/morton-inl.h index 80354a76d..4f27aef91 100644 --- a/include/coal/broadphase/detail/morton-inl.h +++ b/include/coal/broadphase/detail/morton-inl.h @@ -39,7 +39,7 @@ #ifndef COAL_MORTON_INL_H #define COAL_MORTON_INL_H -#include "hpp/fcl/broadphase/detail/morton.h" +#include "coal/broadphase/detail/morton.h" namespace coal { /// @cond IGNORE namespace detail { diff --git a/include/coal/broadphase/detail/morton.h b/include/coal/broadphase/detail/morton.h index a43abdf9a..549e7df0b 100644 --- a/include/coal/broadphase/detail/morton.h +++ b/include/coal/broadphase/detail/morton.h @@ -39,7 +39,7 @@ #ifndef COAL_MORTON_H #define COAL_MORTON_H -#include "hpp/fcl/BV/AABB.h" +#include "coal/BV/AABB.h" #include #include @@ -52,11 +52,11 @@ template uint32_t quantize(S x, uint32_t n); /// @brief compute 30 bit morton code -HPP_FCL_DLLAPI +COAL_DLLAPI uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z); /// @brief compute 60 bit morton code -HPP_FCL_DLLAPI +COAL_DLLAPI uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z); /// @brief Functor to compute the morton code for a given AABB @@ -115,6 +115,6 @@ struct morton_functor> { /// @endcond } // namespace coal -#include "hpp/fcl/broadphase/detail/morton-inl.h" +#include "coal/broadphase/detail/morton-inl.h" #endif diff --git a/include/coal/broadphase/detail/node_base-inl.h b/include/coal/broadphase/detail/node_base-inl.h index a4b30052b..db63bcee1 100644 --- a/include/coal/broadphase/detail/node_base-inl.h +++ b/include/coal/broadphase/detail/node_base-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_NODEBASE_INL_H #define COAL_BROADPHASE_DETAIL_NODEBASE_INL_H -#include "hpp/fcl/broadphase/detail/node_base.h" +#include "coal/broadphase/detail/node_base.h" namespace coal { namespace detail { diff --git a/include/coal/broadphase/detail/node_base.h b/include/coal/broadphase/detail/node_base.h index 33ba47892..179495e6f 100644 --- a/include/coal/broadphase/detail/node_base.h +++ b/include/coal/broadphase/detail/node_base.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_NODEBASE_H #define COAL_BROADPHASE_DETAIL_NODEBASE_H -#include "hpp/fcl/data_types.h" +#include "coal/data_types.h" namespace coal { @@ -74,6 +74,6 @@ struct NodeBase { } // namespace detail } // namespace coal -#include "hpp/fcl/broadphase/detail/node_base-inl.h" +#include "coal/broadphase/detail/node_base-inl.h" #endif diff --git a/include/coal/broadphase/detail/node_base_array-inl.h b/include/coal/broadphase/detail/node_base_array-inl.h index b2f4f6ba2..a48fceef9 100644 --- a/include/coal/broadphase/detail/node_base_array-inl.h +++ b/include/coal/broadphase/detail/node_base_array-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H #define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H -#include "hpp/fcl/broadphase/detail/node_base_array.h" +#include "coal/broadphase/detail/node_base_array.h" namespace coal { diff --git a/include/coal/broadphase/detail/node_base_array.h b/include/coal/broadphase/detail/node_base_array.h index 814b401e6..5c18ef1f7 100644 --- a/include/coal/broadphase/detail/node_base_array.h +++ b/include/coal/broadphase/detail/node_base_array.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H #define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H -#include "hpp/fcl/data_types.h" +#include "coal/data_types.h" namespace coal { @@ -70,6 +70,6 @@ struct NodeBase { } // namespace detail } // namespace coal -#include "hpp/fcl/broadphase/detail/node_base_array-inl.h" +#include "coal/broadphase/detail/node_base_array-inl.h" #endif diff --git a/include/coal/broadphase/detail/simple_hash_table-inl.h b/include/coal/broadphase/detail/simple_hash_table-inl.h index 9922982e5..1243d957d 100644 --- a/include/coal/broadphase/detail/simple_hash_table-inl.h +++ b/include/coal/broadphase/detail/simple_hash_table-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H #define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H -#include "hpp/fcl/broadphase/detail/simple_hash_table.h" +#include "coal/broadphase/detail/simple_hash_table.h" #include namespace coal { diff --git a/include/coal/broadphase/detail/simple_hash_table.h b/include/coal/broadphase/detail/simple_hash_table.h index 8c480a3fb..1b4749026 100644 --- a/include/coal/broadphase/detail/simple_hash_table.h +++ b/include/coal/broadphase/detail/simple_hash_table.h @@ -82,6 +82,6 @@ class SimpleHashTable { } // namespace detail } // namespace coal -#include "hpp/fcl/broadphase/detail/simple_hash_table-inl.h" +#include "coal/broadphase/detail/simple_hash_table-inl.h" #endif diff --git a/include/coal/broadphase/detail/simple_interval-inl.h b/include/coal/broadphase/detail/simple_interval-inl.h index 4625c31e6..95075cbc1 100644 --- a/include/coal/broadphase/detail/simple_interval-inl.h +++ b/include/coal/broadphase/detail/simple_interval-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H -#include "hpp/fcl/broadphase/detail/simple_interval.h" +#include "coal/broadphase/detail/simple_interval.h" #include namespace coal { diff --git a/include/coal/broadphase/detail/simple_interval.h b/include/coal/broadphase/detail/simple_interval.h index d94f81e4d..7b654048c 100644 --- a/include/coal/broadphase/detail/simple_interval.h +++ b/include/coal/broadphase/detail/simple_interval.h @@ -38,15 +38,15 @@ #ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H #define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_H -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" +#include "coal/fwd.hh" +#include "coal/data_types.h" namespace coal { namespace detail { /// @brief Interval trees implemented using red-black-trees as described in /// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. -struct HPP_FCL_DLLAPI SimpleInterval { +struct COAL_DLLAPI SimpleInterval { public: virtual ~SimpleInterval(); diff --git a/include/coal/broadphase/detail/sparse_hash_table-inl.h b/include/coal/broadphase/detail/sparse_hash_table-inl.h index 519e49396..299f9871a 100644 --- a/include/coal/broadphase/detail/sparse_hash_table-inl.h +++ b/include/coal/broadphase/detail/sparse_hash_table-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_SPARSEHASHTABLE_INL_H #define COAL_BROADPHASE_SPARSEHASHTABLE_INL_H -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" +#include "coal/broadphase/detail/sparse_hash_table.h" namespace coal { diff --git a/include/coal/broadphase/detail/sparse_hash_table.h b/include/coal/broadphase/detail/sparse_hash_table.h index be28e379b..13c4aeeca 100644 --- a/include/coal/broadphase/detail/sparse_hash_table.h +++ b/include/coal/broadphase/detail/sparse_hash_table.h @@ -88,6 +88,6 @@ class SparseHashTable { } // namespace detail } // namespace coal -#include "hpp/fcl/broadphase/detail/sparse_hash_table-inl.h" +#include "coal/broadphase/detail/sparse_hash_table-inl.h" #endif diff --git a/include/coal/broadphase/detail/spatial_hash-inl.h b/include/coal/broadphase/detail/spatial_hash-inl.h index 76714036e..499aee848 100644 --- a/include/coal/broadphase/detail/spatial_hash-inl.h +++ b/include/coal/broadphase/detail/spatial_hash-inl.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_SPATIALHASH_INL_H #define COAL_BROADPHASE_SPATIALHASH_INL_H -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/broadphase/detail/spatial_hash.h" #include namespace coal { diff --git a/include/coal/broadphase/detail/spatial_hash.h b/include/coal/broadphase/detail/spatial_hash.h index e51a403a8..a5301a793 100644 --- a/include/coal/broadphase/detail/spatial_hash.h +++ b/include/coal/broadphase/detail/spatial_hash.h @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_SPATIALHASH_H #define COAL_BROADPHASE_SPATIALHASH_H -#include "hpp/fcl/BV/AABB.h" +#include "coal/BV/AABB.h" #include namespace coal { @@ -46,7 +46,7 @@ namespace coal { namespace detail { /// @brief Spatial hash function: hash an AABB to a set of integer values -struct HPP_FCL_DLLAPI SpatialHash { +struct COAL_DLLAPI SpatialHash { SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_); std::vector operator()(const AABB& aabb) const; diff --git a/include/coal/collision.h b/include/coal/collision.h index 0a53146b0..39ae20b63 100644 --- a/include/coal/collision.h +++ b/include/coal/collision.h @@ -39,11 +39,11 @@ #ifndef COAL_COLLISION_H #define COAL_COLLISION_H -#include -#include -#include -#include -#include +#include "coal/data_types.h" +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/collision_func_matrix.h" +#include "coal/timings.h" namespace coal { @@ -54,19 +54,19 @@ namespace coal { /// depth; otherwise only contact primitive id is returned), this function /// performs the collision between them. /// Return value is the number of contacts generated between the two objects. -HPP_FCL_DLLAPI std::size_t collide(const CollisionObject* o1, - const CollisionObject* o2, - const CollisionRequest& request, - CollisionResult& result); +COAL_DLLAPI std::size_t collide(const CollisionObject* o1, + const CollisionObject* o2, + const CollisionRequest& request, + CollisionResult& result); /// @copydoc collide(const CollisionObject*, const CollisionObject*, const /// CollisionRequest&, CollisionResult&) -HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const CollisionRequest& request, - CollisionResult& result); +COAL_DLLAPI std::size_t collide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionRequest& request, + CollisionResult& result); /// @brief This class reduces the cost of identifying the geometry pair. /// This is mostly useful for repeated shape-shape queries. @@ -75,7 +75,7 @@ HPP_FCL_DLLAPI std::size_t collide(const CollisionGeometry* o1, /// ComputeCollision calc_collision (o1, o2); /// std::size_t ncontacts = calc_collision(tf1, tf2, request, result); /// \endcode -class HPP_FCL_DLLAPI ComputeCollision { +class COAL_DLLAPI ComputeCollision { public: /// @brief Default constructor from two Collision Geometries. ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2); diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index cec422aaa..505716a7a 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -45,17 +45,17 @@ #include #include -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/config.hh" -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/timings.h" -#include "hpp/fcl/narrowphase/narrowphase_defaults.h" -#include "hpp/fcl/logging.h" +#include "coal/collision_object.h" +#include "coal/config.hh" +#include "coal/data_types.h" +#include "coal/timings.h" +#include "coal/narrowphase/narrowphase_defaults.h" +#include "coal/logging.h" namespace coal { /// @brief Contact information returned by collision -struct HPP_FCL_DLLAPI Contact { +struct COAL_DLLAPI Contact { /// @brief collision object 1 const CollisionGeometry* o1; @@ -167,13 +167,13 @@ struct HPP_FCL_DLLAPI Contact { struct QueryResult; /// @brief base class for all query requests -struct HPP_FCL_DLLAPI QueryRequest { +struct COAL_DLLAPI QueryRequest { // @brief Initial guess to use for the GJK algorithm GJKInitialGuess gjk_initial_guess; /// @brief whether enable gjk initial guess /// @Deprecated Use gjk_initial_guess instead - HPP_FCL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) + COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) bool enable_cached_gjk_guess; /// @brief the gjk initial guess set by user @@ -272,7 +272,7 @@ struct HPP_FCL_DLLAPI QueryRequest { }; /// @brief base class for all query results -struct HPP_FCL_DLLAPI QueryResult { +struct COAL_DLLAPI QueryResult { /// @brief stores the last GJK ray when relevant. Vec3f cached_gjk_guess; @@ -308,7 +308,7 @@ enum CollisionRequestFlag { }; /// @brief request to the collision algorithm -struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest { +struct COAL_DLLAPI CollisionRequest : QueryRequest { /// @brief The maximum number of contacts that can be returned size_t num_max_contacts; @@ -317,7 +317,7 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest { bool enable_contact; /// Whether a lower bound on distance is returned when objects are disjoint - HPP_FCL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.) + COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.) bool enable_distance_lower_bound; /// @brief Distance below which objects are considered in collision. @@ -387,7 +387,7 @@ inline FCL_REAL Contact::getDistanceToCollision( } /// @brief collision result -struct HPP_FCL_DLLAPI CollisionResult : QueryResult { +struct COAL_DLLAPI CollisionResult : QueryResult { private: /// @brief contact information std::vector contacts; @@ -508,7 +508,7 @@ struct HPP_FCL_DLLAPI CollisionResult : QueryResult { /// stored in a counter-clockwise fashion. /// @note If needed, the internal algorithms of hpp-fcl can easily be extended /// to compute a contact volume instead of a contact patch. -struct HPP_FCL_DLLAPI ContactPatch { +struct COAL_DLLAPI ContactPatch { public: using Polygon = std::vector; @@ -722,7 +722,7 @@ inline void constructContactPatchFrameFromContact(const Contact& contact, using SupportSet = ContactPatch; /// @brief Request for a contact patch computation. -struct HPP_FCL_DLLAPI ContactPatchRequest { +struct COAL_DLLAPI ContactPatchRequest { /// @brief Maximum number of contact patches that will be computed. size_t max_num_patch; @@ -820,7 +820,7 @@ struct HPP_FCL_DLLAPI ContactPatchRequest { }; /// @brief Result for a contact patch computation. -struct HPP_FCL_DLLAPI ContactPatchResult { +struct COAL_DLLAPI ContactPatchResult { using ContactPatchVector = std::vector; using ContactPatchRef = std::reference_wrapper; using ContactPatchRefVector = std::vector; @@ -983,11 +983,11 @@ struct HPP_FCL_DLLAPI ContactPatchResult { struct DistanceResult; /// @brief request to the distance computation -struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest { +struct COAL_DLLAPI DistanceRequest : QueryRequest { /// @brief whether to return the nearest points. /// Nearest points are always computed and are the points of the shapes that /// achieve a distance of `DistanceResult::min_distance`. - HPP_FCL_DEPRECATED_MESSAGE( + COAL_DEPRECATED_MESSAGE( Nearest points are always computed : they are the points of the shapes that achieve a distance of `DistanceResult::min_distance` @@ -1049,7 +1049,7 @@ struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest { }; /// @brief distance result -struct HPP_FCL_DLLAPI DistanceResult : QueryResult { +struct COAL_DLLAPI DistanceResult : QueryResult { public: /// @brief minimum distance between two objects. /// If two objects are in collision and diff --git a/include/coal/collision_func_matrix.h b/include/coal/collision_func_matrix.h index 5d504c69c..4311d4613 100644 --- a/include/coal/collision_func_matrix.h +++ b/include/coal/collision_func_matrix.h @@ -38,16 +38,16 @@ #ifndef COAL_COLLISION_FUNC_MATRIX_H #define COAL_COLLISION_FUNC_MATRIX_H -#include -#include -#include +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/narrowphase/narrowphase.h" namespace coal { /// @brief collision matrix stores the functions for collision between different /// types of objects and provides a uniform call interface -struct HPP_FCL_DLLAPI CollisionFunctionMatrix { +struct COAL_DLLAPI CollisionFunctionMatrix { /// @brief the uniform call interface for collision: for collision, we need /// know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h index efad46b3c..5e7900ff0 100644 --- a/include/coal/collision_object.h +++ b/include/coal/collision_object.h @@ -41,10 +41,10 @@ #include #include -#include -#include -#include -#include +#include "coal/deprecated.hh" +#include "coal/fwd.hh" +#include "coal/BV/AABB.h" +#include "coal/math/transform.h" namespace coal { @@ -91,7 +91,7 @@ enum NODE_TYPE { /// @{ /// @brief The geometry for the object for collision or distance computation -class HPP_FCL_DLLAPI CollisionGeometry { +class COAL_DLLAPI CollisionGeometry { public: CollisionGeometry() : aabb_center(Vec3f::Constant((std::numeric_limits::max)())), @@ -211,7 +211,7 @@ class HPP_FCL_DLLAPI CollisionGeometry { /// @brief the object for collision or distance computation, contains the /// geometry and the transform information -class HPP_FCL_DLLAPI CollisionObject { +class COAL_DLLAPI CollisionObject { public: CollisionObject(const shared_ptr& cgeom_, bool compute_local_aabb = true) diff --git a/include/coal/collision_utility.h b/include/coal/collision_utility.h index e151dd533..7f72d0e9c 100644 --- a/include/coal/collision_utility.h +++ b/include/coal/collision_utility.h @@ -18,13 +18,13 @@ #ifndef COAL_COLLISION_UTILITY_H #define COAL_COLLISION_UTILITY_H -#include +#include "coal/collision_object.h" namespace coal { -HPP_FCL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, - const Transform3f& pose, - const AABB& aabb); +COAL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, + const Transform3f& pose, + const AABB& aabb); /** * \brief Returns the name associated to a NODE_TYPE diff --git a/include/coal/contact_patch.h b/include/coal/contact_patch.h index 7b9159bd3..edea02d2c 100644 --- a/include/coal/contact_patch.h +++ b/include/coal/contact_patch.h @@ -37,10 +37,10 @@ #ifndef COAL_CONTACT_PATCH_H #define COAL_CONTACT_PATCH_H -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/contact_patch/contact_patch_solver.h" -#include "hpp/fcl/contact_patch_func_matrix.h" +#include "coal/data_types.h" +#include "coal/collision_data.h" +#include "coal/contact_patch/contact_patch_solver.h" +#include "coal/contact_patch_func_matrix.h" namespace coal { @@ -49,22 +49,22 @@ namespace coal { /// more info on the content of the input/output of this function. Also, please /// read @ref ContactPatch if you want to fully understand what is meant by /// "contact patch". -HPP_FCL_DLLAPI void computeContactPatch(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const CollisionResult& collision_result, - const ContactPatchRequest& request, - ContactPatchResult& result); +COAL_DLLAPI void computeContactPatch(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result); /// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3f&, // const CollisionGeometry*, const Transform3f&, const CollisionResult&, const // ContactPatchRequest&, ContactPatchResult&); -HPP_FCL_DLLAPI void computeContactPatch(const CollisionObject* o1, - const CollisionObject* o2, - const CollisionResult& collision_result, - const ContactPatchRequest& request, - ContactPatchResult& result); +COAL_DLLAPI void computeContactPatch(const CollisionObject* o1, + const CollisionObject* o2, + const CollisionResult& collision_result, + const ContactPatchRequest& request, + ContactPatchResult& result); /// @brief This class reduces the cost of identifying the geometry pair. /// This is usefull for repeated shape-shape queries. @@ -74,7 +74,7 @@ HPP_FCL_DLLAPI void computeContactPatch(const CollisionObject* o1, /// ComputeContactPatch calc_patch (o1, o2); /// calc_patch(tf1, tf2, collision_result, patch_request, patch_result); /// \endcode -class HPP_FCL_DLLAPI ComputeContactPatch { +class COAL_DLLAPI ComputeContactPatch { public: /// @brief Default constructor from two Collision Geometries. ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2); diff --git a/include/coal/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h index e6af26b57..2d230c141 100644 --- a/include/coal/contact_patch/contact_patch_solver.h +++ b/include/coal/contact_patch/contact_patch_solver.h @@ -36,9 +36,9 @@ #ifndef COAL_CONTACT_PATCH_SOLVER_H #define COAL_CONTACT_PATCH_SOLVER_H -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/logging.h" -#include "hpp/fcl/narrowphase/gjk.h" +#include "coal/collision_data.h" +#include "coal/logging.h" +#include "coal/narrowphase/gjk.h" namespace coal { @@ -56,7 +56,7 @@ namespace coal { /// /// TODO(louis): algo improvement: /// - The clipping algo is currently n1 * n2; it can be done in n1 + n2. -struct HPP_FCL_DLLAPI ContactPatchSolver { +struct COAL_DLLAPI ContactPatchSolver { // Note: `ContactPatch` is an alias for `SupportSet`. // The two can be used interchangeably. using ShapeSupportData = details::ShapeSupportData; @@ -203,6 +203,6 @@ struct HPP_FCL_DLLAPI ContactPatchSolver { } // namespace coal -#include "hpp/fcl/contact_patch/contact_patch_solver.hxx" +#include "coal/contact_patch/contact_patch_solver.hxx" #endif // COAL_CONTACT_PATCH_SOLVER_H diff --git a/include/coal/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx index d6cc0c96a..9eecdd071 100644 --- a/include/coal/contact_patch/contact_patch_solver.hxx +++ b/include/coal/contact_patch/contact_patch_solver.hxx @@ -37,8 +37,8 @@ #ifndef COAL_CONTACT_PATCH_SOLVER_HXX #define COAL_CONTACT_PATCH_SOLVER_HXX -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/shape/geometric_shapes_traits.h" +#include "coal/data_types.h" +#include "coal/shape/geometric_shapes_traits.h" namespace coal { diff --git a/include/coal/contact_patch_func_matrix.h b/include/coal/contact_patch_func_matrix.h index faa01a9a5..50b08d291 100644 --- a/include/coal/contact_patch_func_matrix.h +++ b/include/coal/contact_patch_func_matrix.h @@ -37,16 +37,16 @@ #ifndef COAL_CONTACT_PATCH_FUNC_MATRIX_H #define COAL_CONTACT_PATCH_FUNC_MATRIX_H -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/contact_patch/contact_patch_solver.h" -#include "hpp/fcl/narrowphase/narrowphase.h" +#include "coal/collision_data.h" +#include "coal/contact_patch/contact_patch_solver.h" +#include "coal/narrowphase/narrowphase.h" namespace coal { /// @brief The contact patch matrix stores the functions for contact patches /// computation between different types of objects and provides a uniform call /// interface -struct HPP_FCL_DLLAPI ContactPatchFunctionMatrix { +struct COAL_DLLAPI ContactPatchFunctionMatrix { /// @brief the uniform call interface for computing contact patches: we need /// know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 diff --git a/include/coal/data_types.h b/include/coal/data_types.h index 2cbb183e9..0759485bf 100644 --- a/include/coal/data_types.h +++ b/include/coal/data_types.h @@ -42,7 +42,7 @@ #include #include -#include +#include "coal/config.hh" #ifdef HPP_FCL_HAS_OCTOMAP #define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ @@ -94,7 +94,7 @@ enum GJKConvergenceCriterion { Default, DualityGap, Hybrid }; enum GJKConvergenceCriterionType { Relative, Absolute }; /// @brief Triangle with 3 indices for points -class HPP_FCL_DLLAPI Triangle { +class COAL_DLLAPI Triangle { public: typedef std::size_t index_type; typedef int size_type; @@ -140,7 +140,7 @@ class HPP_FCL_DLLAPI Triangle { }; /// @brief Quadrilateral with 4 indices for points -struct HPP_FCL_DLLAPI Quadrilateral { +struct COAL_DLLAPI Quadrilateral { typedef std::size_t index_type; typedef int size_type; diff --git a/include/coal/distance.h b/include/coal/distance.h index f734c760a..bf4beef7a 100644 --- a/include/coal/distance.h +++ b/include/coal/distance.h @@ -38,10 +38,10 @@ #ifndef COAL_DISTANCE_H #define COAL_DISTANCE_H -#include -#include -#include -#include +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/distance_func_matrix.h" +#include "coal/timings.h" namespace coal { @@ -49,19 +49,19 @@ namespace coal { /// requirements for contacts, including whether return the nearest points, this /// function performs the distance between them. Return value is the minimum /// distance generated between the two objects. -HPP_FCL_DLLAPI FCL_REAL distance(const CollisionObject* o1, - const CollisionObject* o2, - const DistanceRequest& request, - DistanceResult& result); +COAL_DLLAPI FCL_REAL distance(const CollisionObject* o1, + const CollisionObject* o2, + const DistanceRequest& request, + DistanceResult& result); /// @copydoc distance(const CollisionObject*, const CollisionObject*, const /// DistanceRequest&, DistanceResult&) -HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result); +COAL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result); /// This class reduces the cost of identifying the geometry pair. /// This is mostly useful for repeated shape-shape queries. @@ -70,7 +70,7 @@ HPP_FCL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, /// ComputeDistance calc_distance (o1, o2); /// FCL_REAL distance = calc_distance(tf1, tf2, request, result); /// \endcode -class HPP_FCL_DLLAPI ComputeDistance { +class COAL_DLLAPI ComputeDistance { public: ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2); diff --git a/include/coal/distance_func_matrix.h b/include/coal/distance_func_matrix.h index 29d79a5cb..d6bd5b7e6 100644 --- a/include/coal/distance_func_matrix.h +++ b/include/coal/distance_func_matrix.h @@ -38,15 +38,15 @@ #ifndef COAL_DISTANCE_FUNC_MATRIX_H #define COAL_DISTANCE_FUNC_MATRIX_H -#include -#include -#include +#include "coal/collision_object.h" +#include "coal/collision_data.h" +#include "coal/narrowphase/narrowphase.h" namespace coal { /// @brief distance matrix stores the functions for distance between different /// types of objects and provides a uniform call interface -struct HPP_FCL_DLLAPI DistanceFunctionMatrix { +struct COAL_DLLAPI DistanceFunctionMatrix { /// @brief the uniform call interface for distance: for distance, we need know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 /// and tf2; diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh index 14b673c20..e699a7b16 100644 --- a/include/coal/fwd.hh +++ b/include/coal/fwd.hh @@ -43,9 +43,9 @@ #include #include -#include -#include -#include +#include "coal/config.hh" +#include "coal/deprecated.hh" +#include "coal/warning.hh" #if _WIN32 #define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__ diff --git a/include/coal/hfield.h b/include/coal/hfield.h index af6b43629..571d150ca 100644 --- a/include/coal/hfield.h +++ b/include/coal/hfield.h @@ -37,11 +37,11 @@ #ifndef COAL_HEIGHT_FIELD_H #define COAL_HEIGHT_FIELD_H -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/BV/BV_node.h" -#include "hpp/fcl/BVH/BVH_internal.h" +#include "coal/fwd.hh" +#include "coal/data_types.h" +#include "coal/collision_object.h" +#include "coal/BV/BV_node.h" +#include "coal/BVH/BVH_internal.h" #include @@ -50,7 +50,7 @@ namespace coal { /// @addtogroup Construction_Of_HeightField /// @{ -struct HPP_FCL_DLLAPI HFNodeBase { +struct COAL_DLLAPI HFNodeBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW enum class FaceOrientation { @@ -125,7 +125,7 @@ inline int operator&(int a, HFNodeBase::FaceOrientation b) { } template -struct HPP_FCL_DLLAPI HFNode : public HFNodeBase { +struct COAL_DLLAPI HFNode : public HFNodeBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef HFNodeBase Base; @@ -199,7 +199,7 @@ struct UpdateBoundingVolume { /// The height field is centered at the origin and the corners of the geometry /// correspond to the following coordinates [± x_dim/2; ± y_dim/2]. template -class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { +class COAL_DLLAPI HeightField : public CollisionGeometry { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/coal/internal/BV_fitter.h b/include/coal/internal/BV_fitter.h index 4461b0b22..93f126782 100644 --- a/include/coal/internal/BV_fitter.h +++ b/include/coal/internal/BV_fitter.h @@ -38,10 +38,10 @@ #ifndef COAL_BV_FITTER_H #define COAL_BV_FITTER_H -#include -#include -#include -#include +#include "coal/BVH/BVH_internal.h" +#include "coal/BV/kIOS.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BV/AABB.h" #include namespace coal { @@ -73,7 +73,7 @@ void fit(Vec3f* ps, unsigned int n, AABB& bv); /// @brief The class for the default algorithm fitting a bounding volume to a /// set of points template -class HPP_FCL_DLLAPI BVFitterTpl { +class COAL_DLLAPI BVFitterTpl { public: /// @brief default deconstructor virtual ~BVFitterTpl() {} @@ -118,7 +118,7 @@ class HPP_FCL_DLLAPI BVFitterTpl { /// @brief The class for the default algorithm fitting a bounding volume to a /// set of points template -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { +class COAL_DLLAPI BVFitter : public BVFitterTpl { typedef BVFitterTpl Base; public: @@ -167,7 +167,7 @@ class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { /// @brief Specification of BVFitter for OBB bounding volume template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { +class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and @@ -177,7 +177,7 @@ class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { /// @brief Specification of BVFitter for RSS bounding volume template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { +class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and @@ -187,7 +187,7 @@ class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { /// @brief Specification of BVFitter for kIOS bounding volume template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { +class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and @@ -197,7 +197,7 @@ class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { /// @brief Specification of BVFitter for OBBRSS bounding volume template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { +class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and @@ -207,7 +207,7 @@ class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { /// @brief Specification of BVFitter for AABB bounding volume template <> -class HPP_FCL_DLLAPI BVFitter : public BVFitterTpl { +class COAL_DLLAPI BVFitter : public BVFitterTpl { public: /// @brief Compute a bounding volume that fits a set of primitives (points or /// triangles). The primitive data was set by set function and diff --git a/include/coal/internal/BV_splitter.h b/include/coal/internal/BV_splitter.h index d5edd706c..01fabf1c7 100644 --- a/include/coal/internal/BV_splitter.h +++ b/include/coal/internal/BV_splitter.h @@ -38,9 +38,9 @@ #ifndef COAL_BV_SPLITTER_H #define COAL_BV_SPLITTER_H -#include -#include -#include +#include "coal/BVH/BVH_internal.h" +#include "coal/BV/kIOS.h" +#include "coal/BV/OBBRSS.h" #include #include @@ -207,74 +207,74 @@ class BVSplitter { }; template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; template <> -bool HPP_FCL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( +void COAL_DLLAPI BVSplitter::computeRule_mean( const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( +void COAL_DLLAPI BVSplitter::computeRule_median( const OBB& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( +void COAL_DLLAPI BVSplitter::computeRule_mean( const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( +void COAL_DLLAPI BVSplitter::computeRule_median( const RSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( +void COAL_DLLAPI BVSplitter::computeRule_mean( const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( +void COAL_DLLAPI BVSplitter::computeRule_median( const kIOS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_bvcenter( +void COAL_DLLAPI BVSplitter::computeRule_bvcenter( const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_mean( +void COAL_DLLAPI BVSplitter::computeRule_mean( const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); template <> -void HPP_FCL_DLLAPI BVSplitter::computeRule_median( +void COAL_DLLAPI BVSplitter::computeRule_median( const OBBRSS& bv, unsigned int* primitive_indices, unsigned int num_primitives); diff --git a/include/coal/internal/intersect.h b/include/coal/internal/intersect.h index d6dd5f518..0353df055 100644 --- a/include/coal/internal/intersect.h +++ b/include/coal/internal/intersect.h @@ -40,21 +40,21 @@ /// @cond INTERNAL -#include +#include "coal/math/transform.h" namespace coal { /// @brief CCD intersect kernel among primitives -class HPP_FCL_DLLAPI Intersect { +class COAL_DLLAPI Intersect { public: static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t); }; // class Intersect /// @brief Project functions -class HPP_FCL_DLLAPI Project { +class COAL_DLLAPI Project { public: - struct HPP_FCL_DLLAPI ProjectResult { + struct COAL_DLLAPI ProjectResult { /// @brief Parameterization of the projected point (based on the simplex to /// be projected, use 2 or 3 or 4 of the array) FCL_REAL parameterization[4]; @@ -94,7 +94,7 @@ class HPP_FCL_DLLAPI Project { }; /// @brief Triangle distance functions -class HPP_FCL_DLLAPI TriangleDistance { +class COAL_DLLAPI TriangleDistance { public: /// @brief Returns closest points between an segment pair. /// The first segment is P + t * A diff --git a/include/coal/internal/shape_shape_contact_patch_func.h b/include/coal/internal/shape_shape_contact_patch_func.h index 1f6fc4923..e854fb577 100644 --- a/include/coal/internal/shape_shape_contact_patch_func.h +++ b/include/coal/internal/shape_shape_contact_patch_func.h @@ -37,11 +37,11 @@ #ifndef COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H #define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/collision_utility.h" -#include "hpp/fcl/narrowphase/narrowphase.h" -#include "hpp/fcl/contact_patch/contact_patch_solver.h" -#include "hpp/fcl/shape/geometric_shapes_traits.h" +#include "coal/collision_data.h" +#include "coal/collision_utility.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/contact_patch/contact_patch_solver.h" +#include "coal/shape/geometric_shapes_traits.h" namespace coal { diff --git a/include/coal/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h index cee08823c..64b11d3b8 100644 --- a/include/coal/internal/shape_shape_func.h +++ b/include/coal/internal/shape_shape_func.h @@ -40,10 +40,10 @@ /// @cond INTERNAL -#include -#include -#include -#include +#include "coal/collision_data.h" +#include "coal/collision_utility.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_traits.h" namespace coal { @@ -212,19 +212,19 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ template <> \ - HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ + COAL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ Vec3f& p2, Vec3f& normal); \ template <> \ - HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ + COAL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ Vec3f& p2, Vec3f& normal); \ template <> \ - inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ + inline COAL_DLLAPI FCL_REAL ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ @@ -239,7 +239,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, return result.min_distance; \ } \ template <> \ - inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ + inline COAL_DLLAPI FCL_REAL ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ @@ -256,13 +256,13 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, #define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \ template <> \ - HPP_FCL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ + COAL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ Vec3f& p2, Vec3f& normal); \ template <> \ - inline HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance( \ + inline COAL_DLLAPI FCL_REAL ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ diff --git a/include/coal/internal/tools.h b/include/coal/internal/tools.h index e9a918a12..cb3eb0e0e 100644 --- a/include/coal/internal/tools.h +++ b/include/coal/internal/tools.h @@ -38,13 +38,13 @@ #ifndef COAL_INTERNAL_TOOLS_H #define COAL_INTERNAL_TOOLS_H -#include +#include "coal/fwd.hh" #include #include #include -#include +#include "coal/data_types.h" namespace coal { diff --git a/include/coal/internal/traversal.h b/include/coal/internal/traversal.h index eb74e2ab8..033ae89dd 100644 --- a/include/coal/internal/traversal.h +++ b/include/coal/internal/traversal.h @@ -45,7 +45,7 @@ enum { RelativeTransformationIsIdentity = 1 }; namespace details { template -struct HPP_FCL_DLLAPI RelativeTransformation { +struct COAL_DLLAPI RelativeTransformation { RelativeTransformation() : R(Matrix3f::Identity()) {} const Matrix3f& _R() const { return R; } @@ -56,7 +56,7 @@ struct HPP_FCL_DLLAPI RelativeTransformation { }; template <> -struct HPP_FCL_DLLAPI RelativeTransformation { +struct COAL_DLLAPI RelativeTransformation { static const Matrix3f& _R() { HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); } diff --git a/include/coal/internal/traversal_node_base.h b/include/coal/internal/traversal_node_base.h index b27334b4c..3dc33f321 100644 --- a/include/coal/internal/traversal_node_base.h +++ b/include/coal/internal/traversal_node_base.h @@ -40,9 +40,9 @@ /// @cond INTERNAL -#include -#include -#include +#include "coal/data_types.h" +#include "coal/math/transform.h" +#include "coal/collision_data.h" namespace coal { diff --git a/include/coal/internal/traversal_node_bvh_hfield.h b/include/coal/internal/traversal_node_bvh_hfield.h index dc43abbc5..c9c1c659a 100644 --- a/include/coal/internal/traversal_node_bvh_hfield.h +++ b/include/coal/internal/traversal_node_bvh_hfield.h @@ -39,17 +39,17 @@ /// @cond INTERNAL -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision_data.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_hfield_shape.h" +#include "coal/BV/BV_node.h" +#include "coal/BV/BV.h" +#include "coal/BVH/BVH_model.h" +#include "coal/hfield.h" +#include "coal/internal/intersect.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/internal/traversal.h" #include #include diff --git a/include/coal/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h index 7e03bf9dd..3b5d09ce6 100644 --- a/include/coal/internal/traversal_node_bvh_shape.h +++ b/include/coal/internal/traversal_node_bvh_shape.h @@ -40,14 +40,14 @@ /// @cond INTERNAL -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision_data.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal.h" +#include "coal/BVH/BVH_model.h" +#include "coal/internal/shape_shape_func.h" namespace coal { diff --git a/include/coal/internal/traversal_node_bvhs.h b/include/coal/internal/traversal_node_bvhs.h index 56cb923b5..2f6fd1c91 100644 --- a/include/coal/internal/traversal_node_bvhs.h +++ b/include/coal/internal/traversal_node_bvhs.h @@ -40,16 +40,16 @@ /// @cond INTERNAL -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision_data.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/BV/BV_node.h" +#include "coal/BV/BV.h" +#include "coal/BVH/BVH_model.h" +#include "coal/internal/intersect.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/internal/traversal.h" +#include "coal/internal/shape_shape_func.h" #include diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h index 2b62dead1..afc8c4bf8 100644 --- a/include/coal/internal/traversal_node_hfield_shape.h +++ b/include/coal/internal/traversal_node_hfield_shape.h @@ -39,16 +39,16 @@ /// @cond INTERNAL -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision_data.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/shape_shape_func.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal.h" +#include "coal/internal/intersect.h" +#include "coal/hfield.h" +#include "coal/shape/convex.h" namespace coal { diff --git a/include/coal/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h index 55a622cd1..33bd2e1f0 100644 --- a/include/coal/internal/traversal_node_octree.h +++ b/include/coal/internal/traversal_node_octree.h @@ -41,20 +41,20 @@ /// @cond INTERNAL -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision_data.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_hfield_shape.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/hfield.h" +#include "coal/octree.h" +#include "coal/BVH/BVH_model.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/shape_shape_func.h" namespace coal { /// @brief Algorithms for collision related with octree -class HPP_FCL_DLLAPI OcTreeSolver { +class COAL_DLLAPI OcTreeSolver { private: const GJKSolver* solver; @@ -1005,7 +1005,7 @@ class HPP_FCL_DLLAPI OcTreeSolver { /// @{ /// @brief Traversal node for octree collision -class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode +class COAL_DLLAPI OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeCollisionTraversalNode(const CollisionRequest& request) @@ -1034,7 +1034,7 @@ class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode /// @brief Traversal node for shape-octree collision template -class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode +class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request) @@ -1067,7 +1067,7 @@ class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode /// @brief Traversal node for octree-shape collision template -class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode +class COAL_DLLAPI OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeShapeCollisionTraversalNode(const CollisionRequest& request) @@ -1099,7 +1099,7 @@ class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode /// @brief Traversal node for mesh-octree collision template -class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode +class COAL_DLLAPI MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: MeshOcTreeCollisionTraversalNode(const CollisionRequest& request) @@ -1131,7 +1131,7 @@ class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode /// @brief Traversal node for octree-mesh collision template -class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode +class COAL_DLLAPI OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeMeshCollisionTraversalNode(const CollisionRequest& request) @@ -1163,7 +1163,7 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode /// @brief Traversal node for octree-height-field collision template -class HPP_FCL_DLLAPI OcTreeHeightFieldCollisionTraversalNode +class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request) @@ -1194,7 +1194,7 @@ class HPP_FCL_DLLAPI OcTreeHeightFieldCollisionTraversalNode /// @brief Traversal node for octree-height-field collision template -class HPP_FCL_DLLAPI HeightFieldOcTreeCollisionTraversalNode +class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request) @@ -1229,7 +1229,7 @@ class HPP_FCL_DLLAPI HeightFieldOcTreeCollisionTraversalNode /// @{ /// @brief Traversal node for octree distance -class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode +class COAL_DLLAPI OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeDistanceTraversalNode() { @@ -1257,7 +1257,7 @@ class HPP_FCL_DLLAPI OcTreeDistanceTraversalNode /// @brief Traversal node for shape-octree distance template -class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode +class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeOcTreeDistanceTraversalNode() { @@ -1281,7 +1281,7 @@ class HPP_FCL_DLLAPI ShapeOcTreeDistanceTraversalNode /// @brief Traversal node for octree-shape distance template -class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode +class COAL_DLLAPI OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeShapeDistanceTraversalNode() { @@ -1305,7 +1305,7 @@ class HPP_FCL_DLLAPI OcTreeShapeDistanceTraversalNode /// @brief Traversal node for mesh-octree distance template -class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode +class COAL_DLLAPI MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: MeshOcTreeDistanceTraversalNode() { @@ -1329,7 +1329,7 @@ class HPP_FCL_DLLAPI MeshOcTreeDistanceTraversalNode /// @brief Traversal node for octree-mesh distance template -class HPP_FCL_DLLAPI OcTreeMeshDistanceTraversalNode +class COAL_DLLAPI OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeMeshDistanceTraversalNode() { diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h index b3644f528..d63af3da6 100644 --- a/include/coal/internal/traversal_node_setup.h +++ b/include/coal/internal/traversal_node_setup.h @@ -40,20 +40,20 @@ /// @cond INTERNAL -#include -#include +#include "coal/internal/tools.h" +#include "coal/internal/traversal_node_shapes.h" -#include -#include +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_bvh_shape.h" // #include -#include +#include "coal/internal/traversal_node_hfield_shape.h" #ifdef HPP_FCL_HAS_OCTOMAP -#include +#include "coal/internal/traversal_node_octree.h" #endif -#include +#include "coal/BVH/BVH_utility.h" namespace coal { diff --git a/include/coal/internal/traversal_node_shapes.h b/include/coal/internal/traversal_node_shapes.h index 4e0326a94..b38885888 100644 --- a/include/coal/internal/traversal_node_shapes.h +++ b/include/coal/internal/traversal_node_shapes.h @@ -40,11 +40,11 @@ /// @cond INTERNAL -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/BV/BV.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" -#include "hpp/fcl/internal/traversal_node_base.h" -#include "hpp/fcl/internal/shape_shape_func.h" +#include "coal/collision_data.h" +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/shape_shape_func.h" namespace coal { @@ -53,7 +53,7 @@ namespace coal { /// @brief Traversal node for collision between two shapes template -class HPP_FCL_DLLAPI ShapeCollisionTraversalNode +class COAL_DLLAPI ShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: ShapeCollisionTraversalNode(const CollisionRequest& request) @@ -88,7 +88,7 @@ class HPP_FCL_DLLAPI ShapeCollisionTraversalNode /// @brief Traversal node for distance between two shapes template -class HPP_FCL_DLLAPI ShapeDistanceTraversalNode +class COAL_DLLAPI ShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { diff --git a/include/coal/internal/traversal_recurse.h b/include/coal/internal/traversal_recurse.h index 2d19879ec..ad28bcd24 100644 --- a/include/coal/internal/traversal_recurse.h +++ b/include/coal/internal/traversal_recurse.h @@ -40,10 +40,10 @@ /// @cond INTERNAL -#include +#include "coal/BVH/BVH_front.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_bvhs.h" #include -#include -#include namespace coal { diff --git a/include/coal/math/matrix_3f.h b/include/coal/math/matrix_3f.h index 408d0229a..70f555a02 100644 --- a/include/coal/math/matrix_3f.h +++ b/include/coal/math/matrix_3f.h @@ -38,9 +38,9 @@ #ifndef COAL_MATRIX_3F_H #define COAL_MATRIX_3F_H -#warning "This file is deprecated. Include instead." +#warning "This file is deprecated. Include instead." // List of equivalent includes. -#include +#include "coal/data_types.h" #endif diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h index dfa468c6e..0795da1f6 100644 --- a/include/coal/math/transform.h +++ b/include/coal/math/transform.h @@ -38,12 +38,12 @@ #ifndef COAL_TRANSFORM_H #define COAL_TRANSFORM_H -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/data_types.h" +#include "coal/fwd.hh" +#include "coal/data_types.h" namespace coal { -HPP_FCL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; +COAL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; typedef Eigen::Quaternion Quatf; static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) { @@ -52,7 +52,7 @@ static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) { } /// @brief Simple transform class used locally by InterpMotion -class HPP_FCL_DLLAPI Transform3f { +class COAL_DLLAPI Transform3f { /// @brief Matrix cache Matrix3f R; diff --git a/include/coal/math/types.h b/include/coal/math/types.h index ea9569499..4b9b5a91f 100644 --- a/include/coal/math/types.h +++ b/include/coal/math/types.h @@ -38,9 +38,9 @@ #ifndef COAL_MATH_TYPES_H #define COAL_MATH_TYPES_H -#warning "This file is deprecated. Include instead." +#warning "This file is deprecated. Include instead." // List of equivalent includes. -#include +#include "coal/data_types.h" #endif diff --git a/include/coal/math/vec_3f.h b/include/coal/math/vec_3f.h index 62df29a0e..5b79b2191 100644 --- a/include/coal/math/vec_3f.h +++ b/include/coal/math/vec_3f.h @@ -38,9 +38,9 @@ #ifndef COAL_VEC_3F_H #define COAL_VEC_3F_H -#warning "This file is deprecated. Include instead." +#warning "This file is deprecated. Include instead." // List of equivalent includes. -#include +#include "coal/data_types.h" #endif diff --git a/include/coal/mesh_loader/assimp.h b/include/coal/mesh_loader/assimp.h index d5672a44a..484a604cc 100644 --- a/include/coal/mesh_loader/assimp.h +++ b/include/coal/mesh_loader/assimp.h @@ -38,10 +38,10 @@ #ifndef COAL_MESH_LOADER_ASSIMP_H #define COAL_MESH_LOADER_ASSIMP_H -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/config.hh" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" struct aiScene; namespace Assimp { @@ -52,12 +52,12 @@ namespace coal { namespace internal { -struct HPP_FCL_DLLAPI TriangleAndVertices { +struct COAL_DLLAPI TriangleAndVertices { std::vector vertices_; std::vector triangles_; }; -struct HPP_FCL_DLLAPI Loader { +struct COAL_DLLAPI Loader { Loader(); ~Loader(); @@ -75,9 +75,8 @@ struct HPP_FCL_DLLAPI Loader { * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -HPP_FCL_DLLAPI void buildMesh(const coal::Vec3f& scale, const aiScene* scene, - unsigned vertices_offset, - TriangleAndVertices& tv); +COAL_DLLAPI void buildMesh(const coal::Vec3f& scale, const aiScene* scene, + unsigned vertices_offset, TriangleAndVertices& tv); /** * @brief Convert an assimp scene to a mesh diff --git a/include/coal/mesh_loader/loader.h b/include/coal/mesh_loader/loader.h index 167873592..c24f48d00 100644 --- a/include/coal/mesh_loader/loader.h +++ b/include/coal/mesh_loader/loader.h @@ -38,10 +38,10 @@ #ifndef COAL_MESH_LOADER_LOADER_H #define COAL_MESH_LOADER_LOADER_H -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/config.hh" +#include "coal/data_types.h" +#include "coal/collision_object.h" #include #include @@ -49,7 +49,7 @@ namespace coal { /// Base class for building polyhedron from files. /// This class builds a new object for each file. -class HPP_FCL_DLLAPI MeshLoader { +class COAL_DLLAPI MeshLoader { public: virtual ~MeshLoader() {} @@ -70,7 +70,7 @@ class HPP_FCL_DLLAPI MeshLoader { /// This class builds a new object for each different file. /// If method CachedMeshLoader::load is called twice with the same arguments, /// the second call returns the result of the first call. -class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader { +class COAL_DLLAPI CachedMeshLoader : public MeshLoader { public: virtual ~CachedMeshLoader() {} @@ -78,7 +78,7 @@ class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader { virtual BVHModelPtr_t load(const std::string& filename, const Vec3f& scale); - struct HPP_FCL_DLLAPI Key { + struct COAL_DLLAPI Key { std::string filename; Vec3f scale; @@ -86,7 +86,7 @@ class HPP_FCL_DLLAPI CachedMeshLoader : public MeshLoader { bool operator<(const CachedMeshLoader::Key& b) const; }; - struct HPP_FCL_DLLAPI Value { + struct COAL_DLLAPI Value { BVHModelPtr_t model; std::time_t mtime; }; diff --git a/include/coal/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h index b84e6eadf..b37407f93 100644 --- a/include/coal/narrowphase/gjk.h +++ b/include/coal/narrowphase/gjk.h @@ -41,7 +41,7 @@ #include -#include "hpp/fcl/narrowphase/minkowski_difference.h" +#include "coal/narrowphase/minkowski_difference.h" namespace coal { @@ -50,8 +50,8 @@ namespace details { /// @brief class for GJK algorithm /// /// @note The computations are performed in the frame of the first shape. -struct HPP_FCL_DLLAPI GJK { - struct HPP_FCL_DLLAPI SimplexV { +struct COAL_DLLAPI GJK { + struct COAL_DLLAPI SimplexV { /// @brief support vector for shape 0 and 1. Vec3f w0, w1; /// @brief support vector (i.e., the furthest point on the shape along the @@ -68,7 +68,7 @@ struct HPP_FCL_DLLAPI GJK { /// Since GJK does not need any more storage, it reuses these vertices /// throughout the algorithm by using multiple instance of this `Simplex` /// class. - struct HPP_FCL_DLLAPI Simplex { + struct COAL_DLLAPI Simplex { /// @brief simplex vertex SimplexV* vertex[4]; /// @brief size of simplex (number of vertices) @@ -255,9 +255,9 @@ struct HPP_FCL_DLLAPI GJK { }; /// @brief class for EPA algorithm -struct HPP_FCL_DLLAPI EPA { +struct COAL_DLLAPI EPA { typedef GJK::SimplexV SimplexVertex; - struct HPP_FCL_DLLAPI SimplexFace { + struct COAL_DLLAPI SimplexFace { Vec3f n; FCL_REAL d; bool ignore; // If the origin does not project inside the face, we @@ -278,7 +278,7 @@ struct HPP_FCL_DLLAPI EPA { /// @brief The simplex list of EPA is a linked list of faces. /// Note: EPA's linked list does **not** own any memory. /// The memory it refers to is contiguous and owned by a std::vector. - struct HPP_FCL_DLLAPI SimplexFaceList { + struct COAL_DLLAPI SimplexFaceList { SimplexFace* root; size_t count; SimplexFaceList() : root(nullptr), count(0) {} @@ -318,7 +318,7 @@ struct HPP_FCL_DLLAPI EPA { fb->adjacent_faces[eb] = fa; } - struct HPP_FCL_DLLAPI SimplexHorizon { + struct COAL_DLLAPI SimplexHorizon { SimplexFace* current_face; // current face in the horizon SimplexFace* first_face; // first face in the horizon size_t num_faces; // number of faces in the horizon diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h index 619b00c78..677373e56 100644 --- a/include/coal/narrowphase/minkowski_difference.h +++ b/include/coal/narrowphase/minkowski_difference.h @@ -39,9 +39,9 @@ #ifndef COAL_MINKOWSKI_DIFFERENCE_H #define COAL_MINKOWSKI_DIFFERENCE_H -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/narrowphase/support_functions.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/math/transform.h" +#include "coal/narrowphase/support_functions.h" namespace coal { @@ -50,7 +50,7 @@ namespace details { /// @brief Minkowski difference class of two shapes /// /// @note The Minkowski difference is expressed in the frame of the first shape. -struct HPP_FCL_DLLAPI MinkowskiDiff { +struct COAL_DLLAPI MinkowskiDiff { typedef Eigen::Array Array2d; /// @brief points to two shapes diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h index b0f463d59..28380ad21 100644 --- a/include/coal/narrowphase/narrowphase.h +++ b/include/coal/narrowphase/narrowphase.h @@ -42,10 +42,10 @@ #include -#include -#include -#include -#include +#include "coal/narrowphase/gjk.h" +#include "coal/collision_data.h" +#include "coal/narrowphase/narrowphase_defaults.h" +#include "coal/logging.h" namespace coal { @@ -54,7 +54,7 @@ namespace coal { /// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA /// have been largely modified to be faster and more robust to numerical /// accuracy and edge cases. -struct HPP_FCL_DLLAPI GJKSolver { +struct COAL_DLLAPI GJKSolver { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -72,7 +72,7 @@ struct HPP_FCL_DLLAPI GJKSolver { /// @brief Whether smart guess can be provided /// @Deprecated Use gjk_initial_guess instead - HPP_FCL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) + COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead) bool enable_cached_guess; /// @brief smart guess diff --git a/include/coal/narrowphase/narrowphase_defaults.h b/include/coal/narrowphase/narrowphase_defaults.h index 5ed5bb224..54ce75328 100644 --- a/include/coal/narrowphase/narrowphase_defaults.h +++ b/include/coal/narrowphase/narrowphase_defaults.h @@ -38,7 +38,7 @@ #ifndef COAL_NARROWPHASE_DEFAULTS_H #define COAL_NARROWPHASE_DEFAULTS_H -#include +#include "coal/data_types.h" namespace coal { diff --git a/include/coal/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h index d1ecec32c..5bc3facba 100644 --- a/include/coal/narrowphase/support_functions.h +++ b/include/coal/narrowphase/support_functions.h @@ -39,9 +39,9 @@ #ifndef COAL_SUPPORT_FUNCTIONS_H #define COAL_SUPPORT_FUNCTIONS_H -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/collision_data.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/math/transform.h" +#include "coal/collision_data.h" namespace coal { @@ -77,7 +77,7 @@ template Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint); /// @brief Stores temporary data for the computation of support points. -struct HPP_FCL_DLLAPI ShapeSupportData { +struct COAL_DLLAPI ShapeSupportData { // @brief Tracks which points have been visited in a ConvexBase. std::vector visited; @@ -298,8 +298,8 @@ void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, /// @param[in] cloud data which contains the 2d points of the support set which /// convex-hull we want to compute. /// @param[out] 2d points of the the support set's convex-hull. -HPP_FCL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud, - SupportSet::Polygon& cvx_hull); +COAL_DLLAPI void computeSupportSetConvexHull(SupportSet::Polygon& cloud, + SupportSet::Polygon& cvx_hull); } // namespace details diff --git a/include/coal/octree.h b/include/coal/octree.h index 2ee09b5eb..c3635c643 100644 --- a/include/coal/octree.h +++ b/include/coal/octree.h @@ -42,15 +42,15 @@ #include #include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/BV/AABB.h" +#include "coal/collision_object.h" namespace coal { /// @brief Octree is one type of collision geometry which can encode uncertainty /// information in the sensor data. -class HPP_FCL_DLLAPI OcTree : public CollisionGeometry { +class COAL_DLLAPI OcTree : public CollisionGeometry { protected: shared_ptr tree; @@ -330,7 +330,7 @@ static inline void computeChildBV(const AABB& root_bv, unsigned int i, /// /// \returns An OcTree that can be used for collision checking and more. /// -HPP_FCL_DLLAPI OcTreePtr_t +COAL_DLLAPI OcTreePtr_t makeOctree(const Eigen::Matrix& point_cloud, const FCL_REAL resolution); diff --git a/include/coal/serialization/AABB.h b/include/coal/serialization/AABB.h index 853cf073c..75e9be045 100644 --- a/include/coal/serialization/AABB.h +++ b/include/coal/serialization/AABB.h @@ -5,8 +5,8 @@ #ifndef COAL_SERIALIZATION_AABB_H #define COAL_SERIALIZATION_AABB_H -#include "hpp/fcl/BV/AABB.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/BV/AABB.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/BVH_model.h b/include/coal/serialization/BVH_model.h index 4783004c5..6fee61b98 100644 --- a/include/coal/serialization/BVH_model.h +++ b/include/coal/serialization/BVH_model.h @@ -5,14 +5,14 @@ #ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H #define HPP_FCL_SERIALIZATION_BVH_MODEL_H -#include "hpp/fcl/BVH/BVH_model.h" - -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/BV_node.h" -#include "hpp/fcl/serialization/BV_splitter.h" -#include "hpp/fcl/serialization/collision_object.h" -#include "hpp/fcl/serialization/memory.h" -#include "hpp/fcl/serialization/triangle.h" +#include "coal/BVH/BVH_model.h" + +#include "coal/serialization/fwd.h" +#include "coal/serialization/BV_node.h" +#include "coal/serialization/BV_splitter.h" +#include "coal/serialization/collision_object.h" +#include "coal/serialization/memory.h" +#include "coal/serialization/triangle.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/BV_node.h b/include/coal/serialization/BV_node.h index 2fc0406d3..6e207ee66 100644 --- a/include/coal/serialization/BV_node.h +++ b/include/coal/serialization/BV_node.h @@ -5,10 +5,10 @@ #ifndef COAL_SERIALIZATION_BV_NODE_H #define COAL_SERIALIZATION_BV_NODE_H -#include "hpp/fcl/BV/BV_node.h" +#include "coal/BV/BV_node.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/OBBRSS.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/OBBRSS.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/BV_splitter.h b/include/coal/serialization/BV_splitter.h index 1cd380550..9f6d7e98f 100644 --- a/include/coal/serialization/BV_splitter.h +++ b/include/coal/serialization/BV_splitter.h @@ -5,9 +5,9 @@ #ifndef COAL_SERIALIZATION_BV_SPLITTER_H #define COAL_SERIALIZATION_BV_SPLITTER_H -#include "hpp/fcl/internal/BV_splitter.h" +#include "coal/internal/BV_splitter.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/OBB.h b/include/coal/serialization/OBB.h index 0a584b350..298ec15a6 100644 --- a/include/coal/serialization/OBB.h +++ b/include/coal/serialization/OBB.h @@ -5,9 +5,9 @@ #ifndef COAL_SERIALIZATION_OBB_H #define COAL_SERIALIZATION_OBB_H -#include "hpp/fcl/BV/OBB.h" +#include "coal/BV/OBB.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/OBBRSS.h b/include/coal/serialization/OBBRSS.h index 2e769d9d2..990587c7f 100644 --- a/include/coal/serialization/OBBRSS.h +++ b/include/coal/serialization/OBBRSS.h @@ -5,11 +5,11 @@ #ifndef COAL_SERIALIZATION_OBBRSS_H #define COAL_SERIALIZATION_OBBRSS_H -#include "hpp/fcl/BV/OBBRSS.h" +#include "coal/BV/OBBRSS.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/OBB.h" -#include "hpp/fcl/serialization/RSS.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/OBB.h" +#include "coal/serialization/RSS.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/RSS.h b/include/coal/serialization/RSS.h index 06b586993..0e317c1f9 100644 --- a/include/coal/serialization/RSS.h +++ b/include/coal/serialization/RSS.h @@ -5,9 +5,9 @@ #ifndef COAL_SERIALIZATION_RSS_H #define COAL_SERIALIZATION_RSS_H -#include "hpp/fcl/BV/RSS.h" +#include "coal/BV/RSS.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/archive.h b/include/coal/serialization/archive.h index 5063e2c3e..428c7ee36 100644 --- a/include/coal/serialization/archive.h +++ b/include/coal/serialization/archive.h @@ -7,7 +7,7 @@ #ifndef COAL_SERIALIZATION_ARCHIVE_H #define COAL_SERIALIZATION_ARCHIVE_H -#include "hpp/fcl/fwd.hh" +#include "coal/fwd.hh" #include #include diff --git a/include/coal/serialization/collision_data.h b/include/coal/serialization/collision_data.h index b1a15ab4e..829fb5446 100644 --- a/include/coal/serialization/collision_data.h +++ b/include/coal/serialization/collision_data.h @@ -5,8 +5,8 @@ #ifndef COAL_SERIALIZATION_COLLISION_DATA_H #define COAL_SERIALIZATION_COLLISION_DATA_H -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/collision_data.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/collision_object.h b/include/coal/serialization/collision_object.h index e1c26582e..6d8a91c04 100644 --- a/include/coal/serialization/collision_object.h +++ b/include/coal/serialization/collision_object.h @@ -5,10 +5,10 @@ #ifndef COAL_SERIALIZATION_COLLISION_OBJECT_H #define COAL_SERIALIZATION_COLLISION_OBJECT_H -#include "hpp/fcl/collision_object.h" +#include "coal/collision_object.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/AABB.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/AABB.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/contact_patch.h b/include/coal/serialization/contact_patch.h index e9024e157..cdad60478 100644 --- a/include/coal/serialization/contact_patch.h +++ b/include/coal/serialization/contact_patch.h @@ -5,9 +5,9 @@ #ifndef COAL_SERIALIZATION_CONTACT_PATCH_H #define COAL_SERIALIZATION_CONTACT_PATCH_H -#include "hpp/fcl/collision_data.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/transform.h" +#include "coal/collision_data.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/transform.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/convex.h b/include/coal/serialization/convex.h index 390007e2d..ea5bb59ac 100644 --- a/include/coal/serialization/convex.h +++ b/include/coal/serialization/convex.h @@ -5,13 +5,13 @@ #ifndef COAL_SERIALIZATION_CONVEX_H #define COAL_SERIALIZATION_CONVEX_H -#include "hpp/fcl/shape/convex.h" +#include "coal/shape/convex.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/geometric_shapes.h" -#include "hpp/fcl/serialization/memory.h" -#include "hpp/fcl/serialization/triangle.h" -#include "hpp/fcl/serialization/quadrilateral.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/memory.h" +#include "coal/serialization/triangle.h" +#include "coal/serialization/quadrilateral.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/fwd.h b/include/coal/serialization/fwd.h index 6657898e7..04fe25ad5 100644 --- a/include/coal/serialization/fwd.h +++ b/include/coal/serialization/fwd.h @@ -18,8 +18,8 @@ #include #include -#include "hpp/fcl/fwd.hh" -#include "hpp/fcl/serialization/eigen.h" +#include "coal/fwd.h" +#include "coal/serialization/eigen.h" #define HPP_FCL_SERIALIZATION_SPLIT(Type) \ template \ diff --git a/include/coal/serialization/geometric_shapes.h b/include/coal/serialization/geometric_shapes.h index 7edd15600..18f0753b3 100644 --- a/include/coal/serialization/geometric_shapes.h +++ b/include/coal/serialization/geometric_shapes.h @@ -5,9 +5,9 @@ #ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H #define COAL_SERIALIZATION_GEOMETRIC_SHAPES_H -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/collision_object.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/collision_object.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/hfield.h b/include/coal/serialization/hfield.h index 544de9d2e..682f7d2c5 100644 --- a/include/coal/serialization/hfield.h +++ b/include/coal/serialization/hfield.h @@ -5,10 +5,10 @@ #ifndef COAL_SERIALIZATION_HFIELD_H #define COAL_SERIALIZATION_HFIELD_H -#include "hpp/fcl/hfield.h" +#include "coal/hfield.h" -#include "hpp/fcl/serialization/fwd.h" -#include "hpp/fcl/serialization/OBBRSS.h" +#include "coal/serialization/fwd.h" +#include "coal/serialization/OBBRSS.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/kDOP.h b/include/coal/serialization/kDOP.h index 954df2993..f4cf992c6 100644 --- a/include/coal/serialization/kDOP.h +++ b/include/coal/serialization/kDOP.h @@ -5,9 +5,9 @@ #ifndef COAL_SERIALIZATION_kDOP_H #define COAL_SERIALIZATION_kDOP_H -#include "hpp/fcl/BV/kDOP.h" +#include "coal/BV/kDOP.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/kIOS.h b/include/coal/serialization/kIOS.h index 27a0e5a69..2a024e056 100644 --- a/include/coal/serialization/kIOS.h +++ b/include/coal/serialization/kIOS.h @@ -5,10 +5,10 @@ #ifndef COAL_SERIALIZATION_kIOS_H #define COAL_SERIALIZATION_kIOS_H -#include "hpp/fcl/BV/kIOS.h" +#include "coal/BV/kIOS.h" -#include "hpp/fcl/serialization/OBB.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/serialization/OBB.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/octree.h b/include/coal/serialization/octree.h index 42a8afcdc..17edf12ad 100644 --- a/include/coal/serialization/octree.h +++ b/include/coal/serialization/octree.h @@ -10,8 +10,8 @@ #include -#include "hpp/fcl/octree.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/octree.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/quadrilateral.h b/include/coal/serialization/quadrilateral.h index eb2792e98..b805f5bde 100644 --- a/include/coal/serialization/quadrilateral.h +++ b/include/coal/serialization/quadrilateral.h @@ -5,8 +5,8 @@ #ifndef COAL_SERIALIZATION_QUADRILATERAL_H #define COAL_SERIALIZATION_QUADRILATERAL_H -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/data_types.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/serializer.h b/include/coal/serialization/serializer.h index 8990cc3fa..daef9fc64 100644 --- a/include/coal/serialization/serializer.h +++ b/include/coal/serialization/serializer.h @@ -5,7 +5,7 @@ #ifndef COAL_SERIALIZATION_SERIALIZER_H #define COAL_SERIALIZATION_SERIALIZER_H -#include "hpp/fcl/serialization/archive.h" +#include "coal/serialization/archive.h" namespace coal { namespace serialization { diff --git a/include/coal/serialization/transform.h b/include/coal/serialization/transform.h index 0f5c11e1f..e2905e999 100644 --- a/include/coal/serialization/transform.h +++ b/include/coal/serialization/transform.h @@ -5,8 +5,8 @@ #ifndef COAL_SERIALIZATION_TRANSFORM_H #define COAL_SERIALIZATION_TRANSFORM_H -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/math/transform.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/serialization/triangle.h b/include/coal/serialization/triangle.h index 2ca478543..052706be5 100644 --- a/include/coal/serialization/triangle.h +++ b/include/coal/serialization/triangle.h @@ -5,8 +5,8 @@ #ifndef COAL_SERIALIZATION_TRIANGLE_H #define COAL_SERIALIZATION_TRIANGLE_H -#include "hpp/fcl/data_types.h" -#include "hpp/fcl/serialization/fwd.h" +#include "coal/data_types.h" +#include "coal/serialization/fwd.h" namespace boost { namespace serialization { diff --git a/include/coal/shape/convex.h b/include/coal/shape/convex.h index fc2f24814..067160ad6 100644 --- a/include/coal/shape/convex.h +++ b/include/coal/shape/convex.h @@ -38,7 +38,7 @@ #ifndef COAL_SHAPE_CONVEX_H #define COAL_SHAPE_CONVEX_H -#include "hpp/fcl/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes.h" namespace coal { @@ -107,6 +107,6 @@ class Convex : public ConvexBase { } // namespace coal -#include "hpp/fcl/shape/details/convex.hxx" +#include "coal/shape/details/convex.hxx" #endif diff --git a/include/coal/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h index c6d51bc7a..0dd176265 100644 --- a/include/coal/shape/geometric_shape_to_BVH_model.h +++ b/include/coal/shape/geometric_shape_to_BVH_model.h @@ -38,8 +38,8 @@ #ifndef COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H #define COAL_GEOMETRIC_SHAPE_TO_BVH_MODEL_H -#include -#include +#include "coal/shape/geometric_shapes.h" +#include "coal/BVH/BVH_model.h" #include namespace coal { diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h index b0c1481f9..4a639e868 100644 --- a/include/coal/shape/geometric_shapes.h +++ b/include/coal/shape/geometric_shapes.h @@ -43,8 +43,8 @@ #include -#include "hpp/fcl/collision_object.h" -#include "hpp/fcl/data_types.h" +#include "coal/collision_object.h" +#include "coal/data_types.h" #ifdef HPP_FCL_HAS_QHULL namespace orgQhull { @@ -55,7 +55,7 @@ class Qhull; namespace coal { /// @brief Base class for all basic geometric shapes -class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { +class COAL_DLLAPI ShapeBase : public CollisionGeometry { public: ShapeBase() {} @@ -105,7 +105,7 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { /// @{ /// @brief Triangle stores the points instead of only indices of points -class HPP_FCL_DLLAPI TriangleP : public ShapeBase { +class COAL_DLLAPI TriangleP : public ShapeBase { public: TriangleP() {}; @@ -160,7 +160,7 @@ class HPP_FCL_DLLAPI TriangleP : public ShapeBase { }; /// @brief Center at zero point, axis aligned box -class HPP_FCL_DLLAPI Box : public ShapeBase { +class COAL_DLLAPI Box : public ShapeBase { public: Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {} @@ -234,7 +234,7 @@ class HPP_FCL_DLLAPI Box : public ShapeBase { }; /// @brief Center at zero point sphere -class HPP_FCL_DLLAPI Sphere : public ShapeBase { +class COAL_DLLAPI Sphere : public ShapeBase { public: /// @brief Default constructor Sphere() {} @@ -299,7 +299,7 @@ class HPP_FCL_DLLAPI Sphere : public ShapeBase { }; /// @brief Ellipsoid centered at point zero -class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { +class COAL_DLLAPI Ellipsoid : public ShapeBase { public: /// @brief Default constructor Ellipsoid() {} @@ -377,7 +377,7 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { /// It is \f$ { x~\in~\mathbb{R}^3, d(x, AB) \leq radius } \f$ /// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule /// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$. -class HPP_FCL_DLLAPI Capsule : public ShapeBase { +class COAL_DLLAPI Capsule : public ShapeBase { public: /// @brief Default constructor Capsule() {} @@ -461,7 +461,7 @@ class HPP_FCL_DLLAPI Capsule : public ShapeBase { /// @brief Cone /// The base of the cone is at \f$ z = - halfLength \f$ and the top is at /// \f$ z = halfLength \f$. -class HPP_FCL_DLLAPI Cone : public ShapeBase { +class COAL_DLLAPI Cone : public ShapeBase { public: /// @brief Default constructor Cone() {} @@ -551,7 +551,7 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase { /// @brief Cylinder along Z axis. /// The cylinder is defined at its centroid. -class HPP_FCL_DLLAPI Cylinder : public ShapeBase { +class COAL_DLLAPI Cylinder : public ShapeBase { public: /// @brief Default constructor Cylinder() {} @@ -634,7 +634,7 @@ class HPP_FCL_DLLAPI Cylinder : public ShapeBase { /// @brief Base for convex polytope. /// @note Inherited classes are responsible for filling ConvexBase::neighbors; -class HPP_FCL_DLLAPI ConvexBase : public ShapeBase { +class COAL_DLLAPI ConvexBase : public ShapeBase { public: /// @brief Build a convex hull based on Qhull library /// and store the vertices and optionally the triangles @@ -653,7 +653,7 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase { const char* qhullCommand = NULL); // TODO(louis): put this method in private sometime in the future. - HPP_FCL_DEPRECATED static ConvexBase* convexHull( + COAL_DEPRECATED static ConvexBase* convexHull( const Vec3f* points, unsigned int num_points, bool keepTriangles, const char* qhullCommand = NULL); @@ -676,7 +676,7 @@ class HPP_FCL_DLLAPI ConvexBase : public ShapeBase { void buildDoubleDescription(); #endif - struct HPP_FCL_DLLAPI Neighbors { + struct COAL_DLLAPI Neighbors { unsigned char count_; unsigned int* n_; @@ -881,7 +881,7 @@ class Convex; /// are outside the half space. /// Note: prefer using a Halfspace instead of a Plane if possible, it has better /// behavior w.r.t. collision detection algorithms. -class HPP_FCL_DLLAPI Halfspace : public ShapeBase { +class COAL_DLLAPI Halfspace : public ShapeBase { public: /// @brief Construct a half space with normal direction and offset Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { @@ -972,7 +972,7 @@ class HPP_FCL_DLLAPI Halfspace : public ShapeBase { /// A plane can be viewed as two half spaces; it has no priviledged direction. /// Note: prefer using a Halfspace instead of a Plane if possible, it has better /// behavior w.r.t. collision detection algorithms. -class HPP_FCL_DLLAPI Plane : public ShapeBase { +class COAL_DLLAPI Plane : public ShapeBase { public: /// @brief Construct a plane with normal direction and offset Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { diff --git a/include/coal/shape/geometric_shapes_traits.h b/include/coal/shape/geometric_shapes_traits.h index 899711315..fcc769e66 100644 --- a/include/coal/shape/geometric_shapes_traits.h +++ b/include/coal/shape/geometric_shapes_traits.h @@ -37,7 +37,7 @@ #ifndef COAL_GEOMETRIC_SHAPES_TRAITS_H #define COAL_GEOMETRIC_SHAPES_TRAITS_H -#include +#include "coal/shape/geometric_shapes.h" namespace coal { diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h index 05ce509c1..3773afda0 100644 --- a/include/coal/shape/geometric_shapes_utility.h +++ b/include/coal/shape/geometric_shapes_utility.h @@ -39,9 +39,9 @@ #define COAL_GEOMETRIC_SHAPES_UTILITY_H #include -#include -#include -#include +#include "coal/shape/geometric_shapes.h" +#include "coal/BV/BV.h" +#include "coal/internal/BV_fitter.h" namespace coal { @@ -49,22 +49,22 @@ namespace coal { namespace details { /// @brief get the vertices of some convex shape which can bound the given shape /// in a specific configuration -HPP_FCL_DLLAPI std::vector getBoundVertices(const Box& box, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Cone& cone, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, - const Transform3f& tf); -HPP_FCL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, - const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const Box& box, + const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, + const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, + const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, + const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const Cone& cone, + const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, + const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, + const Transform3f& tf); +COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, + const Transform3f& tf); } // namespace details /// @endcond @@ -81,180 +81,179 @@ inline void computeBV(const S& s, const Transform3f& tf, BV& bv) { } template <> -HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, + AABB& bv); + +template <> +COAL_DLLAPI void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Sphere& s, +COAL_DLLAPI void computeBV(const Ellipsoid& e, const Transform3f& tf, AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Ellipsoid& e, - const Transform3f& tf, AABB& bv); +COAL_DLLAPI void computeBV(const Capsule& s, + const Transform3f& tf, AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Capsule& s, - const Transform3f& tf, AABB& bv); +COAL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, + AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, - AABB& bv); +COAL_DLLAPI void computeBV(const Cylinder& s, + const Transform3f& tf, AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Cylinder& s, - const Transform3f& tf, AABB& bv); +COAL_DLLAPI void computeBV(const ConvexBase& s, + const Transform3f& tf, AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, - const Transform3f& tf, - AABB& bv); +COAL_DLLAPI void computeBV(const TriangleP& s, + const Transform3f& tf, AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const TriangleP& s, - const Transform3f& tf, AABB& bv); +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, AABB& bv); +COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, + AABB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, - const Transform3f& tf, AABB& bv); +COAL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, + OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Sphere& s, - const Transform3f& tf, OBB& bv); +COAL_DLLAPI void computeBV(const Capsule& s, + const Transform3f& tf, OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Capsule& s, - const Transform3f& tf, OBB& bv); +COAL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, + OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, - OBB& bv); +COAL_DLLAPI void computeBV(const Cylinder& s, + const Transform3f& tf, OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Cylinder& s, - const Transform3f& tf, OBB& bv); - -template <> -HPP_FCL_DLLAPI void computeBV(const ConvexBase& s, - const Transform3f& tf, OBB& bv); +COAL_DLLAPI void computeBV(const ConvexBase& s, + const Transform3f& tf, OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, OBB& bv); +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, RSS& bv); +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, RSS& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, - OBBRSS& bv); +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, + OBBRSS& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, kIOS& bv); +COAL_DLLAPI void computeBV(const Halfspace& s, + const Transform3f& tf, kIOS& bv); template <> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, - KDOP<16>& bv); +COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3f& tf, + KDOP<16>& bv); template <> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, - KDOP<18>& bv); +COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3f& tf, + KDOP<18>& bv); template <> -HPP_FCL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, - KDOP<24>& bv); +COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, + const Transform3f& tf, + KDOP<24>& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, - OBB& bv); +COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, + OBB& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, - RSS& bv); +COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, + RSS& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, - const Transform3f& tf, OBBRSS& bv); +COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, + OBBRSS& bv); template <> -HPP_FCL_DLLAPI void computeBV(const Plane& s, - const Transform3f& tf, kIOS& bv); +COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, + kIOS& bv); template <> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, - KDOP<16>& bv); +COAL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3f& tf, + KDOP<16>& bv); template <> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, - KDOP<18>& bv); +COAL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3f& tf, + KDOP<18>& bv); template <> -HPP_FCL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, - KDOP<24>& bv); +COAL_DLLAPI void computeBV, Plane>(const Plane& s, + const Transform3f& tf, + KDOP<24>& bv); /// @brief construct a box shape (with a configuration) from a given bounding /// volume -HPP_FCL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const AABB& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const AABB& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const OBB& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const kIOS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const RSS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, + Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, + Box& box, Transform3f& tf); -HPP_FCL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3f& tf); +COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3f& tf); -HPP_FCL_DLLAPI Plane transform(const Plane& a, const Transform3f& tf); +COAL_DLLAPI Plane transform(const Plane& a, const Transform3f& tf); -HPP_FCL_DLLAPI std::array transformToHalfspaces( +COAL_DLLAPI std::array transformToHalfspaces( const Plane& a, const Transform3f& tf); } // namespace coal diff --git a/include/coal/timings.h b/include/coal/timings.h index 5316a832a..9b361d852 100644 --- a/include/coal/timings.h +++ b/include/coal/timings.h @@ -5,7 +5,7 @@ #ifndef COAL_TIMINGS_FWD_H #define COAL_TIMINGS_FWD_H -#include "hpp/fcl/fwd.hh" +#include "coal/fwd.hh" #ifdef HPP_FCL_WITH_CXX11_SUPPORT #include @@ -28,7 +28,7 @@ struct CPUTimes { /// using the modern std::chrono library. /// Importantly, this class will only have an effect for C++11 and more. /// -struct HPP_FCL_DLLAPI Timer { +struct COAL_DLLAPI Timer { #ifdef HPP_FCL_WITH_CXX11_SUPPORT typedef std::chrono::steady_clock clock_type; typedef clock_type::duration duration_type; diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index dd99421ad..edcffde1f 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -32,17 +32,17 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include +#include "coal/fwd.hh" #include "../fcl.hh" #include "../utils/std-pair.hh" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_spatialhash.h" HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index c5d64f37f..998894eb0 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -37,8 +37,8 @@ #include -#include -#include +#include "coal/fwd.hh" +#include "coal/broadphase/broadphase_callbacks.h" #include "../fcl.hh" diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index 70578b439..ba37eaf0b 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -37,9 +37,9 @@ #include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/default_broadphase_callbacks.h" #include "../fcl.hh" diff --git a/python/broadphase/fwd.hh b/python/broadphase/fwd.hh index c2cc67082..1367cc065 100644 --- a/python/broadphase/fwd.hh +++ b/python/broadphase/fwd.hh @@ -5,7 +5,7 @@ #ifndef COAL_PYTHON_BROADPHASE_FWD_HH #define COAL_PYTHON_BROADPHASE_FWD_HH -#include "hppfcl/fwd.hh" +#include "coal/fwd.hh" namespace coal { namespace python { diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index cec7ea32b..acc14597d 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -38,18 +38,18 @@ #include "fcl.hh" #include "deprecation.hh" -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/convex.h" +#include "coal/BVH/BVH_model.h" +#include "coal/hfield.h" + +#include "coal/serialization/memory.h" +#include "coal/serialization/AABB.h" +#include "coal/serialization/BVH_model.h" +#include "coal/serialization/hfield.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" #include "pickle.hh" #include "serializable.hh" @@ -57,8 +57,8 @@ #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC // FIXME for a reason I do not understand, doxygen fails to understand that // BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h -#include -#include +#include "coal/internal/BV_splitter.h" +#include "coal/broadphase/detail/hierarchy_tree.h" #include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h" #include "doxygen_autodoc/hpp/fcl/BV/AABB.h" diff --git a/python/collision.cc b/python/collision.cc index a3a47deb4..7bbb5a108 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -34,12 +34,12 @@ #include -#include -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#include -#include -HPP_FCL_COMPILER_DIAGNOSTIC_POP +#include "coal/fwd.hh" +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#include "coal/collision.h" +#include "coal/serialization/collision_data.h" +COAL_COMPILER_DIAGNOSTIC_POP #include "fcl.hh" #include "deprecation.hh" diff --git a/python/contact_patch.cc b/python/contact_patch.cc index d09c9f246..42d26835b 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -34,9 +34,9 @@ #include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/contact_patch.h" +#include "coal/serialization/collision_data.h" #include "fcl.hh" #include "deprecation.hh" diff --git a/python/distance.cc b/python/distance.cc index 21209eae4..4ae1b26d1 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -36,13 +36,14 @@ #include "fcl.hh" -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#include -#include -#include +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#include "coal/fwd.hh" +#include "coal/distance.h" +#include "coal/serialization/collision_data.h" + #include "deprecation.hh" -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP #include "serializable.hh" diff --git a/python/fcl.cc b/python/fcl.cc index d0fea53b8..c0c6e5489 100644 --- a/python/fcl.cc +++ b/python/fcl.cc @@ -36,13 +36,13 @@ #include "fcl.hh" -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/shape/geometric_shapes.h" +#include "coal/BVH/BVH_model.h" -#include +#include "coal/mesh_loader/loader.h" -#include +#include "coal/collision.h" #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/hpp/fcl/mesh_loader/loader.h" diff --git a/python/fwd.hh b/python/fwd.hh index 6d0cbff2f..018ec0e6a 100644 --- a/python/fwd.hh +++ b/python/fwd.hh @@ -5,7 +5,7 @@ #ifndef COAL_PYTHON_FWD_HH #define COAL_PYTHON_FWD_HH -#include +#include "coal/fwd.hh" #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC namespace doxygen { using hpp::fcl::shared_ptr; diff --git a/python/gjk.cc b/python/gjk.cc index 1b4f7765a..d1b13dbbf 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -36,8 +36,8 @@ #include "fcl.hh" -#include -#include +#include "coal/fwd.hh" +#include "coal/narrowphase/gjk.h" #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" diff --git a/python/math.cc b/python/math.cc index 5e1c4c698..7d372c731 100644 --- a/python/math.cc +++ b/python/math.cc @@ -35,9 +35,9 @@ #include #include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/math/transform.h" +#include "coal/serialization/transform.h" #include "fcl.hh" #include "pickle.hh" diff --git a/python/octree.cc b/python/octree.cc index b6bdc376a..9fa4bf660 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -3,8 +3,8 @@ #include -#include -#include +#include "coal/fwd.hh" +#include "coal/octree.h" #ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" diff --git a/python/serializable.hh b/python/serializable.hh index 87c1690fe..2ab337fb5 100644 --- a/python/serializable.hh +++ b/python/serializable.hh @@ -9,8 +9,8 @@ #include -#include "hpp/fcl/serialization/archive.h" -#include "hpp/fcl/serialization/serializer.h" +#include "coal/serialization/archive.h" +#include "coal/serialization/serializer.h" namespace coal { namespace python { diff --git a/python/version.cc b/python/version.cc index 4bc0eea93..f0c2e3dd1 100644 --- a/python/version.cc +++ b/python/version.cc @@ -2,28 +2,28 @@ // Copyright (c) 2019-2023 CNRS INRIA // -#include "hpp/fcl/config.hh" +#include "coal/config.hh" #include "fcl.hh" #include namespace bp = boost::python; inline bool checkVersionAtLeast(int major, int minor, int patch) { - return HPP_FCL_VERSION_AT_LEAST(major, minor, patch); + return COAL_VERSION_AT_LEAST(major, minor, patch); } inline bool checkVersionAtMost(int major, int minor, int patch) { - return HPP_FCL_VERSION_AT_MOST(major, minor, patch); + return COAL_VERSION_AT_MOST(major, minor, patch); } void exposeVersion() { // Define release numbers of the current hpp-fcl version. bp::scope().attr("__version__") = - BOOST_PP_STRINGIZE(HPP_FCL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(HPP_FCL_PATCH_VERSION); - bp::scope().attr("__raw_version__") = HPP_FCL_VERSION; - bp::scope().attr("HPP_FCL_MAJOR_VERSION") = HPP_FCL_MAJOR_VERSION; - bp::scope().attr("HPP_FCL_MINOR_VERSION") = HPP_FCL_MINOR_VERSION; - bp::scope().attr("HPP_FCL_PATCH_VERSION") = HPP_FCL_PATCH_VERSION; + BOOST_PP_STRINGIZE(COAL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_PATCH_VERSION); + bp::scope().attr("__raw_version__") = COAL_VERSION; + bp::scope().attr("COAL_MAJOR_VERSION") = COAL_MAJOR_VERSION; + bp::scope().attr("COAL_MINOR_VERSION") = COAL_MINOR_VERSION; + bp::scope().attr("COAL_PATCH_VERSION") = COAL_PATCH_VERSION; #if HPP_FCL_HAS_QHULL bp::scope().attr("WITH_QHULL") = true; @@ -39,11 +39,11 @@ void exposeVersion() { bp::def("checkVersionAtLeast", &checkVersionAtLeast, bp::args("major", "minor", "patch"), - "Checks if the current version of hpp-fcl is at least" + "Checks if the current version of coal is at least" " the version provided by the input arguments."); bp::def("checkVersionAtMost", &checkVersionAtMost, bp::args("major", "minor", "patch"), - "Checks if the current version of hpp-fcl is at most" + "Checks if the current version of coal is at most" " the version provided by the input arguments."); } diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index 40a352375..c7b427a75 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -35,11 +35,11 @@ /** \author Jia Pan */ -#include -#include +#include "coal/BV/AABB.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/collision_data.h" #include -#include namespace coal { diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 44036f7f3..8fee27530 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -35,11 +35,11 @@ /** \author Jia Pan, Florent Lamiraux */ -#include -#include -#include -#include -#include +#include "coal/BV/OBB.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/math/transform.h" +#include "coal/collision_data.h" +#include "coal/internal/tools.h" #include #include @@ -310,7 +310,7 @@ inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, } template -struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi { +struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { static inline bool run(int ia, int ja, int ka, const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const Matrix3f& Bf, const FCL_REAL& breakDistance2, diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp index d74381f9c..18a0c83f7 100644 --- a/src/BV/OBBRSS.cpp +++ b/src/BV/OBBRSS.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#include +#include "coal/BV/OBBRSS.h" namespace coal { diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index f793d2fad..ead66ce7a 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -35,10 +35,10 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/BV/RSS.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/internal/tools.h" +#include "coal/collision_data.h" #include diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp index 6d9b4c9f6..80e94990e 100644 --- a/src/BV/kDOP.cpp +++ b/src/BV/kDOP.cpp @@ -35,12 +35,12 @@ /** \author Jia Pan */ -#include +#include "coal/collision_data.h" +#include "coal/BV/kDOP.h" + #include #include -#include - namespace coal { /// @brief Find the smaller and larger one of two values diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp index 48bb08024..2b50f4b65 100644 --- a/src/BV/kIOS.cpp +++ b/src/BV/kIOS.cpp @@ -35,9 +35,9 @@ /** \author Jia Pan */ -#include -#include -#include +#include "coal/BV/kIOS.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/math/transform.h" #include #include diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp index 30271906f..cd45cabf6 100644 --- a/src/BVH/BVH_model.cpp +++ b/src/BVH/BVH_model.cpp @@ -36,17 +36,17 @@ /** \author Jia Pan */ -#include "hpp/fcl/BV/BV_node.h" -#include +#include "coal/BV/BV_node.h" +#include "coal/BVH/BVH_model.h" -#include -#include +#include "coal/BV/BV.h" +#include "coal/shape/convex.h" -#include -#include +#include "coal/internal/BV_splitter.h" +#include "coal/internal/BV_fitter.h" -#include -#include +#include +#include namespace coal { diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp index 638acf3e3..2072921b2 100644 --- a/src/BVH/BVH_utility.cpp +++ b/src/BVH/BVH_utility.cpp @@ -35,10 +35,10 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/BVH/BVH_utility.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/shape_shape_func.h" namespace coal { diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp index 86b0bc3fa..3c0a80bf0 100644 --- a/src/BVH/BV_fitter.cpp +++ b/src/BVH/BV_fitter.cpp @@ -35,10 +35,11 @@ /** \author Jia Pan */ -#include -#include +#include "coal/internal/BV_fitter.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/internal/tools.h" + #include -#include namespace coal { diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp index 7e4dbbc17..1a78e332c 100644 --- a/src/BVH/BV_splitter.cpp +++ b/src/BVH/BV_splitter.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#include +#include "coal/internal/BV_splitter.h" namespace coal { diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 74e7fb304..cb3639f4d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -179,9 +179,9 @@ SET(PROJECT_HEADERS_FULL_PATH) FOREACH(header ${${PROJECT_NAME}_HEADERS}) LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_SOURCE_DIR}/${header}) ENDFOREACH() -LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/config.hh) -LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/deprecated.hh) -LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/hpp/fcl/warning.hh) +LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/config.hh) +LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/deprecated.hh) +LIST(APPEND PROJECT_HEADERS_FULL_PATH ${PROJECT_BINARY_DIR}/include/coal/warning.hh) add_library(${LIBRARY_NAME} SHARED ${PROJECT_HEADERS_FULL_PATH} diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index 71000e45d..124f8c671 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -35,7 +35,7 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_SSaP.h" namespace coal { @@ -64,7 +64,7 @@ struct SortByZLow { }; /** @brief Dummy collision object with a point AABB */ -class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject { +class COAL_DLLAPI DummyCollisionObject : public CollisionObject { public: DummyCollisionObject(const AABB& aabb_) : CollisionObject(shared_ptr()) { diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 59871347e..61f039d53 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -35,7 +35,7 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SaP.h" namespace coal { diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index 0eb81752f..f1a241d71 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -35,7 +35,7 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_bruteforce.h" #include #include diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 2c5c340a2..59e1a854e 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -35,7 +35,7 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_collision_manager.h" +#include "coal/broadphase/broadphase_collision_manager.h" namespace coal { diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 7fceeb57c..efc82c6da 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -35,16 +35,16 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" - -#include +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" #ifdef HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" +#include "coal/octree.h" #endif -#include "hpp/fcl/BV/BV.h" -#include "hpp/fcl/shape/geometric_shapes_utility.h" +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes_utility.h" + +#include namespace coal { namespace detail { diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index ced67ae1a..6c1e65bd7 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -35,10 +35,10 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #ifdef HPP_FCL_HAVE_OCTOMAP -#include "hpp/fcl/octree.h" +#include "coal/octree.h" #endif namespace coal { namespace detail { @@ -262,7 +262,7 @@ bool collisionRecurse( } //============================================================================== -inline HPP_FCL_DLLAPI bool collisionRecurse( +inline COAL_DLLAPI bool collisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index b9cc6f9db..9107335dc 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -35,7 +35,7 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_interval_tree.h" namespace coal { diff --git a/src/broadphase/default_broadphase_callbacks.cpp b/src/broadphase/default_broadphase_callbacks.cpp index 55db074da..222e77b85 100644 --- a/src/broadphase/default_broadphase_callbacks.cpp +++ b/src/broadphase/default_broadphase_callbacks.cpp @@ -34,7 +34,7 @@ /** @author Sean Curtis (sean@tri.global) */ -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" +#include "coal/broadphase/default_broadphase_callbacks.h" #include namespace coal { diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp index 88b1119cc..63fdd8d76 100644 --- a/src/broadphase/detail/interval_tree.cpp +++ b/src/broadphase/detail/interval_tree.cpp @@ -38,7 +38,7 @@ #ifndef COAL_INTERVAL_TREE_INL_H #define COAL_INTERVAL_TREE_INL_H -#include "hpp/fcl/broadphase/detail/interval_tree.h" +#include "coal/broadphase/detail/interval_tree.h" #include diff --git a/src/broadphase/detail/interval_tree_node.cpp b/src/broadphase/detail/interval_tree_node.cpp index afb55734e..51a1a457a 100644 --- a/src/broadphase/detail/interval_tree_node.cpp +++ b/src/broadphase/detail/interval_tree_node.cpp @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H #define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H -#include "hpp/fcl/broadphase/detail/interval_tree_node.h" +#include "coal/broadphase/detail/interval_tree_node.h" #include #include diff --git a/src/broadphase/detail/morton-inl.h b/src/broadphase/detail/morton-inl.h index 80354a76d..4f27aef91 100644 --- a/src/broadphase/detail/morton-inl.h +++ b/src/broadphase/detail/morton-inl.h @@ -39,7 +39,7 @@ #ifndef COAL_MORTON_INL_H #define COAL_MORTON_INL_H -#include "hpp/fcl/broadphase/detail/morton.h" +#include "coal/broadphase/detail/morton.h" namespace coal { /// @cond IGNORE namespace detail { diff --git a/src/broadphase/detail/morton.cpp b/src/broadphase/detail/morton.cpp index 83fc06ced..17e554f0e 100644 --- a/src/broadphase/detail/morton.cpp +++ b/src/broadphase/detail/morton.cpp @@ -36,7 +36,7 @@ /** @author Jia Pan */ -#include "hpp/fcl/broadphase/detail/morton-inl.h" +#include "coal/broadphase/detail/morton-inl.h" namespace coal { diff --git a/src/broadphase/detail/simple_interval.cpp b/src/broadphase/detail/simple_interval.cpp index 4625c31e6..95075cbc1 100644 --- a/src/broadphase/detail/simple_interval.cpp +++ b/src/broadphase/detail/simple_interval.cpp @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H #define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H -#include "hpp/fcl/broadphase/detail/simple_interval.h" +#include "coal/broadphase/detail/simple_interval.h" #include namespace coal { diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index 46afc7d32..ba45f0e9b 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -38,7 +38,7 @@ #ifndef COAL_BROADPHASE_SPATIALHASH_INL_H #define COAL_BROADPHASE_SPATIALHASH_INL_H -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/broadphase/detail/spatial_hash.h" #include namespace coal { diff --git a/src/collision.cpp b/src/collision.cpp index 3b2f5e41b..0cc76d8e9 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -35,10 +35,10 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/collision.h" +#include "coal/collision_utility.h" +#include "coal/collision_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" namespace coal { diff --git a/src/collision_data.cpp b/src/collision_data.cpp index 51f0083a6..7561ba831 100644 --- a/src/collision_data.cpp +++ b/src/collision_data.cpp @@ -36,7 +36,7 @@ /** \author Jia Pan */ -#include +#include "coal/collision_data.h" namespace coal { diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 11347a048..6a23dd856 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -35,13 +35,13 @@ /** \author Jia Pan */ -#include +#include "coal/collision_func_matrix.h" -#include +#include "coal/internal/traversal_node_setup.h" #include <../src/collision_node.h> -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/internal/shape_shape_func.h" +#include "coal/shape/geometric_shapes_traits.h" #include <../src/traits_traversal.h> namespace coal { @@ -98,7 +98,7 @@ BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS); /// - 0 if the query should be made with non-aligned object frames. template ::Options> -struct HPP_FCL_LOCAL BVHShapeCollider { +struct COAL_LOCAL BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, @@ -165,7 +165,7 @@ struct HPP_FCL_LOCAL BVHShapeCollider { /// into the frame of object 2 before computing collisions. /// - 0 if the query should be made with non-aligned object frames. template -struct HPP_FCL_LOCAL HeightFieldShapeCollider { +struct COAL_LOCAL HeightFieldShapeCollider { typedef HeightField HF; static std::size_t collide(const CollisionGeometry* o1, diff --git a/src/collision_node.cpp b/src/collision_node.cpp index a5e196533..c263dc95e 100644 --- a/src/collision_node.cpp +++ b/src/collision_node.cpp @@ -36,7 +36,7 @@ /** \author Jia Pan */ #include <../src/collision_node.h> -#include +#include "coal/internal/traversal_recurse.h" namespace coal { diff --git a/src/collision_node.h b/src/collision_node.h index 9c48e8a7b..f2780cfc5 100644 --- a/src/collision_node.h +++ b/src/collision_node.h @@ -40,9 +40,9 @@ /// @cond INTERNAL -#include -#include -#include +#include "coal/BVH/BVH_front.h" +#include "coal/internal/traversal_node_base.h" +#include "coal/internal/traversal_node_bvhs.h" /// @brief collision and distance function on traversal nodes. these functions /// provide a higher level abstraction for collision functions provided in @@ -56,18 +56,18 @@ namespace coal { /// do not collide. /// @param front_list list of nodes visited by the query, can be used to /// accelerate computation -/// \todo should be HPP_FCL_LOCAL but used in unit test. -HPP_FCL_DLLAPI void collide(CollisionTraversalNodeBase* node, - const CollisionRequest& request, - CollisionResult& result, - BVHFrontList* front_list = NULL, - bool recursive = true); +/// \todo should be COAL_LOCAL but used in unit test. +COAL_DLLAPI void collide(CollisionTraversalNodeBase* node, + const CollisionRequest& request, + CollisionResult& result, + BVHFrontList* front_list = NULL, + bool recursive = true); /// @brief distance computation on distance traversal node; can use front list -/// to accelerate \todo should be HPP_FCL_LOCAL but used in unit test. -HPP_FCL_DLLAPI void distance(DistanceTraversalNodeBase* node, - BVHFrontList* front_list = NULL, - unsigned int qsize = 2); +/// to accelerate \todo should be COAL_LOCAL but used in unit test. +COAL_DLLAPI void distance(DistanceTraversalNodeBase* node, + BVHFrontList* front_list = NULL, + unsigned int qsize = 2); } // namespace coal /// @endcond diff --git a/src/collision_object.cpp b/src/collision_object.cpp index f335f7fce..a0501b146 100644 --- a/src/collision_object.cpp +++ b/src/collision_object.cpp @@ -35,7 +35,7 @@ /** \author Florent Lamiraux */ -#include +#include "coal/collision_object.h" namespace coal { bool CollisionGeometry::isUncertain() const { diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp index 932a6ecd3..831040888 100644 --- a/src/collision_utility.cpp +++ b/src/collision_utility.cpp @@ -14,9 +14,8 @@ // received a copy of the GNU Lesser General Public License along with // hpp-fcl. If not, see . -#include - -#include +#include "coal/collision_utility.h" +#include "coal/BVH/BVH_utility.h" namespace coal { namespace details { diff --git a/src/contact_patch.cpp b/src/contact_patch.cpp index c5870e190..9b6c22364 100644 --- a/src/contact_patch.cpp +++ b/src/contact_patch.cpp @@ -34,8 +34,8 @@ /** \author Louis Montaut */ -#include "hpp/fcl/contact_patch.h" -#include "hpp/fcl/collision_utility.h" +#include "coal/contact_patch.h" +#include "coal/collision_utility.h" namespace coal { diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp index d6b07bdcf..fdb16217e 100644 --- a/src/contact_patch/contact_patch_solver.cpp +++ b/src/contact_patch/contact_patch_solver.cpp @@ -34,7 +34,7 @@ /** \authors Louis Montaut */ -#include "hpp/fcl/contact_patch/contact_patch_solver.h" +#include "coal/contact_patch/contact_patch_solver.h" namespace coal { diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp index 0ab71ea5d..b1b0f90cf 100644 --- a/src/contact_patch_func_matrix.cpp +++ b/src/contact_patch_func_matrix.cpp @@ -34,11 +34,10 @@ /** \author Louis Montaut */ -#include "hpp/fcl/contact_patch_func_matrix.h" -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/internal/shape_shape_contact_patch_func.h" - -#include "hpp/fcl/BV/BV.h" +#include "coal/contact_patch_func_matrix.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/shape_shape_contact_patch_func.h" +#include "coal/BV/BV.h" namespace coal { @@ -117,7 +116,7 @@ struct BVHComputeContactPatch { } }; -HPP_FCL_LOCAL void contact_patch_function_not_implemented( +COAL_LOCAL void contact_patch_function_not_implemented( const CollisionGeometry* o1, const Transform3f& /*tf1*/, const CollisionGeometry* o2, const Transform3f& /*tf2*/, const CollisionResult& /*collision_result*/, diff --git a/src/distance.cpp b/src/distance.cpp index b22063028..e802f5872 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -35,10 +35,10 @@ /** \author Jia Pan */ -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/collision_utility.h" +#include "coal/distance_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" #include diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp index 8dffb32e4..39f8a792a 100644 --- a/src/distance/box_halfspace.cpp +++ b/src/distance/box_halfspace.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp index 159739a8b..f0c45e3fd 100644 --- a/src/distance/box_plane.cpp +++ b/src/distance/box_plane.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp index e3a8bb462..220c20eb9 100644 --- a/src/distance/box_sphere.cpp +++ b/src/distance/box_sphere.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp index 58ac989ba..c229f80e7 100644 --- a/src/distance/capsule_capsule.cpp +++ b/src/distance/capsule_capsule.cpp @@ -31,9 +31,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/shape_shape_func.h" // Note that partial specialization of template functions is not allowed. // Therefore, two implementations with the default narrow phase solvers are diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp index 294fe7041..15daa03d5 100644 --- a/src/distance/capsule_halfspace.cpp +++ b/src/distance/capsule_halfspace.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp index 24ac3e321..9644e43e7 100644 --- a/src/distance/capsule_plane.cpp +++ b/src/distance/capsule_plane.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp index 10356c4eb..39649095a 100644 --- a/src/distance/cone_halfspace.cpp +++ b/src/distance/cone_halfspace.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp index 0b95a1576..ddd863ff5 100644 --- a/src/distance/cone_plane.cpp +++ b/src/distance/cone_plane.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 552b41dd8..22fc88ad4 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -34,9 +34,9 @@ /** \author Joseph Mirabel */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp index da1752c53..e2ece56ed 100644 --- a/src/distance/convex_plane.cpp +++ b/src/distance/convex_plane.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp index 58850b15e..17b599908 100644 --- a/src/distance/cylinder_halfspace.cpp +++ b/src/distance/cylinder_halfspace.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp index 975d8389f..fd1e31b79 100644 --- a/src/distance/cylinder_plane.cpp +++ b/src/distance/cylinder_plane.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp index edd376f9f..836eb493f 100644 --- a/src/distance/ellipsoid_halfspace.cpp +++ b/src/distance/ellipsoid_halfspace.cpp @@ -34,10 +34,10 @@ /** \author Louis Montaut */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp index f37a03a2f..77d7cd2e9 100644 --- a/src/distance/ellipsoid_plane.cpp +++ b/src/distance/ellipsoid_plane.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp index eb39873e9..260b12ec0 100644 --- a/src/distance/halfspace_halfspace.cpp +++ b/src/distance/halfspace_halfspace.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp index ed595868f..096b8ea40 100644 --- a/src/distance/halfspace_plane.cpp +++ b/src/distance/halfspace_plane.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp index bab49f799..a716999e8 100644 --- a/src/distance/plane_plane.cpp +++ b/src/distance/plane_plane.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp index 05a15c39d..d02101177 100644 --- a/src/distance/sphere_capsule.cpp +++ b/src/distance/sphere_capsule.cpp @@ -33,10 +33,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp index 24bed10f1..c97bec1d5 100644 --- a/src/distance/sphere_cylinder.cpp +++ b/src/distance/sphere_cylinder.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp index 6f09b4fe8..3f3d580cb 100644 --- a/src/distance/sphere_halfspace.cpp +++ b/src/distance/sphere_halfspace.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp index 5aa1ec725..a3d5a262b 100644 --- a/src/distance/sphere_plane.cpp +++ b/src/distance/sphere_plane.cpp @@ -36,10 +36,10 @@ /** \author Florent Lamiraux */ -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index d7403e4bd..ea60685ad 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -33,10 +33,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include -#include +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/shape_shape_func.h" +#include "coal/internal/traversal_node_base.h" #include "../narrowphase/details.h" diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index 5925376cd..a74cdc8ba 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -34,9 +34,9 @@ /** \author Joseph Mirabel */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp index c77ddd648..89b60f7a9 100644 --- a/src/distance/triangle_plane.cpp +++ b/src/distance/triangle_plane.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp index f0d670ea7..aed689961 100644 --- a/src/distance/triangle_sphere.cpp +++ b/src/distance/triangle_sphere.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp index 91ac7c20b..602e9efb8 100644 --- a/src/distance/triangle_triangle.cpp +++ b/src/distance/triangle_triangle.cpp @@ -34,9 +34,9 @@ /** \author Louis Montaut */ -#include +#include "coal/shape/geometric_shapes.h" -#include +#include "coal/internal/shape_shape_func.h" #include "../narrowphase/details.h" namespace coal { diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index e9ccc50e1..7e140ba96 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -35,12 +35,12 @@ /** \author Jia Pan */ -#include +#include "coal/distance_func_matrix.h" #include <../src/collision_node.h> -#include -#include -#include +#include "coal/internal/shape_shape_func.h" +#include "coal/internal/traversal_node_setup.h" +#include "coal/internal/shape_shape_func.h" #include <../src/traits_traversal.h> namespace coal { @@ -66,7 +66,7 @@ FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, #endif -HPP_FCL_LOCAL FCL_REAL distance_function_not_implemented( +COAL_LOCAL FCL_REAL distance_function_not_implemented( const CollisionGeometry* o1, const Transform3f& /*tf1*/, const CollisionGeometry* o2, const Transform3f& /*tf2*/, const GJKSolver* /*nsolver*/, const DistanceRequest& /*request*/, @@ -83,7 +83,7 @@ HPP_FCL_LOCAL FCL_REAL distance_function_not_implemented( } template -struct HPP_FCL_LOCAL BVHShapeDistancer { +struct COAL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, @@ -129,7 +129,7 @@ FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, } // namespace details template -struct HPP_FCL_LOCAL BVHShapeDistancer { +struct COAL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, @@ -142,7 +142,7 @@ struct HPP_FCL_LOCAL BVHShapeDistancer { }; template -struct HPP_FCL_LOCAL BVHShapeDistancer { +struct COAL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, @@ -155,7 +155,7 @@ struct HPP_FCL_LOCAL BVHShapeDistancer { }; template -struct HPP_FCL_LOCAL BVHShapeDistancer { +struct COAL_LOCAL BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, @@ -168,7 +168,7 @@ struct HPP_FCL_LOCAL BVHShapeDistancer { }; template -struct HPP_FCL_LOCAL HeightFieldShapeDistancer { +struct COAL_LOCAL HeightFieldShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, diff --git a/src/hfield.cpp b/src/hfield.cpp index 2a44f01d2..af5391379 100644 --- a/src/hfield.cpp +++ b/src/hfield.cpp @@ -34,16 +34,16 @@ /** \author Justin Carpentier */ -#include +#include "coal/hfield.h" -#include -#include +#include "coal/BV/BV.h" +#include "coal/shape/convex.h" -#include -#include +#include "coal/internal/BV_splitter.h" +#include "coal/internal/BV_fitter.h" -#include -#include +#include +#include namespace coal { diff --git a/src/intersect.cpp b/src/intersect.cpp index 876b9fd7f..6feb74e18 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -35,12 +35,13 @@ /** \author Jia Pan */ -#include +#include "coal/internal/intersect.h" +#include "coal/internal/tools.h" + #include #include #include #include -#include namespace coal { diff --git a/src/math/transform.cpp b/src/math/transform.cpp index 480039ab0..5843b03ad 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#include +#include "coal/math/transform.h" namespace coal { diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp index 31bb61fa7..4efd6b7c8 100644 --- a/src/mesh_loader/assimp.cpp +++ b/src/mesh_loader/assimp.cpp @@ -35,7 +35,7 @@ #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) #define nullptr NULL #endif -#include +#include "coal/mesh_loader/assimp.h" // Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted // https://github.com/assimp/assimp/pull/2758. The next lines fixes the bug for diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp index 992b7bd16..8304b4974 100644 --- a/src/mesh_loader/loader.cpp +++ b/src/mesh_loader/loader.cpp @@ -34,16 +34,16 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include "coal/mesh_loader/loader.h" +#include "coal/mesh_loader/assimp.h" #include #ifdef HPP_FCL_HAS_OCTOMAP -#include +#include "coal/octree.h" #endif -#include +#include "coal/BV/BV.h" namespace coal { bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const { diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index 14cba9f77..df6b5e2d6 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -39,8 +39,8 @@ #ifndef COAL_SRC_NARROWPHASE_DETAILS_H #define COAL_SRC_NARROWPHASE_DETAILS_H -#include -#include +#include "coal/internal/traversal_node_setup.h" +#include "coal/narrowphase/narrowphase.h" namespace coal { namespace details { diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index ab0541488..450cc2f4b 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -36,12 +36,12 @@ /** \author Jia Pan */ -#include -#include -#include -#include -#include -#include +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/gjk.h" +#include "coal/internal/intersect.h" +#include "coal/internal/tools.h" +#include "coal/shape/geometric_shapes_traits.h" +#include "coal/narrowphase/narrowphase_defaults.h" namespace coal { diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index 5edd3dda1..636313c0c 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -36,8 +36,8 @@ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ -#include "hpp/fcl/narrowphase/minkowski_difference.h" -#include "hpp/fcl/shape/geometric_shapes_traits.h" +#include "coal/narrowphase/minkowski_difference.h" +#include "coal/shape/geometric_shapes_traits.h" namespace coal { namespace details { @@ -283,9 +283,9 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, shape0, shape1, identity, swept_sphere_radius, data); } // clang-format off -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*); // clang-format on // ============================================================================ @@ -303,20 +303,20 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { shape0, shape1, true, swept_sphere_radius, data); } // clang-format off -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); -template void HPP_FCL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); // clang-format on // ============================================================================ // clang-format off -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; +template Vec3f COAL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; +template Vec3f COAL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; +template Vec3f COAL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; -template Vec3f HPP_FCL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; +template Vec3f COAL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; // clang-format on } // namespace details diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp index 3f817ef47..947ffb60e 100644 --- a/src/narrowphase/support_functions.cpp +++ b/src/narrowphase/support_functions.cpp @@ -36,7 +36,7 @@ /** \authors Jia Pan, Florent Lamiraux, Josef Mirabel, Louis Montaut */ -#include "hpp/fcl/narrowphase/support_functions.h" +#include "coal/narrowphase/support_functions.h" #include @@ -89,20 +89,19 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) { // Explicit instantiation // clang-format off -template HPP_FCL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); +template COAL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); -template HPP_FCL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); +template COAL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); // clang-format on // ============================================================================ -#define getShapeSupportTplInstantiation(ShapeType) \ - template HPP_FCL_DLLAPI void getShapeSupport( \ - const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ - ShapeSupportData& support_data); \ - \ - template HPP_FCL_DLLAPI void \ - getShapeSupport( \ - const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ +#define getShapeSupportTplInstantiation(ShapeType) \ + template COAL_DLLAPI void getShapeSupport( \ + const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ + ShapeSupportData& support_data); \ + \ + template COAL_DLLAPI void getShapeSupport( \ + const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ ShapeSupportData& support_data); // ============================================================================ @@ -507,21 +506,20 @@ getShapeSupportTplInstantiation(LargeConvex) // Explicit instantiation // clang-format off -template HPP_FCL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); +template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); -template HPP_FCL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); +template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); // clang-format on // ============================================================================ -#define getShapeSupportSetTplInstantiation(ShapeType) \ - template HPP_FCL_DLLAPI void \ - getShapeSupportSet( \ - const ShapeType* shape_, SupportSet& support_set, int& hint, \ - ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); \ - \ - template HPP_FCL_DLLAPI void \ - getShapeSupportSet( \ - const ShapeType* shape_, SupportSet& support_set, int& hint, \ +#define getShapeSupportSetTplInstantiation(ShapeType) \ + template COAL_DLLAPI void getShapeSupportSet( \ + const ShapeType* shape_, SupportSet& support_set, int& hint, \ + ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); \ + \ + template COAL_DLLAPI void \ + getShapeSupportSet( \ + const ShapeType* shape_, SupportSet& support_set, int& hint, \ ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); // ============================================================================ @@ -984,13 +982,13 @@ void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, support_data, num_sampled_supports, tol); } //clang-format off -getShapeSupportSetTplInstantiation(LargeConvex) - // clang-format on +getShapeSupportSetTplInstantiation(LargeConvex); +// clang-format on - // ============================================================================ - HPP_FCL_DLLAPI - void computeSupportSetConvexHull(SupportSet::Polygon& cloud, - SupportSet::Polygon& cvx_hull) { +// ============================================================================ +COAL_DLLAPI +void computeSupportSetConvexHull(SupportSet::Polygon& cloud, + SupportSet::Polygon& cvx_hull) { cvx_hull.clear(); if (cloud.size() <= 2) { diff --git a/src/octree.cpp b/src/octree.cpp index 7fc5aacec..2abfe3707 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -32,7 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include "coal/octree.h" + #include namespace coal { diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp index c4fe900fe..e97a00ba8 100644 --- a/src/serialization/serialization.cpp +++ b/src/serialization/serialization.cpp @@ -34,18 +34,18 @@ /** \author Justin Carpentier */ -#include "hpp/fcl/serialization/fwd.h" +#include "coal/serialization/fwd.h" using namespace coal; -#include "hpp/fcl/serialization/transform.h" -#include "hpp/fcl/serialization/collision_data.h" -#include "hpp/fcl/serialization/geometric_shapes.h" -#include "hpp/fcl/serialization/convex.h" -#include "hpp/fcl/serialization/hfield.h" -#include "hpp/fcl/serialization/BVH_model.h" +#include "coal/serialization/transform.h" +#include "coal/serialization/collision_data.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" +#include "coal/serialization/hfield.h" +#include "coal/serialization/BVH_model.h" #ifdef HPP_FCL_HAS_OCTOMAP -#include "hpp/fcl/serialization/octree.h" +#include "coal/serialization/octree.h" #endif HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest) diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index 69be54056..dcfb586f6 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -1,4 +1,4 @@ -#include +#include "coal/shape/convex.h" #ifdef HPP_FCL_HAS_QHULL #include diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index 87b5d739a..517ad3f35 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#include -#include +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" namespace coal { diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index b01d11472..f9bbd85f6 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -35,9 +35,9 @@ /** \author Jia Pan */ -#include -#include -#include +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/BV_fitter.h" +#include "coal/internal/tools.h" namespace coal { diff --git a/src/traits_traversal.h b/src/traits_traversal.h index c3ae4883a..45defd06d 100644 --- a/src/traits_traversal.h +++ b/src/traits_traversal.h @@ -7,53 +7,53 @@ /** \author Lucile Remigy, Joseph Mirabel */ -#include -#include +#include "coal/collision_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" #include <../src/collision_node.h> -#include -#include +#include "coal/internal/traversal_node_setup.h" +#include "coal/internal/shape_shape_func.h" namespace coal { // TraversalTraitsCollision for collision_func_matrix.cpp template -struct HPP_FCL_LOCAL TraversalTraitsCollision {}; +struct COAL_LOCAL TraversalTraitsCollision {}; #ifdef HPP_FCL_HAS_OCTOMAP template -struct HPP_FCL_LOCAL TraversalTraitsCollision { +struct COAL_LOCAL TraversalTraitsCollision { typedef ShapeOcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision { +struct COAL_LOCAL TraversalTraitsCollision { typedef OcTreeShapeCollisionTraversalNode CollisionTraversal_t; }; template <> -struct HPP_FCL_LOCAL TraversalTraitsCollision { +struct COAL_LOCAL TraversalTraitsCollision { typedef OcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision > { +struct COAL_LOCAL TraversalTraitsCollision > { typedef OcTreeMeshCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision, OcTree> { +struct COAL_LOCAL TraversalTraitsCollision, OcTree> { typedef MeshOcTreeCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision > { +struct COAL_LOCAL TraversalTraitsCollision > { typedef OcTreeHeightFieldCollisionTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsCollision, OcTree> { +struct COAL_LOCAL TraversalTraitsCollision, OcTree> { typedef HeightFieldOcTreeCollisionTraversalNode CollisionTraversal_t; }; @@ -62,32 +62,32 @@ struct HPP_FCL_LOCAL TraversalTraitsCollision, OcTree> { // TraversalTraitsDistance for distance_func_matrix.cpp template -struct HPP_FCL_LOCAL TraversalTraitsDistance {}; +struct COAL_LOCAL TraversalTraitsDistance {}; #ifdef HPP_FCL_HAS_OCTOMAP template -struct HPP_FCL_LOCAL TraversalTraitsDistance { +struct COAL_LOCAL TraversalTraitsDistance { typedef ShapeOcTreeDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance { +struct COAL_LOCAL TraversalTraitsDistance { typedef OcTreeShapeDistanceTraversalNode CollisionTraversal_t; }; template <> -struct HPP_FCL_LOCAL TraversalTraitsDistance { +struct COAL_LOCAL TraversalTraitsDistance { typedef OcTreeDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance > { +struct COAL_LOCAL TraversalTraitsDistance > { typedef OcTreeMeshDistanceTraversalNode CollisionTraversal_t; }; template -struct HPP_FCL_LOCAL TraversalTraitsDistance, OcTree> { +struct COAL_LOCAL TraversalTraitsDistance, OcTree> { typedef MeshOcTreeDistanceTraversalNode CollisionTraversal_t; }; diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp index 96b755545..01ed51e1c 100644 --- a/src/traversal/traversal_recurse.cpp +++ b/src/traversal/traversal_recurse.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#include +#include "coal/internal/traversal_recurse.h" #include @@ -202,7 +202,7 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, } /** @brief Bounding volume test structure */ -struct HPP_FCL_LOCAL BVT { +struct COAL_LOCAL BVT { /** @brief distance between bvs */ FCL_REAL d; @@ -211,13 +211,13 @@ struct HPP_FCL_LOCAL BVT { }; /** @brief Comparer between two BVT */ -struct HPP_FCL_LOCAL BVT_Comparer { +struct COAL_LOCAL BVT_Comparer { bool operator()(const BVT& lhs, const BVT& rhs) const { return lhs.d > rhs.d; } }; -struct HPP_FCL_LOCAL BVTQ { +struct COAL_LOCAL BVTQ { BVTQ() : qsize(2) {} bool empty() const { return pq.empty(); } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2891ad789..adfdeb82b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -7,7 +7,7 @@ macro(add_fcl_test test_name source) ADD_UNIT_TEST(${test_name} ${source}) target_link_libraries(${test_name} PUBLIC - hpp-fcl + ${LIBRARY_NAME} Boost::filesystem utility ) diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp index 98160e78a..be71edefc 100644 --- a/test/accelerated_gjk.cpp +++ b/test/accelerated_gjk.cpp @@ -38,9 +38,9 @@ #include #include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" #include "utility.h" diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 49afac16a..372c8f06f 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -16,10 +16,10 @@ #include -#include -#include +#include "coal/internal/traversal_node_setup.h" +#include "coal/internal/traversal_node_bvhs.h" #include "../src/collision_node.h" -#include +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp index cfa19eff6..af49add91 100644 --- a/test/box_box_collision.cpp +++ b/test/box_box_collision.cpp @@ -2,9 +2,9 @@ #include #include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" #include "utility.h" diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp index 7d21bd487..f1a8c1144 100644 --- a/test/box_box_distance.cpp +++ b/test/box_box_distance.cpp @@ -44,11 +44,11 @@ #include #include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" #include "utility.h" diff --git a/test/broadphase.cpp b/test/broadphase.cpp index d2efdc82a..b0ed10f06 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -38,10 +38,10 @@ #define BOOST_TEST_MODULE FCL_BROADPHASE #include -#include -#include -#include -#include +#include "coal/config.hh" +#include "coal/broadphase/broadphase.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/math/transform.h" #include "utility.h" #if USE_GOOGLEHASH diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp index 9fdc98f60..3ea150af4 100644 --- a/test/broadphase_collision_1.cpp +++ b/test/broadphase_collision_1.cpp @@ -38,16 +38,16 @@ #define BOOST_TEST_MODULE BROADPHASE_COLLISION_1 #include -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/default_broadphase_callbacks.h" +#include "coal/broadphase/detail/sparse_hash_table.h" +#include "coal/broadphase/detail/spatial_hash.h" #include "utility.h" #include diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp index 859206194..72f477422 100644 --- a/test/broadphase_collision_2.cpp +++ b/test/broadphase_collision_2.cpp @@ -38,16 +38,16 @@ #define BOOST_TEST_MODULE BROADPHASE_COLLISION_2 #include -#include "hpp/fcl/broadphase/broadphase_bruteforce.h" -#include "hpp/fcl/broadphase/broadphase_spatialhash.h" -#include "hpp/fcl/broadphase/broadphase_SaP.h" -#include "hpp/fcl/broadphase/broadphase_SSaP.h" -#include "hpp/fcl/broadphase/broadphase_interval_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -#include "hpp/fcl/broadphase/default_broadphase_callbacks.h" -#include "hpp/fcl/broadphase/detail/sparse_hash_table.h" -#include "hpp/fcl/broadphase/detail/spatial_hash.h" +#include "coal/broadphase/broadphase_bruteforce.h" +#include "coal/broadphase/broadphase_spatialhash.h" +#include "coal/broadphase/broadphase_SaP.h" +#include "coal/broadphase/broadphase_SSaP.h" +#include "coal/broadphase/broadphase_interval_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +#include "coal/broadphase/default_broadphase_callbacks.h" +#include "coal/broadphase/detail/sparse_hash_table.h" +#include "coal/broadphase/detail/spatial_hash.h" #include "utility.h" #if USE_GOOGLEHASH diff --git a/test/broadphase_dynamic_AABB_tree.cpp b/test/broadphase_dynamic_AABB_tree.cpp index df225687f..4db353737 100644 --- a/test/broadphase_dynamic_AABB_tree.cpp +++ b/test/broadphase_dynamic_AABB_tree.cpp @@ -36,15 +36,15 @@ /** Tests the dynamic axis-aligned bounding box tree.*/ -#include -#include - #define BOOST_TEST_MODULE BROADPHASE_DYNAMIC_AABB_TREE #include -// #include "hpp/fcl/data_types.h" -#include "hpp/fcl/shape/geometric_shapes.h" -#include "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" +// #include "coal/data_types.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/broadphase/broadphase_dynamic_AABB_tree.h" + +#include +#include using namespace coal; diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 92a715e41..b966e98c3 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -41,14 +41,14 @@ #include "fcl_resources/config.h" -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/collision.h" +#include "coal/BVH/BVH_model.h" +#include "coal/BVH/BVH_utility.h" +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/mesh_loader/assimp.h" +#include "coal/mesh_loader/loader.h" #include "utility.h" #include diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp index f34a20faa..c3a094735 100644 --- a/test/capsule_box_1.cpp +++ b/test/capsule_box_1.cpp @@ -40,11 +40,11 @@ #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) #include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" #include "utility.h" diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp index 6af75b959..ed94fc22a 100644 --- a/test/capsule_box_2.cpp +++ b/test/capsule_box_2.cpp @@ -42,11 +42,11 @@ #include "utility.h" #include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" BOOST_AUTO_TEST_CASE(distance_capsule_box) { typedef coal::shared_ptr CollisionGeometryPtr_t; diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index 593950fd9..d7f8af59f 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -42,12 +42,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/collision.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" #include "utility.h" diff --git a/test/collision.cpp b/test/collision.cpp index 115e50bbc..6484d8765 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -43,26 +43,26 @@ #include #include -#include +#include "coal/fwd.hh" -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#include +#include "coal/collision.h" -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP -#include -#include -#include -#include +#include "coal/BV/BV.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/mesh_loader/assimp.h" -#include -#include +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_setup.h" #include "../src/collision_node.h" -#include +#include "coal/internal/BV_splitter.h" -#include +#include "coal/timings.h" #include "utility.h" #include "fcl_resources/config.h" diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp index fb8b57f62..e81d75b51 100644 --- a/test/collision_node_asserts.cpp +++ b/test/collision_node_asserts.cpp @@ -2,8 +2,8 @@ #include #include -#include -#include +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" using namespace coal; diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp index 457f8be5a..fdc1a258d 100644 --- a/test/contact_patch.cpp +++ b/test/contact_patch.cpp @@ -37,7 +37,7 @@ #define BOOST_TEST_MODULE FCL_CONTACT_PATCH #include -#include +#include "coal/contact_patch.h" #include "utility.h" diff --git a/test/convex.cpp b/test/convex.cpp index ad6522029..52043cf95 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -37,9 +37,9 @@ #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES #include -#include -#include -#include +#include "coal/shape/convex.h" +#include "coal/collision.h" +#include "coal/distance.h" #include "utility.h" diff --git a/test/distance.cpp b/test/distance.cpp index 567e2109b..89db86065 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -41,10 +41,10 @@ #include #include -#include -#include +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_setup.h" #include "../src/collision_node.h" -#include +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index 7442280e8..598f1a33e 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -36,13 +36,13 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/data_types.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/collision.h" +#include "coal/distance.h" #include "utility.h" #include "fcl_resources/config.h" diff --git a/test/frontlist.cpp b/test/frontlist.cpp index f90634d5e..8c63fb1e2 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -38,10 +38,10 @@ #define BOOST_TEST_MODULE FCL_FRONT_LIST #include -#include -#include +#include "coal/internal/traversal_node_bvhs.h" +#include "coal/internal/traversal_node_setup.h" #include <../src/collision_node.h> -#include +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index ba5114c3f..150a43f30 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -39,14 +39,14 @@ #define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES #include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/collision.h" +#include "coal/distance.h" #include "utility.h" #include -#include -#include -#include +#include "coal/internal/tools.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/internal/shape_shape_func.h" using namespace coal; diff --git a/test/gjk.cpp b/test/gjk.cpp index 1631a2777..75c96efa1 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -39,10 +39,10 @@ #include #include -#include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" +#include "coal/internal/shape_shape_func.h" #include "utility.h" diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp index afe6ae5de..b65b5d111 100644 --- a/test/gjk_asserts.cpp +++ b/test/gjk_asserts.cpp @@ -2,8 +2,8 @@ #include #include -#include -#include +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" using namespace coal; diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index bc331a7a7..2cbb124fb 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -34,15 +34,15 @@ /** \author Louis Montaut */ -#include "hpp/fcl/data_types.h" -#include #define BOOST_TEST_MODULE FCL_NESTEROV_GJK #include +#include #include -#include -#include -#include +#include "coal/data_types.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/internal/tools.h" #include "utility.h" diff --git a/test/hfields.cpp b/test/hfields.cpp index a28ea8052..92bd16f49 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -40,15 +40,15 @@ #include "fcl_resources/config.h" -#include -#include -#include -#include -#include -#include - -#include -#include +#include "coal/collision.h" +#include "coal/hfield.h" +#include "coal/math/transform.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/mesh_loader/assimp.h" +#include "coal/mesh_loader/loader.h" + +#include "coal/collision.h" +#include "coal/internal/traversal_node_hfield_shape.h" #include "utility.h" #include diff --git a/test/math.cpp b/test/math.cpp index 8a4edf638..251f49873 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -39,11 +39,11 @@ #define BOOST_TEST_MODULE FCL_MATH #include -#include -#include +#include "coal/data_types.h" +#include "coal/math/transform.h" -#include -#include +#include "coal/internal/intersect.h" +#include "coal/internal/tools.h" using namespace coal; diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp index 41ac105ee..5b31c7aea 100644 --- a/test/normal_and_nearest_points.cpp +++ b/test/normal_and_nearest_points.cpp @@ -37,12 +37,12 @@ #define BOOST_TEST_MODULE NORMAL_AND_NEAREST_POINTS #include -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/shape/geometric_shapes.h" +#include "coal/collision_data.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" #include "utility.h" diff --git a/test/obb.cpp b/test/obb.cpp index bd94b94ed..8405484b8 100644 --- a/test/obb.cpp +++ b/test/obb.cpp @@ -40,10 +40,10 @@ #include -#include +#include "coal/narrowphase/narrowphase.h" #include "../src/BV/OBB.h" -#include +#include "coal/internal/shape_shape_func.h" #include "utility.h" using namespace coal; diff --git a/test/octree.cpp b/test/octree.cpp index 5b1100de5..4d51ad612 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -40,13 +40,13 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" +#include "coal/distance.h" +#include "coal/hfield.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/internal/BV_splitter.h" #include "utility.h" #include "fcl_resources/config.h" diff --git a/test/profiling.cpp b/test/profiling.cpp index 2462cdf0b..f75566149 100644 --- a/test/profiling.cpp +++ b/test/profiling.cpp @@ -16,14 +16,14 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" +#include "coal/collision.h" +#include "coal/BVH/BVH_model.h" +#include "coal/collision_utility.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/collision_func_matrix.h" +#include "coal/narrowphase/narrowphase.h" +#include "coal/mesh_loader/assimp.h" #include "utility.h" #include "fcl_resources/config.h" diff --git a/test/security_margin.cpp b/test/security_margin.cpp index 9fe867be3..b9a825045 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -37,13 +37,13 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" #include "utility.h" diff --git a/test/serialization.cpp b/test/serialization.cpp index 4f2bee37d..7c46dcb54 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -36,30 +36,31 @@ #include #include -#include - -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "coal/fwd.hh" + +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + +#include "coal/collision.h" + +#include "coal/contact_patch.h" +#include "coal/distance.h" +#include "coal/BV/OBBRSS.h" +#include "coal/BVH/BVH_model.h" + +#include "coal/serialization/collision_data.h" +#include "coal/serialization/contact_patch.h" +#include "coal/serialization/AABB.h" +#include "coal/serialization/BVH_model.h" +#include "coal/serialization/hfield.h" +#include "coal/serialization/transform.h" +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" +#include "coal/serialization/archive.h" +#include "coal/serialization/memory.h" #ifdef HPP_FCL_HAS_OCTOMAP -#include +#include "coal/serialization/octree.h" #endif #include "utility.h" @@ -590,4 +591,4 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) { computeMemoryFootprint(m1)); } -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index 323dc90bb..9c1abd985 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -37,12 +37,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include "coal/distance.h" +#include "coal/math/transform.h" +#include "coal/collision.h" +#include "coal/collision_object.h" +#include "coal/shape/geometric_shapes.h" +#include "coal/shape/geometric_shapes_utility.h" #include "utility.h" diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index e84f179fc..1e45c3451 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -38,10 +38,10 @@ #define BOOST_TEST_MODULE FCL_SHAPE_MESH_CONSISTENCY #include -#include -#include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/distance.h" +#include "coal/collision.h" #include "utility.h" using namespace coal; diff --git a/test/simple.cpp b/test/simple.cpp index b1582ef3a..401053714 100644 --- a/test/simple.cpp +++ b/test/simple.cpp @@ -1,9 +1,9 @@ #define BOOST_TEST_MODULE FCL_SIMPLE #include -#include -#include -#include +#include "coal/internal/intersect.h" +#include "coal/collision.h" +#include "coal/BVH/BVH_model.h" #include "fcl_resources/config.h" #include diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index b829fd7b9..df7974e28 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -37,13 +37,13 @@ #define BOOST_TEST_MODULE SWEPT_SPHERE_RADIUS #include -#include -#include +#include "coal/narrowphase/narrowphase.h" +#include "coal/collision_utility.h" -#include -#include -#include -#include +#include "coal/serialization/geometric_shapes.h" +#include "coal/serialization/convex.h" +#include "coal/serialization/transform.h" +#include "coal/serialization/archive.h" #include "utility.h" diff --git a/test/utility.cpp b/test/utility.cpp index a17831eb9..5efd2ca1e 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -1,13 +1,13 @@ #include "utility.h" -#include -#include -#include -#include -#include - -#include -#include +#include "coal/BV/BV.h" +#include "coal/BVH/BVH_model.h" +#include "coal/shape/geometric_shape_to_BVH_model.h" +#include "coal/collision_utility.h" +#include "coal/fwd.hh" + +#include "coal/collision.h" +#include "coal/distance.h" #include #include diff --git a/test/utility.h b/test/utility.h index 96b09c26d..444f8aa54 100644 --- a/test/utility.h +++ b/test/utility.h @@ -38,14 +38,14 @@ #ifndef TEST_HPP_FCL_UTILITY_H #define TEST_HPP_FCL_UTILITY_H -#include -#include -#include -#include -#include +#include "coal/math/transform.h" +#include "coal/collision_data.h" +#include "coal/collision_object.h" +#include "coal/broadphase/default_broadphase_callbacks.h" +#include "coal/shape/convex.h" #ifdef HPP_FCL_HAS_OCTOMAP -#include +#include "coal/octree.h" #endif #ifdef _WIN32 From 69ac92e2e9e8205e70e03c88dc5920c8636c16f8 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 19 Jun 2024 12:27:12 +0200 Subject: [PATCH 06/57] all: change `HPP_FCL_...` macros to `COAL_...` macros --- CMakeLists.txt | 34 +++---- include/coal/BV/kDOP.h | 4 +- ...adphase_continuous_collision_manager-inl.h | 4 +- .../broadphase_dynamic_AABB_tree-inl.h | 4 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 4 +- .../detail/hierarchy_tree_array-inl.h | 24 ++--- .../broadphase/detail/simple_hash_table-inl.h | 4 +- include/coal/collision_data.h | 68 +++++++------ include/coal/data_types.h | 4 +- include/coal/fwd.hh | 68 ++++++------- include/coal/hfield.h | 8 +- .../internal/shape_shape_contact_patch_func.h | 22 ++--- include/coal/internal/traversal.h | 4 +- .../internal/traversal_node_hfield_shape.h | 5 +- include/coal/internal/traversal_node_setup.h | 30 +++--- include/coal/internal/traversal_node_shapes.h | 2 +- include/coal/logging.h | 22 ++--- include/coal/mesh_loader/assimp.h | 2 +- include/coal/narrowphase/gjk.h | 4 +- include/coal/narrowphase/narrowphase.h | 97 +++++++++---------- include/coal/serialization/AABB.h | 2 +- include/coal/serialization/BVH_model.h | 24 ++--- include/coal/serialization/archive.h | 6 +- include/coal/serialization/collision_data.h | 32 +++--- include/coal/serialization/collision_object.h | 2 +- include/coal/serialization/convex.h | 4 +- include/coal/serialization/eigen.h | 4 +- include/coal/serialization/fwd.h | 34 +++---- include/coal/serialization/geometric_shapes.h | 22 ++--- include/coal/serialization/hfield.h | 6 +- include/coal/serialization/octree.h | 2 +- include/coal/shape/details/convex.hxx | 2 +- include/coal/shape/geometric_shapes.h | 68 ++++++------- include/coal/shape/geometric_shapes_utility.h | 4 +- include/coal/timings.h | 16 +-- python/CMakeLists.txt | 6 +- python/broadphase/broadphase.cc | 14 +-- python/broadphase/broadphase_callbacks.hh | 2 +- .../broadphase_collision_manager.hh | 2 +- python/collision-geometries.cc | 2 +- python/collision.cc | 2 +- python/contact_patch.cc | 2 +- python/distance.cc | 2 +- python/fcl.cc | 4 +- python/fcl.hh | 2 +- python/fwd.hh | 2 +- python/gjk.cc | 2 +- python/math.cc | 2 +- python/octree.cc | 2 +- python/version.cc | 4 +- src/CMakeLists.txt | 24 ++--- .../broadphase_collision_manager.cpp | 4 +- .../broadphase_dynamic_AABB_tree.cpp | 12 +-- .../broadphase_dynamic_AABB_tree_array.cpp | 10 +- src/collision.cpp | 40 ++++---- src/collision_func_matrix.cpp | 11 +-- src/collision_node.cpp | 21 ++-- src/collision_node.h | 4 +- src/collision_utility.cpp | 12 +-- src/contact_patch.cpp | 36 +++---- src/contact_patch/contact_patch_solver.cpp | 2 +- src/contact_patch_func_matrix.cpp | 44 ++++----- src/distance.cpp | 36 +++---- src/distance_func_matrix.cpp | 30 +++--- src/mesh_loader/assimp.cpp | 4 +- src/mesh_loader/loader.cpp | 13 ++- src/narrowphase/details.h | 4 +- src/narrowphase/gjk.cpp | 28 +++--- src/narrowphase/minkowski_difference.cpp | 6 +- src/narrowphase/support_functions.cpp | 6 +- src/octree.cpp | 2 +- src/serialization/serialization.cpp | 52 +++++----- src/shape/convex.cpp | 34 +++---- src/shape/geometric_shapes.cpp | 8 +- src/shape/geometric_shapes_utility.cpp | 80 +++++++-------- src/traits_traversal.h | 4 +- test/CMakeLists.txt | 8 +- test/convex.cpp | 2 +- test/serialization.cpp | 6 +- test/swept_sphere_radius.cpp | 45 +++++---- test/utility.cpp | 16 +-- test/utility.h | 10 +- 82 files changed, 647 insertions(+), 659 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c2acb4bf7..081f6ebd9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,8 +50,8 @@ SET(DOXYGEN_USE_TEMPLATE_CSS TRUE) # Need to be set before including base.cmake # ---------------------------------------------------- option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) -option(HPP_FCL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical HPP-FCL asserts to exception." FALSE) -option(HPP_FCL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE) +option(COAL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical COAL asserts to exception." FALSE) +option(COAL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE) # Check if the submodule cmake have been initialized set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") @@ -128,7 +128,7 @@ endif() SET_BOOST_DEFAULT_OPTIONS() EXPORT_BOOST_DEFAULT_OPTIONS() ADD_PROJECT_DEPENDENCY(Boost REQUIRED chrono thread date_time serialization filesystem) -if (HPP_FCL_ENABLE_LOGGING) +if (COAL_ENABLE_LOGGING) ADD_PROJECT_DEPENDENCY(Boost REQUIRED log) endif() if(BUILD_PYTHON_INTERFACE) @@ -146,23 +146,23 @@ endif() # Optional dependencies ADD_PROJECT_DEPENDENCY(octomap PKG_CONFIG_REQUIRES "octomap >= 1.6") if(octomap_FOUND) - SET(HPP_FCL_HAS_OCTOMAP TRUE) + SET(COAL_HAS_OCTOMAP TRUE) string(REPLACE "." ";" VERSION_LIST ${octomap_VERSION}) list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION) list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION) list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION) - message(STATUS "HPP-FCL uses Octomap") + message(STATUS "COAL uses Octomap") else() - SET(HPP_FCL_HAS_OCTOMAP FALSE) - message(STATUS "HPP-FCL does not use Octomap") + SET(COAL_HAS_OCTOMAP FALSE) + message(STATUS "COAL does not use Octomap") endif() -option(HPP_FCL_HAS_QHULL "use qhull library to compute convex hulls." FALSE) -if(HPP_FCL_HAS_QHULL) +option(COAL_HAS_QHULL "use qhull library to compute convex hulls." FALSE) +if(COAL_HAS_QHULL) find_package(Qhull COMPONENTS qhull_r qhullcpp) if(Qhull_FOUND) - set(HPP_FCL_USE_SYSTEM_QHULL TRUE) - message(STATUS "HPP-FCL uses system Qhull") + set(COAL_USE_SYSTEM_QHULL TRUE) + message(STATUS "COAL uses system Qhull") else() message(STATUS "Qhullcpp not found: it will be build from sources, if Qhull_r is found") file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/third-parties) @@ -180,7 +180,7 @@ if(HPP_FCL_HAS_QHULL) PATHS ${Qhull_PREFIX} ) if(NOT Qhull_r_LIBRARY) - message(FATAL_ERROR "Qhull_r not found, please install it or turn HPP_FCL_HAS_QHULL OFF") + message(FATAL_ERROR "Qhull_r not found, please install it or turn COAL_HAS_QHULL OFF") endif() endif() endif() @@ -308,13 +308,13 @@ SET(${PROJECT_NAME}_HEADERS include/coal/timings.h ) -IF(HPP_FCL_HAS_OCTOMAP) +IF(COAL_HAS_OCTOMAP) LIST(APPEND ${PROJECT_NAME}_HEADERS include/coal/octree.h include/coal/serialization/octree.h include/coal/internal/traversal_node_octree.h ) -ENDIF(HPP_FCL_HAS_OCTOMAP) +ENDIF(COAL_HAS_OCTOMAP) add_subdirectory(doc) add_subdirectory(src) @@ -326,11 +326,11 @@ if(BUILD_TESTING) endif(BUILD_TESTING) pkg_config_append_libs("coal") -IF(HPP_FCL_HAS_OCTOMAP) +IF(COAL_HAS_OCTOMAP) # FCL_HAVE_OCTOMAP kept for backward compatibility reasons. PKG_CONFIG_APPEND_CFLAGS( - "-DHPP_FCL_HAS_OCTOMAP -DHPP_FCL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}") -ENDIF(HPP_FCL_HAS_OCTOMAP) + "-DCOAL_HAS_OCTOMAP -DCOAL_HAVE_OCTOMAP -DFCL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}") +ENDIF(COAL_HAS_OCTOMAP) # Install catkin package.xml INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME}) diff --git a/include/coal/BV/kDOP.h b/include/coal/BV/kDOP.h index 5a68e15ad..cb35f7838 100644 --- a/include/coal/BV/kDOP.h +++ b/include/coal/BV/kDOP.h @@ -171,14 +171,14 @@ class COAL_DLLAPI KDOP { template bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/) { - HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); + COAL_THROW_PRETTY("not implemented", std::logic_error); } template bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/, const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/) { - HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); + COAL_THROW_PRETTY("not implemented", std::logic_error); } /// @brief translate the KDOP BV diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h index 441a615bc..b3e7a4aa8 100644 --- a/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h +++ b/include/coal/broadphase/broadphase_continuous_collision_manager-inl.h @@ -65,7 +65,7 @@ void BroadPhaseContinuousCollisionManager::registerObjects( void BroadPhaseContinuousCollisionManager::update( ContinuousCollisionObject* updated_obj) { - HPP_FCL_UNUSED_VARIABLE(updated_obj); + COAL_UNUSED_VARIABLE(updated_obj); update(); } @@ -74,7 +74,7 @@ void BroadPhaseContinuousCollisionManager::update( void BroadPhaseContinuousCollisionManager::update( const std::vector& updated_objs) { - HPP_FCL_UNUSED_VARIABLE(updated_objs); + COAL_UNUSED_VARIABLE(updated_objs); update(); } diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h index 924b505ef..05d8881a6 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -42,7 +42,7 @@ #include -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif @@ -54,7 +54,7 @@ namespace detail { namespace dynamic_AABB_tree { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== template diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 70327aa92..caafe0096 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -41,7 +41,7 @@ #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" #include "coal/shape/geometric_shapes_utility.h" -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif @@ -49,7 +49,7 @@ namespace coal { namespace detail { namespace dynamic_AABB_tree_array { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== template diff --git a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h index d9ba6c84f..7fafa1281 100644 --- a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h +++ b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h @@ -297,9 +297,9 @@ bool HierarchyTree::update(size_t leaf, const BV& bv) { template bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin) { - HPP_FCL_UNUSED_VARIABLE(bv); - HPP_FCL_UNUSED_VARIABLE(vel); - HPP_FCL_UNUSED_VARIABLE(margin); + COAL_UNUSED_VARIABLE(bv); + COAL_UNUSED_VARIABLE(vel); + COAL_UNUSED_VARIABLE(margin); if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); @@ -309,7 +309,7 @@ bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, //============================================================================== template bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel) { - HPP_FCL_UNUSED_VARIABLE(vel); + COAL_UNUSED_VARIABLE(vel); if (nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); @@ -909,20 +909,20 @@ template struct SelectImpl { static bool run(size_t query, size_t node1, size_t node2, NodeBase* nodes) { - HPP_FCL_UNUSED_VARIABLE(query); - HPP_FCL_UNUSED_VARIABLE(node1); - HPP_FCL_UNUSED_VARIABLE(node2); - HPP_FCL_UNUSED_VARIABLE(nodes); + COAL_UNUSED_VARIABLE(query); + COAL_UNUSED_VARIABLE(node1); + COAL_UNUSED_VARIABLE(node2); + COAL_UNUSED_VARIABLE(nodes); return 0; } static bool run(const BV& query, size_t node1, size_t node2, NodeBase* nodes) { - HPP_FCL_UNUSED_VARIABLE(query); - HPP_FCL_UNUSED_VARIABLE(node1); - HPP_FCL_UNUSED_VARIABLE(node2); - HPP_FCL_UNUSED_VARIABLE(nodes); + COAL_UNUSED_VARIABLE(query); + COAL_UNUSED_VARIABLE(node1); + COAL_UNUSED_VARIABLE(node2); + COAL_UNUSED_VARIABLE(nodes); return 0; } diff --git a/include/coal/broadphase/detail/simple_hash_table-inl.h b/include/coal/broadphase/detail/simple_hash_table-inl.h index 1243d957d..ae534f037 100644 --- a/include/coal/broadphase/detail/simple_hash_table-inl.h +++ b/include/coal/broadphase/detail/simple_hash_table-inl.h @@ -55,8 +55,8 @@ SimpleHashTable::SimpleHashTable(const HashFnc& h) : h_(h) { template void SimpleHashTable::init(size_t size) { if (size == 0) { - HPP_FCL_THROW_PRETTY("SimpleHashTable must have non-zero size.", - std::logic_error); + COAL_THROW_PRETTY("SimpleHashTable must have non-zero size.", + std::logic_error); } table_.resize(size); diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index 505716a7a..26c12ad79 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -216,8 +216,8 @@ struct COAL_DLLAPI QueryRequest { /// @brief threshold below which a collision is considered. FCL_REAL collision_distance_threshold; - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS /// @brief Default constructor. QueryRequest() : gjk_initial_guess(GJKInitialGuess::DefaultGuess), @@ -240,7 +240,7 @@ struct COAL_DLLAPI QueryRequest { /// @brief Copy assignment operator. QueryRequest& operator=(const QueryRequest& other) = default; - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP /// @brief Updates the guess for the internal GJK algorithm in order to /// warm-start it when reusing this collision request on the same collision @@ -251,8 +251,8 @@ struct COAL_DLLAPI QueryRequest { /// @brief whether two QueryRequest are the same or not inline bool operator==(const QueryRequest& other) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS return gjk_initial_guess == other.gjk_initial_guess && enable_cached_gjk_guess == other.enable_cached_gjk_guess && gjk_variant == other.gjk_variant && @@ -267,7 +267,7 @@ struct COAL_DLLAPI QueryRequest { epa_tolerance == other.epa_tolerance && enable_timings == other.enable_timings && collision_distance_threshold == other.collision_distance_threshold; - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } }; @@ -288,14 +288,14 @@ struct COAL_DLLAPI QueryResult { }; inline void QueryRequest::updateGuess(const QueryResult& result) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (gjk_initial_guess == GJKInitialGuess::CachedGuess || enable_cached_gjk_guess) { cached_gjk_guess = result.cached_gjk_guess; cached_support_func_guess = result.cached_support_func_guess; } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } struct CollisionResult; @@ -344,8 +344,8 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { /// @param[in] flag Collision request flag /// @param[in] num_max_contacts Maximal number of allowed contacts /// - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) : num_max_contacts(num_max_contacts_), enable_contact(flag & CONTACT), @@ -362,14 +362,14 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { security_margin(0), break_distance(1e-3), distance_upper_bound((std::numeric_limits::max)()) {} - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP bool isSatisfied(const CollisionResult& result) const; /// @brief whether two CollisionRequest are the same or not inline bool operator==(const CollisionRequest& other) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS return QueryRequest::operator==(other) && num_max_contacts == other.num_max_contacts && enable_contact == other.enable_contact && @@ -377,7 +377,7 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { security_margin == other.security_margin && break_distance == other.break_distance && distance_upper_bound == other.distance_upper_bound; - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } }; @@ -447,7 +447,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// @brief get the i-th contact calculated const Contact& getContact(size_t i) const { if (contacts.size() == 0) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "The number of contacts is zero. No Contact can be returned.", std::invalid_argument); @@ -460,7 +460,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// @brief set the i-th contact calculated void setContact(size_t i, const Contact& c) { if (contacts.size() == 0) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "The number of contacts is zero. No Contact can be returned.", std::invalid_argument); @@ -619,8 +619,7 @@ struct COAL_DLLAPI ContactPatch { /// @brief Getter for the i-th 2D point in the set. Vec2f& point(const size_t i) { - HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.", - std::logic_error); + COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); if (i < this->m_points.size()) { return this->m_points[i]; } @@ -629,8 +628,7 @@ struct COAL_DLLAPI ContactPatch { /// @brief Const getter for the i-th 2D point in the set. const Vec2f& point(const size_t i) const { - HPP_FCL_ASSERT(this->m_points.size() > 0, "Patch is empty.", - std::logic_error); + COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); if (i < this->m_points.size()) { return this->m_points[i]; } @@ -781,7 +779,7 @@ struct COAL_DLLAPI ContactPatchRequest { /// @copydoc m_num_samples_curved_shapes void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) { if (num_samples_curved_shapes < 3) { - HPP_FCL_LOG_WARNING( + COAL_LOG_WARNING( "`num_samples_curved_shapes` cannot be lower than 3. Setting it to " "3 to prevent bugs."); this->m_num_samples_curved_shapes = 3; @@ -798,7 +796,7 @@ struct COAL_DLLAPI ContactPatchRequest { /// @copydoc m_patch_tolerance void setPatchTolerance(const FCL_REAL patch_tolerance) { if (patch_tolerance < 0) { - HPP_FCL_LOG_WARNING( + COAL_LOG_WARNING( "`patch_tolerance` cannot be negative. Setting it to 0 to prevent " "bugs."); this->m_patch_tolerance = Eigen::NumTraits::dummy_precision(); @@ -862,7 +860,7 @@ struct COAL_DLLAPI ContactPatchResult { /// @brief Returns a new unused contact patch from the internal data vector. ContactPatchRef getUnusedContactPatch() { if (this->m_id_available_patch >= this->m_contact_patches_data.size()) { - HPP_FCL_LOG_WARNING( + COAL_LOG_WARNING( "Trying to get an unused contact patch but all contact patches are " "used. Increasing size of contact patches vector, at the cost of a " "copy. You should increase `max_num_patch` in the " @@ -882,7 +880,7 @@ struct COAL_DLLAPI ContactPatchResult { /// @brief Const getter for the i-th contact patch of the result. const ContactPatch& getContactPatch(const size_t i) const { if (this->m_contact_patches.empty()) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "The number of contact patches is zero. No ContactPatch can be " "returned.", std::invalid_argument); @@ -896,7 +894,7 @@ struct COAL_DLLAPI ContactPatchResult { /// @brief Getter for the i-th contact patch of the result. ContactPatch& contactPatch(const size_t i) { if (this->m_contact_patches.empty()) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "The number of contact patches is zero. No ContactPatch can be " "returned.", std::invalid_argument); @@ -1018,8 +1016,8 @@ struct COAL_DLLAPI DistanceRequest : QueryRequest { /// \param enable_signed_distance_ allows to compute the penetration depth /// \param rel_err_ /// \param abs_err_ - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS DistanceRequest(bool enable_nearest_points_ = true, bool enable_signed_distance_ = true, FCL_REAL rel_err_ = 0.0, FCL_REAL abs_err_ = 0.0) @@ -1027,24 +1025,24 @@ struct COAL_DLLAPI DistanceRequest : QueryRequest { enable_signed_distance(enable_signed_distance_), rel_err(rel_err_), abs_err(abs_err_) {} - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP bool isSatisfied(const DistanceResult& result) const; - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS DistanceRequest& operator=(const DistanceRequest& other) = default; - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP /// @brief whether two DistanceRequest are the same or not inline bool operator==(const DistanceRequest& other) const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS return QueryRequest::operator==(other) && enable_nearest_points == other.enable_nearest_points && enable_signed_distance == other.enable_signed_distance && rel_err == other.rel_err && abs_err == other.abs_err; - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } }; diff --git a/include/coal/data_types.h b/include/coal/data_types.h index 0759485bf..0f69930ee 100644 --- a/include/coal/data_types.h +++ b/include/coal/data_types.h @@ -44,7 +44,7 @@ #include "coal/config.hh" -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP #define OCTOMAP_VERSION_AT_LEAST(x, y, z) \ (OCTOMAP_MAJOR_VERSION > x || \ (OCTOMAP_MAJOR_VERSION >= x && \ @@ -56,7 +56,7 @@ (OCTOMAP_MAJOR_VERSION <= x && \ (OCTOMAP_MINOR_VERSION < y || \ (OCTOMAP_MINOR_VERSION <= y && OCTOMAP_PATCH_VERSION <= z)))) -#endif // HPP_FCL_HAS_OCTOMAP +#endif // COAL_HAS_OCTOMAP namespace coal { typedef double FCL_REAL; diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh index e699a7b16..196a207cf 100644 --- a/include/coal/fwd.hh +++ b/include/coal/fwd.hh @@ -48,52 +48,52 @@ #include "coal/warning.hh" #if _WIN32 -#define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__ +#define COAL_PRETTY_FUNCTION __FUNCSIG__ #else -#define HPP_FCL_PRETTY_FUNCTION __PRETTY_FUNCTION__ +#define COAL_PRETTY_FUNCTION __PRETTY_FUNCTION__ #endif -#define HPP_FCL_UNUSED_VARIABLE(var) (void)(var) +#define COAL_UNUSED_VARIABLE(var) (void)(var) #ifdef NDEBUG -#define HPP_FCL_ONLY_USED_FOR_DEBUG(var) HPP_FCL_UNUSED_VARIABLE(var) +#define COAL_ONLY_USED_FOR_DEBUG(var) COAL_UNUSED_VARIABLE(var) #else -#define HPP_FCL_ONLY_USED_FOR_DEBUG(var) +#define COAL_ONLY_USED_FOR_DEBUG(var) #endif -#define HPP_FCL_THROW_PRETTY(message, exception) \ - { \ - std::stringstream ss; \ - ss << "From file: " << __FILE__ << "\n"; \ - ss << "in function: " << HPP_FCL_PRETTY_FUNCTION << "\n"; \ - ss << "at line: " << __LINE__ << "\n"; \ - ss << "message: " << message << "\n"; \ - throw exception(ss.str()); \ +#define COAL_THROW_PRETTY(message, exception) \ + { \ + std::stringstream ss; \ + ss << "From file: " << __FILE__ << "\n"; \ + ss << "in function: " << COAL_PRETTY_FUNCTION << "\n"; \ + ss << "at line: " << __LINE__ << "\n"; \ + ss << "message: " << message << "\n"; \ + throw exception(ss.str()); \ } -#ifdef HPP_FCL_TURN_ASSERT_INTO_EXCEPTION -#define HPP_FCL_ASSERT(check, message, exception) \ - do { \ - if (!(check)) { \ - HPP_FCL_THROW_PRETTY(message, exception); \ - } \ +#ifdef COAL_TURN_ASSERT_INTO_EXCEPTION +#define COAL_ASSERT(check, message, exception) \ + do { \ + if (!(check)) { \ + COAL_THROW_PRETTY(message, exception); \ + } \ } while (0) #else -#define HPP_FCL_ASSERT(check, message, exception) \ - { \ - HPP_FCL_UNUSED_VARIABLE(exception(message)); \ - assert((check) && message); \ +#define COAL_ASSERT(check, message, exception) \ + { \ + COAL_UNUSED_VARIABLE(exception(message)); \ + assert((check) && message); \ } #endif #if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) -#define HPP_FCL_WITH_CXX11_SUPPORT +#define COAL_WITH_CXX11_SUPPORT #endif #if defined(__GNUC__) || defined(__clang__) -#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push") -#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop") -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ +#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic push") +#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("GCC diagnostic pop") +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") // GCC version 4.6 and higher supports -Wmaybe-uninitialized @@ -110,17 +110,17 @@ #define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED #endif #elif defined(WIN32) -#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)") -#define HPP_FCL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)") -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ +#define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)") +#define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)") +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ _Pragma("warning(disable : 4996)") #define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ _Pragma("warning(disable : 4700)") #else -#define HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -#define HPP_FCL_COMPILER_DIAGNOSTIC_POP -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED +#define COAL_COMPILER_DIAGNOSTIC_PUSH +#define COAL_COMPILER_DIAGNOSTIC_POP +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED #endif // __GNUC__ namespace coal { diff --git a/include/coal/hfield.h b/include/coal/hfield.h index 571d150ca..0e3269bf6 100644 --- a/include/coal/hfield.h +++ b/include/coal/hfield.h @@ -289,7 +289,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { void updateHeights(const MatrixXf& new_heights) { if (new_heights.rows() != heights.rows() || new_heights.cols() != heights.cols()) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "The matrix containing the new heights values does not have the same " "matrix size as the original one.\n" "\tinput values - rows: " @@ -364,7 +364,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1); assert(max_recursive_height == max_height && "the maximal height is not correct"); - HPP_FCL_UNUSED_VARIABLE(max_recursive_height); + COAL_UNUSED_VARIABLE(max_recursive_height); bvs.resize(num_bvs); return BVH_OK; @@ -481,14 +481,14 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// @brief Access the bv giving the its index const HFNode& getBV(unsigned int i) const { if (i >= num_bvs) - HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); + COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument); return bvs[i]; } /// @brief Access the bv giving the its index HFNode& getBV(unsigned int i) { if (i >= num_bvs) - HPP_FCL_THROW_PRETTY("Index out of bounds", std::invalid_argument); + COAL_THROW_PRETTY("Index out of bounds", std::invalid_argument); return bvs[i]; } diff --git a/include/coal/internal/shape_shape_contact_patch_func.h b/include/coal/internal/shape_shape_contact_patch_func.h index e854fb577..b18b6bbc3 100644 --- a/include/coal/internal/shape_shape_contact_patch_func.h +++ b/include/coal/internal/shape_shape_contact_patch_func.h @@ -60,7 +60,7 @@ struct ComputeShapeShapeContactPatch { if (!collision_result.isCollision()) { return; } - HPP_FCL_ASSERT( + COAL_ASSERT( result.check(request), "The contact patch result and request are incompatible (issue of " "contact patch size or maximum number of contact patches). Make sure " @@ -94,8 +94,8 @@ void computePatchPlaneOrHalfspace(const OtherShapeType& s1, const ContactPatchSolver* csolver, const Contact& contact, ContactPatch& contact_patch) { - HPP_FCL_UNUSED_VARIABLE(s2); - HPP_FCL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(s2); + COAL_UNUSED_VARIABLE(tf2); constructContactPatchFrameFromContact(contact, contact_patch); if ((bool)(shape_traits::IsStrictlyConvex)) { // Only one point of contact; it has already been computed. @@ -143,7 +143,7 @@ void computePatchPlaneOrHalfspace(const OtherShapeType& s1, if (!collision_result.isCollision()) { \ return; \ } \ - HPP_FCL_ASSERT( \ + COAL_ASSERT( \ result.check(request), \ "The contact patch result and request are incompatible (issue of " \ "contact patch size or maximum number of contact patches). Make " \ @@ -177,7 +177,7 @@ void computePatchPlaneOrHalfspace(const OtherShapeType& s1, if (!collision_result.isCollision()) { \ return; \ } \ - HPP_FCL_ASSERT( \ + COAL_ASSERT( \ result.check(request), \ "The contact patch result and request are incompatible (issue of " \ "contact patch size or maximum number of contact patches). Make " \ @@ -212,15 +212,15 @@ PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace) const ContactPatchSolver* csolver, \ const ContactPatchRequest& request, \ ContactPatchResult& result) { \ - HPP_FCL_UNUSED_VARIABLE(o1); \ - HPP_FCL_UNUSED_VARIABLE(tf1); \ - HPP_FCL_UNUSED_VARIABLE(o2); \ - HPP_FCL_UNUSED_VARIABLE(tf2); \ - HPP_FCL_UNUSED_VARIABLE(csolver); \ + COAL_UNUSED_VARIABLE(o1); \ + COAL_UNUSED_VARIABLE(tf1); \ + COAL_UNUSED_VARIABLE(o2); \ + COAL_UNUSED_VARIABLE(tf2); \ + COAL_UNUSED_VARIABLE(csolver); \ if (!collision_result.isCollision()) { \ return; \ } \ - HPP_FCL_ASSERT( \ + COAL_ASSERT( \ result.check(request), \ "The contact patch result and request are incompatible (issue of " \ "contact patch size or maximum number of contact patches). Make " \ diff --git a/include/coal/internal/traversal.h b/include/coal/internal/traversal.h index 033ae89dd..36b8004fe 100644 --- a/include/coal/internal/traversal.h +++ b/include/coal/internal/traversal.h @@ -58,10 +58,10 @@ struct COAL_DLLAPI RelativeTransformation { template <> struct COAL_DLLAPI RelativeTransformation { static const Matrix3f& _R() { - HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); + COAL_THROW_PRETTY("should never reach this point", std::logic_error); } static const Vec3f& _T() { - HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); + COAL_THROW_PRETTY("should never reach this point", std::logic_error); } }; } // namespace details diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h index afc8c4bf8..e15c00e18 100644 --- a/include/coal/internal/traversal_node_hfield_shape.h +++ b/include/coal/internal/traversal_node_hfield_shape.h @@ -171,7 +171,7 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, assert(max_height > min_height && "max_height is lower than min_height"); // Check whether the geometry // is degenerated - HPP_FCL_UNUSED_VARIABLE(max_height); + COAL_UNUSED_VARIABLE(max_height); { std::shared_ptr> pts(new std::vector({ @@ -363,8 +363,7 @@ bool binCorrection(const Convex& convex, // We correct only if there is a collision with the bin if (is_collision) { if (!face_triangle.isValid()) - HPP_FCL_THROW_PRETTY("face_triangle is not initialized", - std::logic_error); + COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error); const Vec3f face_pointA = points[face_triangle[0]]; face_normal = computeTriangleNormal(face_triangle, points); diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h index d63af3da6..6a84580ad 100644 --- a/include/coal/internal/traversal_node_setup.h +++ b/include/coal/internal/traversal_node_setup.h @@ -49,7 +49,7 @@ // #include #include "coal/internal/traversal_node_hfield_shape.h" -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP #include "coal/internal/traversal_node_octree.h" #endif @@ -57,7 +57,7 @@ namespace coal { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP /// @brief Initialize traversal node for collision between two octrees, given /// current object transform inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, @@ -333,7 +333,7 @@ bool initialize(MeshShapeCollisionTraversalNode& node, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -380,7 +380,7 @@ bool initialize(MeshShapeCollisionTraversalNode& node, const S& model2, const Transform3f& tf2, const GJKSolver* nsolver, CollisionResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -438,7 +438,7 @@ static inline bool setupShapeMeshCollisionOrientedNode( const BVHModel& model2, const Transform3f& tf2, const GJKSolver* nsolver, CollisionResult& result) { if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -470,11 +470,11 @@ bool initialize( Transform3f& tf2, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -534,11 +534,11 @@ bool initialize(MeshCollisionTraversalNode& node, const BVHModel& model2, const Transform3f& tf2, CollisionResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -591,11 +591,11 @@ bool initialize( Transform3f& tf2, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -657,11 +657,11 @@ bool initialize(MeshDistanceTraversalNode& node, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) if (model2.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -701,7 +701,7 @@ bool initialize(MeshShapeDistanceTraversalNode& node, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) @@ -748,7 +748,7 @@ static inline bool setupMeshShapeDistanceOrientedNode( const S& model2, const Transform3f& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.", std::invalid_argument) diff --git a/include/coal/internal/traversal_node_shapes.h b/include/coal/internal/traversal_node_shapes.h index b38885888..b34eaad59 100644 --- a/include/coal/internal/traversal_node_shapes.h +++ b/include/coal/internal/traversal_node_shapes.h @@ -66,7 +66,7 @@ class COAL_DLLAPI ShapeCollisionTraversalNode /// @brief BV culling test in one BVTT node bool BVDisjoints(int, int, FCL_REAL&) const { - HPP_FCL_THROW_PRETTY("Not implemented", std::runtime_error); + COAL_THROW_PRETTY("Not implemented", std::runtime_error); } /// @brief Intersection testing between leaves (two shapes) diff --git a/include/coal/logging.h b/include/coal/logging.h index 732a890e9..c75db2fbe 100644 --- a/include/coal/logging.h +++ b/include/coal/logging.h @@ -33,22 +33,22 @@ */ /// This file defines basic logging macros for HPP-FCL, based on Boost.Log. -/// To enable logging, define the preprocessor macro `HPP_FCL_ENABLE_LOGGING`. +/// To enable logging, define the preprocessor macro `COAL_ENABLE_LOGGING`. #ifndef COAL_LOGGING_H #define COAL_LOGGING_H -#ifdef HPP_FCL_ENABLE_LOGGING +#ifdef COAL_ENABLE_LOGGING #include -#define HPP_FCL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message -#define HPP_FCL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message -#define HPP_FCL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message -#define HPP_FCL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message +#define COAL_LOG_INFO(message) BOOST_LOG_TRIVIAL(info) << message +#define COAL_LOG_DEBUG(message) BOOST_LOG_TRIVIAL(debug) << message +#define COAL_LOG_WARNING(message) BOOST_LOG_TRIVIAL(warning) << message +#define COAL_LOG_ERROR(message) BOOST_LOG_TRIVIAL(error) << message #else -#define HPP_FCL_LOG_INFO(message) -#define HPP_FCL_LOG_DEBUG(message) -#define HPP_FCL_LOG_WARNING(message) -#define HPP_FCL_LOG_ERROR(message) +#define COAL_LOG_INFO(message) +#define COAL_LOG_DEBUG(message) +#define COAL_LOG_WARNING(message) +#define COAL_LOG_ERROR(message) #endif -#endif // HPP_FCL_LOGGING_H +#endif // COAL_LOGGING_H diff --git a/include/coal/mesh_loader/assimp.h b/include/coal/mesh_loader/assimp.h index 484a604cc..2be9cfc9c 100644 --- a/include/coal/mesh_loader/assimp.h +++ b/include/coal/mesh_loader/assimp.h @@ -94,7 +94,7 @@ inline void meshFromAssimpScene( int res = mesh->beginModel(); if (res != coal::BVH_OK) { - HPP_FCL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); + COAL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); } buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv); diff --git a/include/coal/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h index b37407f93..f72292b2c 100644 --- a/include/coal/narrowphase/gjk.h +++ b/include/coal/narrowphase/gjk.h @@ -142,8 +142,8 @@ struct COAL_DLLAPI GJK { /// Suggested values are 100 iterations and a tolerance of 1e-6. GJK(size_t max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_), tolerance(tolerance_) { - HPP_FCL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", - std::invalid_argument); + COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", + std::invalid_argument); initialize(); } diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h index 28380ad21..90bc04b11 100644 --- a/include/coal/narrowphase/narrowphase.h +++ b/include/coal/narrowphase/narrowphase.h @@ -111,8 +111,8 @@ struct COAL_DLLAPI GJKSolver { static constexpr FCL_REAL m_dummy_precision = 1e-6; public: - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS /// @brief Default constructor for GJK algorithm /// By default, we don't want EPA to allocate memory because /// certain functions of the `GJKSolver` class have specializations @@ -245,8 +245,8 @@ struct COAL_DLLAPI GJKSolver { /// @brief Copy constructor GJKSolver(const GJKSolver& other) = default; - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool operator==(const GJKSolver& other) const { return this->enable_cached_guess == other.enable_cached_guess && // use gjk_initial_guess instead @@ -263,7 +263,7 @@ struct COAL_DLLAPI GJKSolver { this->epa_max_iterations == other.epa_max_iterations && this->epa_tolerance == other.epa_tolerance; } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP bool operator!=(const GJKSolver& other) const { return !(*this == other); } @@ -366,7 +366,7 @@ struct COAL_DLLAPI GJKSolver { break; case GJKInitialGuess::BoundingVolumeGuess: if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "computeLocalAABB must have been called on the shapes before " "using " "GJKInitialGuess::BoundingVolumeGuess.", @@ -378,15 +378,15 @@ struct COAL_DLLAPI GJKSolver { this->minkowski_difference.ot1); break; default: - HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); + COAL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); } // TODO: use gjk_initial_guess instead - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (this->enable_cached_guess) { guess = this->cached_guess; } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } /// @brief Runs the GJK algorithm. @@ -446,8 +446,8 @@ struct COAL_DLLAPI GJKSolver { switch (this->gjk.status) { case details::GJK::DidNotRun: - HPP_FCL_ASSERT(false, "GJK did not run. It should have!", - std::logic_error); + COAL_ASSERT(false, "GJK did not run. It should have!", + std::logic_error); this->cached_guess = Vec3f(1, 0, 0); this->support_func_cached_guess.setZero(); distance = -(std::numeric_limits::max)(); @@ -457,7 +457,7 @@ struct COAL_DLLAPI GJKSolver { case details::GJK::Failed: // // GJK ran out of iterations. - HPP_FCL_LOG_WARNING("GJK ran out of iterations."); + COAL_LOG_WARNING("GJK ran out of iterations."); GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::GJK::NoCollisionEarlyStopped: @@ -467,11 +467,11 @@ struct COAL_DLLAPI GJKSolver { // The two witness points have no meaning. GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT(distance >= this->gjk.distance_upper_bound - - this->m_dummy_precision, - "The distance should be bigger than GJK's " - "`distance_upper_bound`.", - std::logic_error); + COAL_ASSERT(distance >= this->gjk.distance_upper_bound - + this->m_dummy_precision, + "The distance should be bigger than GJK's " + "`distance_upper_bound`.", + std::logic_error); break; case details::GJK::NoCollision: // @@ -479,18 +479,18 @@ struct COAL_DLLAPI GJKSolver { // collision, i.e their distance is above GJK's tolerance (default // 1e-6). GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT(std::abs((p1 - p2).norm() - distance) <= - this->gjk.getTolerance() + this->m_dummy_precision, - "The distance found by GJK should coincide with the " - "distance between the closest points.", - std::logic_error); + COAL_ASSERT(std::abs((p1 - p2).norm() - distance) <= + this->gjk.getTolerance() + this->m_dummy_precision, + "The distance found by GJK should coincide with the " + "distance between the closest points.", + std::logic_error); break; // // Next are the cases where GJK found the shapes to be in collision, i.e. // their distance is below GJK's tolerance (default 1e-6). case details::GJK::CollisionWithPenetrationInformation: GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT( + COAL_ASSERT( distance <= this->gjk.getTolerance() + this->m_dummy_precision, "The distance found by GJK should be negative or at " "least below GJK's tolerance.", @@ -522,27 +522,27 @@ struct COAL_DLLAPI GJKSolver { // EPA either ran out of iterations, of faces or of vertices. // The depth, witness points and the normal are still valid, // simply not at the precision of EPA's tolerance. - // The flag `HPP_FCL_ENABLE_LOGGING` enables feebdack on these + // The flag `COAL_ENABLE_LOGGING` enables feebdack on these // cases. // // TODO: Remove OutOfFaces and OutOfVertices statuses and simply // compute the upper bound on max faces and max vertices as a // function of the number of iterations. case details::EPA::OutOfFaces: - HPP_FCL_LOG_WARNING("EPA ran out of faces."); + COAL_LOG_WARNING("EPA ran out of faces."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::OutOfVertices: - HPP_FCL_LOG_WARNING("EPA ran out of vertices."); + COAL_LOG_WARNING("EPA ran out of vertices."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::Failed: - HPP_FCL_LOG_WARNING("EPA ran out of iterations."); + COAL_LOG_WARNING("EPA ran out of iterations."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::Valid: case details::EPA::AccuracyReached: - HPP_FCL_ASSERT( + COAL_ASSERT( -epa.depth <= epa.getTolerance() + this->m_dummy_precision, "EPA's penetration distance should be negative (or " "at least below EPA's tolerance).", @@ -550,32 +550,32 @@ struct COAL_DLLAPI GJKSolver { EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::Degenerated: - HPP_FCL_LOG_WARNING( + COAL_LOG_WARNING( "EPA warning: created a polytope with a degenerated face."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::NonConvex: - HPP_FCL_LOG_WARNING( + COAL_LOG_WARNING( "EPA warning: EPA got called onto non-convex shapes."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::InvalidHull: - HPP_FCL_LOG_WARNING("EPA warning: created an invalid polytope."); + COAL_LOG_WARNING("EPA warning: created an invalid polytope."); EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::DidNotRun: - HPP_FCL_ASSERT(false, "EPA did not run. It should have!", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); + COAL_ASSERT(false, "EPA did not run. It should have!", + std::logic_error); + COAL_LOG_ERROR("EPA error: did not run. It should have."); EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; case details::EPA::FallBack: - HPP_FCL_ASSERT( + COAL_ASSERT( false, "EPA went into fallback mode. It should never do that.", std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: FallBack."); + COAL_LOG_ERROR("EPA error: FallBack."); EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; @@ -589,7 +589,7 @@ struct COAL_DLLAPI GJKSolver { FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { - HPP_FCL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(tf1); // Cache gjk result for potential future call to this GJKSolver. this->cached_guess = this->gjk.ray; this->support_func_cached_guess = this->gjk.support_hint; @@ -615,11 +615,10 @@ struct COAL_DLLAPI GJKSolver { // 2. GJK ran out of iterations. // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus // it can safely be normalized. - HPP_FCL_ASSERT( - this->gjk.ray.norm() > - this->gjk.getTolerance() - this->m_dummy_precision, - "The norm of GJK's ray should be bigger than GJK's tolerance.", - std::logic_error); + COAL_ASSERT(this->gjk.ray.norm() > + this->gjk.getTolerance() - this->m_dummy_precision, + "The norm of GJK's ray should be bigger than GJK's tolerance.", + std::logic_error); // Cache gjk result for potential future call to this GJKSolver. this->cached_guess = this->gjk.ray; this->support_func_cached_guess = this->gjk.support_hint; @@ -638,11 +637,11 @@ struct COAL_DLLAPI GJKSolver { FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_ASSERT(this->gjk.distance <= - this->gjk.getTolerance() + this->m_dummy_precision, - "The distance should be lower than GJK's tolerance.", - std::logic_error); + COAL_UNUSED_VARIABLE(tf1); + COAL_ASSERT(this->gjk.distance <= + this->gjk.getTolerance() + this->m_dummy_precision, + "The distance should be lower than GJK's tolerance.", + std::logic_error); // Because GJK has returned the `Collision` status and EPA has not run, // we purposefully do not cache the result of GJK, because ray is zero. // However, we can cache the support function hint. @@ -715,7 +714,7 @@ struct COAL_DLLAPI GJKSolver { this->cached_guess = Vec3f(1, 0, 0); this->support_func_cached_guess.setZero(); - HPP_FCL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(tf1); distance = -(std::numeric_limits::max)(); p1 = p2 = normal = Vec3f::Constant(std::numeric_limits::quiet_NaN()); diff --git a/include/coal/serialization/AABB.h b/include/coal/serialization/AABB.h index 75e9be045..d68b2c54f 100644 --- a/include/coal/serialization/AABB.h +++ b/include/coal/serialization/AABB.h @@ -20,4 +20,4 @@ void serialize(Archive& ar, coal::AABB& aabb, const unsigned int /*version*/) { } // namespace serialization } // namespace boost -#endif // ifndef HPP_FCL_SERIALIZATION_AABB_H +#endif // ifndef COAL_SERIALIZATION_AABB_H diff --git a/include/coal/serialization/BVH_model.h b/include/coal/serialization/BVH_model.h index 6fee61b98..0b59a2197 100644 --- a/include/coal/serialization/BVH_model.h +++ b/include/coal/serialization/BVH_model.h @@ -2,8 +2,8 @@ // Copyright (c) 2021-2022 INRIA // -#ifndef HPP_FCL_SERIALIZATION_BVH_MODEL_H -#define HPP_FCL_SERIALIZATION_BVH_MODEL_H +#ifndef COAL_SERIALIZATION_BVH_MODEL_H +#define COAL_SERIALIZATION_BVH_MODEL_H #include "coal/BVH/BVH_model.h" @@ -32,7 +32,7 @@ void save(Archive &ar, const coal::BVHModelBase &bvh_model, if (!(bvh_model.build_state == BVH_BUILD_STATE_PROCESSED || bvh_model.build_state == BVH_BUILD_STATE_UPDATED) && (bvh_model.getModelType() == BVH_MODEL_TRIANGLES)) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "The BVH model is not in a BVH_BUILD_STATE_PROCESSED or " "BVH_BUILD_STATE_UPDATED state.\n" "The BVHModel could not be serialized.", @@ -86,7 +86,7 @@ void load(Archive &ar, coal::BVHModelBase &bvh_model, // ar >> make_nvp("has_convex",has_convex); } -HPP_FCL_SERIALIZATION_SPLIT(coal::BVHModelBase) +COAL_SERIALIZATION_SPLIT(coal::BVHModelBase) namespace internal { template @@ -238,13 +238,13 @@ struct memory_footprint_evaluator<::coal::BVHModel> { } // namespace coal -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::AABB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::RSS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBBRSS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::kIOS>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<16>>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<18>>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<24>>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::AABB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::RSS>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::OBBRSS>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::kIOS>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<16>>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<18>>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::BVHModel<::coal::KDOP<24>>) #endif // ifndef COAL_SERIALIZATION_BVH_MODEL_H diff --git a/include/coal/serialization/archive.h b/include/coal/serialization/archive.h index 428c7ee36..9d7a410c7 100644 --- a/include/coal/serialization/archive.h +++ b/include/coal/serialization/archive.h @@ -172,8 +172,7 @@ template inline void loadFromXML(T& object, const std::string& filename, const std::string& tag_name) { if (filename.empty()) { - HPP_FCL_THROW_PRETTY("Tag name should not be empty.", - std::invalid_argument); + COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument); } std::ifstream ifs(filename.c_str()); @@ -203,8 +202,7 @@ template inline void saveToXML(const T& object, const std::string& filename, const std::string& tag_name) { if (filename.empty()) { - HPP_FCL_THROW_PRETTY("Tag name should not be empty.", - std::invalid_argument); + COAL_THROW_PRETTY("Tag name should not be empty.", std::invalid_argument); } std::ofstream ofs(filename.c_str()); diff --git a/include/coal/serialization/collision_data.h b/include/coal/serialization/collision_data.h index 829fb5446..2cc10f449 100644 --- a/include/coal/serialization/collision_data.h +++ b/include/coal/serialization/collision_data.h @@ -37,18 +37,18 @@ void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) { contact.o2 = NULL; } -HPP_FCL_SERIALIZATION_SPLIT(coal::Contact) +COAL_SERIALIZATION_SPLIT(coal::Contact) template void serialize(Archive& ar, coal::QueryRequest& query_request, const unsigned int /*version*/) { ar& make_nvp("gjk_initial_guess", query_request.gjk_initial_guess); // TODO: use gjk_initial_guess instead - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS ar& make_nvp("enable_cached_gjk_guess", query_request.enable_cached_gjk_guess); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP ar& make_nvp("cached_gjk_guess", query_request.cached_gjk_guess); ar& make_nvp("cached_support_func_guess", query_request.cached_support_func_guess); @@ -81,11 +81,11 @@ void serialize(Archive& ar, coal::CollisionRequest& collision_request, collision_request)); ar& make_nvp("num_max_contacts", collision_request.num_max_contacts); ar& make_nvp("enable_contact", collision_request.enable_contact); - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS ar& make_nvp("enable_distance_lower_bound", collision_request.enable_distance_lower_bound); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP ar& make_nvp("security_margin", collision_request.security_margin); ar& make_nvp("break_distance", collision_request.break_distance); ar& make_nvp("distance_upper_bound", collision_request.distance_upper_bound); @@ -120,17 +120,17 @@ void load(Archive& ar, coal::CollisionResult& collision_result, ar >> make_nvp("normal", collision_result.normal); } -HPP_FCL_SERIALIZATION_SPLIT(coal::CollisionResult) +COAL_SERIALIZATION_SPLIT(coal::CollisionResult) template void serialize(Archive& ar, coal::DistanceRequest& distance_request, const unsigned int /*version*/) { ar& make_nvp("base", boost::serialization::base_object( distance_request)); - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS ar& make_nvp("enable_nearest_points", distance_request.enable_nearest_points); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP ar& make_nvp("enable_signed_distance", distance_request.enable_signed_distance); ar& make_nvp("rel_err", distance_request.rel_err); @@ -166,14 +166,14 @@ void load(Archive& ar, coal::DistanceResult& distance_result, distance_result.o2 = NULL; } -HPP_FCL_SERIALIZATION_SPLIT(coal::DistanceResult) +COAL_SERIALIZATION_SPLIT(coal::DistanceResult) } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionRequest) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionResult) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceRequest) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceResult) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionRequest) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionResult) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceRequest) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::DistanceResult) #endif // ifndef COAL_SERIALIZATION_COLLISION_DATA_H diff --git a/include/coal/serialization/collision_object.h b/include/coal/serialization/collision_object.h index 6d8a91c04..c37ee465c 100644 --- a/include/coal/serialization/collision_object.h +++ b/include/coal/serialization/collision_object.h @@ -36,7 +36,7 @@ void load(Archive& ar, coal::CollisionGeometry& collision_geometry, collision_geometry.user_data = NULL; // no way to recover this } -HPP_FCL_SERIALIZATION_SPLIT(coal::CollisionGeometry) +COAL_SERIALIZATION_SPLIT(coal::CollisionGeometry) } // namespace serialization } // namespace boost diff --git a/include/coal/serialization/convex.h b/include/coal/serialization/convex.h index ea5bb59ac..2aefe25fc 100644 --- a/include/coal/serialization/convex.h +++ b/include/coal/serialization/convex.h @@ -148,8 +148,8 @@ void serialize(Archive& ar, coal::Convex& convex_, } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) +COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) +COAL_SERIALIZATION_DECLARE_EXPORT(coal::Convex) namespace coal { diff --git a/include/coal/serialization/eigen.h b/include/coal/serialization/eigen.h index 16c053dc0..8a1eea87b 100644 --- a/include/coal/serialization/eigen.h +++ b/include/coal/serialization/eigen.h @@ -11,7 +11,7 @@ #ifndef COAL_SERIALIZATION_EIGEN_H #define COAL_SERIALIZATION_EIGEN_H -#ifndef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION +#ifndef COAL_SKIP_EIGEN_BOOST_SERIALIZATION #include @@ -111,6 +111,6 @@ void serialize(Archive& ar, } // namespace serialization } // namespace boost // -#endif // ifned HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION +#endif // ifned COAL_SKIP_EIGEN_BOOST_SERIALIZATION #endif // ifndef COAL_SERIALIZATION_EIGEN_H diff --git a/include/coal/serialization/fwd.h b/include/coal/serialization/fwd.h index 04fe25ad5..e56e661b3 100644 --- a/include/coal/serialization/fwd.h +++ b/include/coal/serialization/fwd.h @@ -21,29 +21,29 @@ #include "coal/fwd.h" #include "coal/serialization/eigen.h" -#define HPP_FCL_SERIALIZATION_SPLIT(Type) \ +#define COAL_SERIALIZATION_SPLIT(Type) \ template \ void serialize(Archive& ar, Type& value, const unsigned int version) { \ split_free(ar, value, version); \ } -#define HPP_FCL_SERIALIZATION_DECLARE_EXPORT(T) \ - BOOST_CLASS_EXPORT_KEY(T) \ - namespace boost { \ - namespace archive { \ - namespace detail { \ - namespace extra_detail { \ - template <> \ - struct init_guid { \ - static guid_initializer const& g; \ - }; \ - } \ - } \ - } \ - } \ +#define COAL_SERIALIZATION_DECLARE_EXPORT(T) \ + BOOST_CLASS_EXPORT_KEY(T) \ + namespace boost { \ + namespace archive { \ + namespace detail { \ + namespace extra_detail { \ + template <> \ + struct init_guid { \ + static guid_initializer const& g; \ + }; \ + } \ + } \ + } \ + } \ /**/ -#define HPP_FCL_SERIALIZATION_DEFINE_EXPORT(T) \ +#define COAL_SERIALIZATION_DEFINE_EXPORT(T) \ namespace boost { \ namespace archive { \ namespace detail { \ @@ -93,7 +93,7 @@ struct register_type { } // namespace serialization } // namespace coal -#define HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ +#define COAL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ namespace coal { \ namespace serialization { \ namespace detail { \ diff --git a/include/coal/serialization/geometric_shapes.h b/include/coal/serialization/geometric_shapes.h index 18f0753b3..661bfa534 100644 --- a/include/coal/serialization/geometric_shapes.h +++ b/include/coal/serialization/geometric_shapes.h @@ -105,16 +105,16 @@ void serialize(Archive& ar, coal::Plane& plane, } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::ShapeBase) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionGeometry) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::TriangleP) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Box) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Sphere) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Ellipsoid) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Capsule) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Cone) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Cylinder) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Halfspace) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::Plane) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::ShapeBase) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::CollisionGeometry) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::TriangleP) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Box) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Sphere) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Ellipsoid) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Capsule) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cone) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Cylinder) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Halfspace) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::Plane) #endif // ifndef COAL_SERIALIZATION_GEOMETRIC_SHAPES_H diff --git a/include/coal/serialization/hfield.h b/include/coal/serialization/hfield.h index 682f7d2c5..8ea3d7120 100644 --- a/include/coal/serialization/hfield.h +++ b/include/coal/serialization/hfield.h @@ -73,8 +73,8 @@ void serialize(Archive &ar, coal::HeightField &hf_model, } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::AABB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBB>) -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBBRSS>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::AABB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBB>) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::HeightField<::coal::OBBRSS>) #endif // ifndef COAL_SERIALIZATION_HFIELD_H diff --git a/include/coal/serialization/octree.h b/include/coal/serialization/octree.h index 17edf12ad..769532379 100644 --- a/include/coal/serialization/octree.h +++ b/include/coal/serialization/octree.h @@ -96,6 +96,6 @@ void serialize(Archive &ar, coal::OcTree &octree, const unsigned int version) { } // namespace serialization } // namespace boost -HPP_FCL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree) +COAL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree) #endif // ifndef COAL_SERIALIZATION_OCTREE_H diff --git a/include/coal/shape/details/convex.hxx b/include/coal/shape/details/convex.hxx index 6dd170be5..e77a6a094 100644 --- a/include/coal/shape/details/convex.hxx +++ b/include/coal/shape/details/convex.hxx @@ -269,7 +269,7 @@ void Convex::fillNeighbors() { for (unsigned int i = 0; i < num_points; ++i) { Neighbors& n = neighbors_[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) - HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error); + COAL_THROW_PRETTY("Too many neighbors.", std::logic_error); n.count_ = (unsigned char)nneighbors[i].size(); n.n_ = p_nneighbors; p_nneighbors = diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h index 4a639e868..ea3ad9882 100644 --- a/include/coal/shape/geometric_shapes.h +++ b/include/coal/shape/geometric_shapes.h @@ -46,7 +46,7 @@ #include "coal/collision_object.h" #include "coal/data_types.h" -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL namespace orgQhull { class Qhull; } @@ -75,8 +75,8 @@ class COAL_DLLAPI ShapeBase : public CollisionGeometry { /// Must be >= 0. void setSweptSphereRadius(FCL_REAL radius) { if (radius < 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius must be positive.", - std::invalid_argument); + COAL_THROW_PRETTY("Swept-sphere radius must be positive.", + std::invalid_argument); } this->m_swept_sphere_radius = radius; } @@ -211,10 +211,10 @@ class COAL_DLLAPI Box : public ShapeBase { /// change of shape frame std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY("value (" << value << ") " - << "is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); + COAL_THROW_PRETTY("value (" << value << ") " + << "is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))), Transform3f()); } @@ -277,10 +277,10 @@ class COAL_DLLAPI Sphere : public ShapeBase { /// the change of shape frame std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); return std::make_pair(Sphere(radius + value), Transform3f()); } @@ -351,10 +351,10 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { /// the change of shape frame std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)), Transform3f()); } @@ -436,10 +436,10 @@ class COAL_DLLAPI Capsule : public ShapeBase { /// the change of shape frame std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); return std::make_pair(Capsule(radius + value, 2 * halfLength), Transform3f()); } @@ -516,10 +516,10 @@ class COAL_DLLAPI Cone : public ShapeBase { /// change of shape frame std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); // tan(alpha) = 2*halfLength/radius; const FCL_REAL tan_alpha = 2 * halfLength / radius; @@ -610,10 +610,10 @@ class COAL_DLLAPI Cylinder : public ShapeBase { /// the change of shape frame std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), Transform3f()); } @@ -646,7 +646,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// "Qt". If \c NULL, "Qt" is passed to Qhull. /// - if \c keepTriangles is \c false, an empty string is passed to /// Qhull. - /// \note hpp-fcl must have been compiled with option \c HPP_FCL_HAS_QHULL set + /// \note hpp-fcl must have been compiled with option \c COAL_HAS_QHULL set /// to \c ON. static ConvexBase* convexHull(std::shared_ptr>& points, unsigned int num_points, bool keepTriangles, @@ -670,7 +670,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// @brief Get node type: a convex polytope NODE_TYPE getNodeType() const { return GEOM_CONVEX; } -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL /// @brief Builds the double description of the convex polytope, i.e. the set /// of hyperplanes which intersection form the polytope. void buildDoubleDescription(); @@ -778,7 +778,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// Only the list of neighbors is copied. ConvexBase(const ConvexBase& other); -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL void buildDoubleDescriptionFromQHullResult(const orgQhull::Qhull& qh); #endif @@ -937,10 +937,10 @@ class COAL_DLLAPI Halfspace : public ShapeBase { /// the change of shape frame std::pair inflated(const FCL_REAL value) const { if (value <= minInflationValue()) - HPP_FCL_THROW_PRETTY( - "value (" << value << ") is two small. It should be at least: " - << minInflationValue(), - std::invalid_argument); + COAL_THROW_PRETTY("value (" << value + << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); return std::make_pair(Halfspace(n, d + value), Transform3f()); } diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h index 3773afda0..83187b4f1 100644 --- a/include/coal/shape/geometric_shapes_utility.h +++ b/include/coal/shape/geometric_shapes_utility.h @@ -72,8 +72,8 @@ COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, template inline void computeBV(const S& s, const Transform3f& tf, BV& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } std::vector convex_bound_vertices = details::getBoundVertices(s, tf); fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(), diff --git a/include/coal/timings.h b/include/coal/timings.h index 9b361d852..c9de3e3ee 100644 --- a/include/coal/timings.h +++ b/include/coal/timings.h @@ -7,7 +7,7 @@ #include "coal/fwd.hh" -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT #include #endif @@ -29,7 +29,7 @@ struct CPUTimes { /// Importantly, this class will only have an effect for C++11 and more. /// struct COAL_DLLAPI Timer { -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT typedef std::chrono::steady_clock clock_type; typedef clock_type::duration duration_type; #endif @@ -46,7 +46,7 @@ struct COAL_DLLAPI Timer { if (m_is_stopped) return m_times; CPUTimes current(m_times); -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT std::chrono::time_point current_clock = std::chrono::steady_clock::now(); current.user += static_cast( @@ -58,7 +58,7 @@ struct COAL_DLLAPI Timer { return current; } -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT duration_type duration() const { return (m_end - m_start); } #endif @@ -67,7 +67,7 @@ struct COAL_DLLAPI Timer { m_is_stopped = false; m_times.clear(); -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT m_start = std::chrono::steady_clock::now(); #endif } @@ -77,7 +77,7 @@ struct COAL_DLLAPI Timer { if (m_is_stopped) return; m_is_stopped = true; -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT m_end = std::chrono::steady_clock::now(); m_times.user += static_cast( std::chrono::duration_cast( @@ -88,7 +88,7 @@ struct COAL_DLLAPI Timer { } void resume() { -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT if (m_is_stopped) { m_start = std::chrono::steady_clock::now(); m_is_stopped = false; @@ -102,7 +102,7 @@ struct COAL_DLLAPI Timer { CPUTimes m_times; bool m_is_stopped; -#ifdef HPP_FCL_WITH_CXX11_SUPPORT +#ifdef COAL_WITH_CXX11_SUPPORT std::chrono::time_point m_start, m_end; #endif }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index a505d0dfa..3fb00ae6a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -113,9 +113,9 @@ SET(${LIBRARY_NAME}_SOURCES broadphase/broadphase.cc ) -IF(HPP_FCL_HAS_OCTOMAP) +IF(COAL_HAS_OCTOMAP) LIST(APPEND ${LIBRARY_NAME}_SOURCES octree.cc) -ENDIF(HPP_FCL_HAS_OCTOMAP) +ENDIF(COAL_HAS_OCTOMAP) ADD_LIBRARY(${LIBRARY_NAME} MODULE ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_HEADERS}) TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS}) @@ -129,7 +129,7 @@ ADD_HEADER_GROUP(${LIBRARY_NAME}_HEADERS) ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES) IF(ENABLE_DOXYGEN_AUTODOC) ADD_DEPENDENCIES(${LIBRARY_NAME} generate_doxygen_cpp_doc) - TARGET_COMPILE_DEFINITIONS(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_DOXYGEN_AUTODOC) + TARGET_COMPILE_DEFINITIONS(${LIBRARY_NAME} PRIVATE -DCOAL_HAS_DOXYGEN_AUTODOC) ENDIF() TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index edcffde1f..afcaab456 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -44,11 +44,11 @@ #include "coal/broadphase/broadphase_interval_tree.h" #include "coal/broadphase/broadphase_spatialhash.h" -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP #include "doxygen_autodoc/hpp/fcl/broadphase/default_broadphase_callbacks.h" // #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" // #include @@ -65,8 +65,8 @@ HPP_FCL_COMPILER_DIAGNOSTIC_POP using namespace coal; -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS void exposeBroadPhase() { CollisionCallBackBaseWrapper::expose(); DistanceCallBackBaseWrapper::expose(); @@ -139,4 +139,4 @@ void exposeBroadPhase() { bp::optional>()); } } -HPP_FCL_COMPILER_DIAGNOSTIC_POP +COAL_COMPILER_DIAGNOSTIC_POP diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index 998894eb0..49b99d4fc 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -42,7 +42,7 @@ #include "../fcl.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_callbacks.h" #endif diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index ba37eaf0b..5ccf40264 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -43,7 +43,7 @@ #include "../fcl.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_collision_manager.h" #endif diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index acc14597d..db33967ed 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -54,7 +54,7 @@ #include "pickle.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC // FIXME for a reason I do not understand, doxygen fails to understand that // BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h #include "coal/internal/BV_splitter.h" diff --git a/python/collision.cc b/python/collision.cc index 7bbb5a108..7717943c4 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -45,7 +45,7 @@ COAL_COMPILER_DIAGNOSTIC_POP #include "deprecation.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/collision_data.h" #endif diff --git a/python/contact_patch.cc b/python/contact_patch.cc index 42d26835b..ed6a650d0 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -42,7 +42,7 @@ #include "deprecation.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/collision_data.h" #endif diff --git a/python/distance.cc b/python/distance.cc index 4ae1b26d1..3cd90b19b 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -47,7 +47,7 @@ COAL_COMPILER_DIAGNOSTIC_POP #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/collision_data.h" #endif diff --git a/python/fcl.cc b/python/fcl.cc index c0c6e5489..08bdf4016 100644 --- a/python/fcl.cc +++ b/python/fcl.cc @@ -44,7 +44,7 @@ #include "coal/collision.h" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/hpp/fcl/mesh_loader/loader.h" #endif @@ -98,7 +98,7 @@ BOOST_PYTHON_MODULE(hppfcl) { exposeContactPatchAPI(); exposeDistanceAPI(); exposeGJK(); -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP exposeOctree(); #endif exposeBroadPhase(); diff --git a/python/fcl.hh b/python/fcl.hh index 6b06095f9..0dda80244 100644 --- a/python/fcl.hh +++ b/python/fcl.hh @@ -18,7 +18,7 @@ void exposeDistanceAPI(); void exposeGJK(); -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP void exposeOctree(); #endif diff --git a/python/fwd.hh b/python/fwd.hh index 018ec0e6a..0a688d67f 100644 --- a/python/fwd.hh +++ b/python/fwd.hh @@ -6,7 +6,7 @@ #define COAL_PYTHON_FWD_HH #include "coal/fwd.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC namespace doxygen { using hpp::fcl::shared_ptr; } diff --git a/python/gjk.cc b/python/gjk.cc index d1b13dbbf..c69779561 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -39,7 +39,7 @@ #include "coal/fwd.hh" #include "coal/narrowphase/gjk.h" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/narrowphase/gjk.h" #endif diff --git a/python/math.cc b/python/math.cc index 7d372c731..dd93e6e3d 100644 --- a/python/math.cc +++ b/python/math.cc @@ -43,7 +43,7 @@ #include "pickle.hh" #include "serializable.hh" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/hpp/fcl/math/transform.h" #endif diff --git a/python/octree.cc b/python/octree.cc index 9fa4bf660..f8cddc926 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -6,7 +6,7 @@ #include "coal/fwd.hh" #include "coal/octree.h" -#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC +#ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" #include "doxygen_autodoc/hpp/fcl/octree.h" #endif diff --git a/python/version.cc b/python/version.cc index f0c2e3dd1..153391eb0 100644 --- a/python/version.cc +++ b/python/version.cc @@ -25,13 +25,13 @@ void exposeVersion() { bp::scope().attr("COAL_MINOR_VERSION") = COAL_MINOR_VERSION; bp::scope().attr("COAL_PATCH_VERSION") = COAL_PATCH_VERSION; -#if HPP_FCL_HAS_QHULL +#if COAL_HAS_QHULL bp::scope().attr("WITH_QHULL") = true; #else bp::scope().attr("WITH_QHULL") = false; #endif -#if HPP_FCL_HAS_OCTOMAP +#if COAL_HAS_OCTOMAP bp::scope().attr("WITH_OCTOMAP") = true; #else bp::scope().attr("WITH_OCTOMAP") = false; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index cb3639f4d..99e6c4298 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -110,11 +110,11 @@ set(${LIBRARY_NAME}_SOURCES serialization/serialization.cpp ) -if(HPP_FCL_HAS_OCTOMAP) +if(COAL_HAS_OCTOMAP) list(APPEND ${LIBRARY_NAME}_SOURCES octree.cpp) -endif(HPP_FCL_HAS_OCTOMAP) +endif(COAL_HAS_OCTOMAP) -if(HPP_FCL_HAS_QHULL AND NOT HPP_FCL_USE_SYSTEM_QHULL) +if(COAL_HAS_QHULL AND NOT COAL_USE_SYSTEM_QHULL) set( libqhullcpp_HEADERS ${Qhullcpp_PREFIX}/libqhullcpp/Coordinates.h @@ -213,10 +213,10 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} Boost::filesystem ) -if (HPP_FCL_ENABLE_LOGGING) +if (COAL_ENABLE_LOGGING) TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC Boost::log) # The compile flag `BOOST_LOG_DYN_LINK` is required here. - target_compile_definitions(${LIBRARY_NAME} PUBLIC HPP_FCL_ENABLE_LOGGING BOOST_LOG_DYN_LINK) + target_compile_definitions(${LIBRARY_NAME} PUBLIC COAL_ENABLE_LOGGING BOOST_LOG_DYN_LINK) endif() IF(WIN32) @@ -230,13 +230,13 @@ IF(WIN32) target_compile_definitions(${LIBRARY_NAME} PRIVATE _ENABLE_EXTENDED_ALIGNED_STORAGE) ENDIF(WIN32) -if (HPP_FCL_TURN_ASSERT_INTO_EXCEPTION) - target_compile_definitions(${LIBRARY_NAME} PUBLIC -DHPP_FCL_TURN_ASSERT_INTO_EXCEPTION) +if (COAL_TURN_ASSERT_INTO_EXCEPTION) + target_compile_definitions(${LIBRARY_NAME} PUBLIC -DCOAL_TURN_ASSERT_INTO_EXCEPTION) endif() -if(HPP_FCL_HAS_QHULL) - target_compile_definitions(${LIBRARY_NAME} PRIVATE -DHPP_FCL_HAS_QHULL) - if (HPP_FCL_USE_SYSTEM_QHULL) +if(COAL_HAS_QHULL) + target_compile_definitions(${LIBRARY_NAME} PRIVATE -DCOAL_HAS_QHULL) + if (COAL_USE_SYSTEM_QHULL) target_link_libraries(${LIBRARY_NAME} PRIVATE Qhull::qhull_r Qhull::qhullcpp) else() target_include_directories(${LIBRARY_NAME} SYSTEM PRIVATE @@ -260,8 +260,8 @@ IF(octomap_FOUND) LIBRARIES ${OCTOMAP_LIBRARIES} INCLUDE_DIRS ${OCTOMAP_INCLUDE_DIRS}) target_compile_definitions (${LIBRARY_NAME} PUBLIC - -DHPP_FCL_HAS_OCTOMAP - -DHPP_FCL_HAVE_OCTOMAP + -DCOAL_HAS_OCTOMAP + -DCOAL_HAVE_OCTOMAP -DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION} -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION} -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}) diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 59e1a854e..afb0a868f 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -58,7 +58,7 @@ void BroadPhaseCollisionManager::registerObjects( //============================================================================== void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) { - HPP_FCL_UNUSED_VARIABLE(updated_obj); + COAL_UNUSED_VARIABLE(updated_obj); update(); } @@ -66,7 +66,7 @@ void BroadPhaseCollisionManager::update(CollisionObject* updated_obj) { //============================================================================== void BroadPhaseCollisionManager::update( const std::vector& updated_objs) { - HPP_FCL_UNUSED_VARIABLE(updated_objs); + COAL_UNUSED_VARIABLE(updated_objs); update(); } diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index efc82c6da..afb765e14 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -37,7 +37,7 @@ #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" -#ifdef HPP_FCL_HAVE_OCTOMAP +#ifdef COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif @@ -51,7 +51,7 @@ namespace detail { namespace dynamic_AABB_tree { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, @@ -610,8 +610,8 @@ void DynamicAABBTreeCollisionManager::update() { DynamicAABBNode* node = it->second; node->bv = obj->getAABB(); if (node->bv.volume() <= 0.) - HPP_FCL_THROW_PRETTY("The bounding volume has a negative volume.", - std::invalid_argument) + COAL_THROW_PRETTY("The bounding volume has a negative volume.", + std::invalid_argument) } dtree.refit(); @@ -666,7 +666,7 @@ void DynamicAABBTreeCollisionManager::collide( callback->init(); if (size() == 0) return; switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_collide) { const OcTree* octree = @@ -692,7 +692,7 @@ void DynamicAABBTreeCollisionManager::distance( if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_distance) { const OcTree* octree = diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 6c1e65bd7..d4f44f205 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -37,14 +37,14 @@ #include "coal/broadphase/broadphase_dynamic_AABB_tree_array.h" -#ifdef HPP_FCL_HAVE_OCTOMAP +#ifdef COAL_HAVE_OCTOMAP #include "coal/octree.h" #endif namespace coal { namespace detail { namespace dynamic_AABB_tree_array { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse_( @@ -441,7 +441,7 @@ bool selfDistanceRecurse( return false; } -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP //============================================================================== bool collisionRecurse( @@ -617,7 +617,7 @@ void DynamicAABBTreeArrayCollisionManager::collide( callback->init(); if (size() == 0) return; switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_collide) { const OcTree* octree = @@ -643,7 +643,7 @@ void DynamicAABBTreeArrayCollisionManager::distance( if (size() == 0) return; FCL_REAL min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { -#if HPP_FCL_HAVE_OCTOMAP +#if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { if (!octree_as_geometry_distance) { const OcTree* octree = diff --git a/src/collision.cpp b/src/collision.cpp index 0cc76d8e9..5b597f453 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -79,8 +79,8 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); std::size_t res; if (request.num_max_contacts == 0) { - HPP_FCL_THROW_PRETTY("Invalid number of max contacts (current value is 0).", - std::invalid_argument); + COAL_THROW_PRETTY("Invalid number of max contacts (current value is 0).", + std::invalid_argument); res = 0; } else { OBJECT_TYPE object_type1 = o1->getObjectType(); @@ -91,12 +91,12 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, if (object_type1 == OT_GEOM && (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 " - << 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); + COAL_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); res = 0; } else { res = looktable.collision_matrix[node_type2][node_type1]( @@ -107,12 +107,12 @@ 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 " - << 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); + COAL_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); res = 0; } else res = looktable.collision_matrix[node_type1][node_type2]( @@ -143,12 +143,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])) { - 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); + COAL_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/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 6a23dd856..030711cb6 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -46,7 +46,7 @@ namespace coal { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, @@ -57,9 +57,8 @@ std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, if (request.isSatisfied(result)) return result.numContacts(); if (request.security_margin < 0) - HPP_FCL_THROW_PRETTY( - "Negative security margin are not handled yet for Octree", - std::invalid_argument); + COAL_THROW_PRETTY("Negative security margin are not handled yet for Octree", + std::invalid_argument); typename TraversalTraitsCollision::CollisionTraversal_t node( request); @@ -108,7 +107,7 @@ struct COAL_LOCAL BVHShapeCollider { if (request.isSatisfied(result)) return result.numContacts(); if (request.security_margin < 0) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "Negative security margin are not handled yet for BVHModel", std::invalid_argument); @@ -661,7 +660,7 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide; collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OctreeCollide; collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OctreeCollide; diff --git a/src/collision_node.cpp b/src/collision_node.cpp index c263dc95e..93caa018e 100644 --- a/src/collision_node.cpp +++ b/src/collision_node.cpp @@ -42,21 +42,20 @@ namespace coal { void checkResultLowerBound(const CollisionResult& result, FCL_REAL sqrDistLowerBound) { - HPP_FCL_UNUSED_VARIABLE(result); + COAL_UNUSED_VARIABLE(result); const FCL_REAL dummy_precision = std::sqrt(Eigen::NumTraits::epsilon()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + COAL_UNUSED_VARIABLE(dummy_precision); if (sqrDistLowerBound == 0) { - HPP_FCL_ASSERT(result.distance_lower_bound <= dummy_precision, - "Distance lower bound should not be positive.", - std::logic_error); + COAL_ASSERT(result.distance_lower_bound <= dummy_precision, + "Distance lower bound should not be positive.", + std::logic_error); } else { - HPP_FCL_ASSERT( - result.distance_lower_bound * result.distance_lower_bound - - sqrDistLowerBound < - dummy_precision, - "Distance lower bound and sqrDistLowerBound should coincide.", - std::logic_error); + COAL_ASSERT(result.distance_lower_bound * result.distance_lower_bound - + sqrDistLowerBound < + dummy_precision, + "Distance lower bound and sqrDistLowerBound should coincide.", + std::logic_error); } } diff --git a/src/collision_node.h b/src/collision_node.h index f2780cfc5..b6be94f5d 100644 --- a/src/collision_node.h +++ b/src/collision_node.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef HPP_FCL_COLLISION_NODE_H -#define HPP_FCL_COLLISION_NODE_H +#ifndef COAL_COLLISION_NODE_H +#define COAL_COLLISION_NODE_H /// @cond INTERNAL diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp index 831040888..213075724 100644 --- a/src/collision_utility.cpp +++ b/src/collision_utility.cpp @@ -26,8 +26,8 @@ inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, const AABB& aabb) { // Ensure AABB is already computed if (model->aabb_radius < 0) - HPP_FCL_THROW_PRETTY("Collision geometry AABB should be computed first.", - std::invalid_argument); + COAL_THROW_PRETTY("Collision geometry AABB should be computed first.", + std::invalid_argument); AABB objAabb = rotate(translate(model->aabb_local, pose.getTranslation()), pose.getRotation()); if (!objAabb.overlap(aabb)) { @@ -58,8 +58,7 @@ CollisionGeometry* extractBVH(const CollisionGeometry* model, case BV_KDOP24: return extractBVHtpl >(model, pose, aabb); default: - HPP_FCL_THROW_PRETTY("Unknown type of bounding volume", - std::runtime_error); + COAL_THROW_PRETTY("Unknown type of bounding volume", std::runtime_error); } } } // namespace details @@ -71,9 +70,8 @@ CollisionGeometry* extract(const CollisionGeometry* model, return details::extractBVH(model, pose, aabb); // case OT_GEOM: return model; default: - HPP_FCL_THROW_PRETTY( - "Extraction is not implemented for this type of object", - std::runtime_error); + COAL_THROW_PRETTY("Extraction is not implemented for this type of object", + std::runtime_error); } } } // namespace coal diff --git a/src/contact_patch.cpp b/src/contact_patch.cpp index 9b6c22364..b6e0e1924 100644 --- a/src/contact_patch.cpp +++ b/src/contact_patch.cpp @@ -69,12 +69,12 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { if (!looktable.contact_patch_matrix[node_type2][node_type1]) { - HPP_FCL_THROW_PRETTY("Computing contact patches 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); + COAL_THROW_PRETTY("Computing contact patches 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); } looktable.contact_patch_matrix[node_type2][node_type1]( o2, tf2, o1, tf1, collision_result, &csolver, request, result); @@ -83,12 +83,12 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, } if (!looktable.contact_patch_matrix[node_type1][node_type2]) { - HPP_FCL_THROW_PRETTY("Contact patch computation 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); + COAL_THROW_PRETTY("Contact patch computation 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); } return looktable.contact_patch_matrix[node_type1][node_type2]( @@ -121,12 +121,12 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1, !looktable.contact_patch_matrix[node_type2][node_type1]) || (!this->swap_geoms && !looktable.contact_patch_matrix[node_type1][node_type2])) { - 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); + COAL_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 (this->swap_geoms) { diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp index fdb16217e..541915bf9 100644 --- a/src/contact_patch/contact_patch_solver.cpp +++ b/src/contact_patch/contact_patch_solver.cpp @@ -98,7 +98,7 @@ ContactPatchSolver::makeSupportSetFunction(const ShapeBase* shape, } } default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } } diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp index b1b0f90cf..e20305b37 100644 --- a/src/contact_patch_func_matrix.cpp +++ b/src/contact_patch_func_matrix.cpp @@ -49,11 +49,11 @@ struct BVHShapeComputeContactPatch { const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { - HPP_FCL_UNUSED_VARIABLE(o1); - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_UNUSED_VARIABLE(o2); - HPP_FCL_UNUSED_VARIABLE(tf2); - HPP_FCL_UNUSED_VARIABLE(csolver); + COAL_UNUSED_VARIABLE(o1); + COAL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(o2); + COAL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; @@ -74,11 +74,11 @@ struct HeightFieldShapeComputeContactPatch { const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { - HPP_FCL_UNUSED_VARIABLE(o1); - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_UNUSED_VARIABLE(o2); - HPP_FCL_UNUSED_VARIABLE(tf2); - HPP_FCL_UNUSED_VARIABLE(csolver); + COAL_UNUSED_VARIABLE(o1); + COAL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(o2); + COAL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; @@ -99,11 +99,11 @@ struct BVHComputeContactPatch { const ContactPatchSolver* csolver, const ContactPatchRequest& request, ContactPatchResult& result) { - HPP_FCL_UNUSED_VARIABLE(o1); - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_UNUSED_VARIABLE(o2); - HPP_FCL_UNUSED_VARIABLE(tf2); - HPP_FCL_UNUSED_VARIABLE(csolver); + COAL_UNUSED_VARIABLE(o1); + COAL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(o2); + COAL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(csolver); for (size_t i = 0; i < collision_result.numContacts(); ++i) { if (i >= request.max_num_patch) { break; @@ -125,12 +125,12 @@ COAL_LOCAL void contact_patch_function_not_implemented( NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); - HPP_FCL_THROW_PRETTY("Contact patch 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); + COAL_THROW_PRETTY("Contact patch 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); } ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { @@ -362,7 +362,7 @@ ContactPatchFunctionMatrix::ContactPatchFunctionMatrix() { contact_patch_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHComputeContactPatch::run; // TODO(louis): octrees -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP contact_patch_matrix[GEOM_OCTREE][GEOM_OCTREE] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_BOX] = &contact_patch_function_not_implemented; contact_patch_matrix[GEOM_OCTREE][GEOM_SPHERE] = &contact_patch_function_not_implemented; diff --git a/src/distance.cpp b/src/distance.cpp index e802f5872..a54efa3c6 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -73,12 +73,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]) { - 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); + COAL_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); @@ -88,12 +88,12 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, } } else { if (!looktable.distance_matrix[node_type1][node_type2]) { - 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); + COAL_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); @@ -122,12 +122,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])) { - 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); + COAL_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]; diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index 7e140ba96..4f37096b9 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -45,7 +45,7 @@ namespace coal { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, @@ -74,12 +74,12 @@ COAL_LOCAL FCL_REAL distance_function_not_implemented( NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); - 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); + COAL_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); } template @@ -174,14 +174,14 @@ struct COAL_LOCAL HeightFieldShapeDistancer { const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { - HPP_FCL_UNUSED_VARIABLE(o1); - HPP_FCL_UNUSED_VARIABLE(tf1); - HPP_FCL_UNUSED_VARIABLE(o2); - HPP_FCL_UNUSED_VARIABLE(tf2); - HPP_FCL_UNUSED_VARIABLE(nsolver); - HPP_FCL_UNUSED_VARIABLE(request); + COAL_UNUSED_VARIABLE(o1); + COAL_UNUSED_VARIABLE(tf1); + COAL_UNUSED_VARIABLE(o2); + COAL_UNUSED_VARIABLE(tf2); + COAL_UNUSED_VARIABLE(nsolver); + COAL_UNUSED_VARIABLE(request); // TODO(jcarpent) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "Distance between a height field and a shape is not implemented", std::invalid_argument); // if(request.isSatisfied(result)) return result.min_distance; @@ -602,7 +602,7 @@ DistanceFunctionMatrix::DistanceFunctionMatrix() { distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance; distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP distance_matrix[GEOM_OCTREE][GEOM_BOX] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Distance; distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Distance; diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp index 4efd6b7c8..2fe749808 100644 --- a/src/mesh_loader/assimp.cpp +++ b/src/mesh_loader/assimp.cpp @@ -88,11 +88,11 @@ void Loader::load(const std::string& resource_path) { std::string("Could not load resource ") + resource_path + std::string("\n") + importer->GetErrorString() + std::string("\n") + "Hint: the mesh directory may be wrong."); - HPP_FCL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument); + COAL_THROW_PRETTY(exception_message.c_str(), std::invalid_argument); } if (!scene->HasMeshes()) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( (std::string("No meshes found in file ") + resource_path).c_str(), std::invalid_argument); } diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp index 8304b4974..e7e275d4e 100644 --- a/src/mesh_loader/loader.cpp +++ b/src/mesh_loader/loader.cpp @@ -39,7 +39,7 @@ #include -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP #include "coal/octree.h" #endif @@ -84,19 +84,18 @@ BVHModelPtr_t MeshLoader::load(const std::string& filename, case BV_KDOP24: return _load >(filename, scale); default: - HPP_FCL_THROW_PRETTY("Unhandled bouding volume type.", - std::invalid_argument); + COAL_THROW_PRETTY("Unhandled bouding volume type.", + std::invalid_argument); } } CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP shared_ptr octree(new octomap::OcTree(filename)); return CollisionGeometryPtr_t(new coal::OcTree(octree)); #else - HPP_FCL_THROW_PRETTY( - "hpp-fcl compiled without OctoMap. Cannot create OcTrees.", - std::logic_error); + COAL_THROW_PRETTY("hpp-fcl compiled without OctoMap. Cannot create OcTrees.", + std::logic_error); #endif } diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index df6b5e2d6..804e6c89e 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -368,7 +368,7 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, const FCL_REAL dummy_precision = std::sqrt(Eigen::NumTraits::dummy_precision()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + COAL_UNUSED_VARIABLE(dummy_precision); assert(new_h.distance(p1) <= dummy_precision); return dist; } @@ -407,7 +407,7 @@ inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1, const FCL_REAL dummy_precision = std::sqrt(Eigen::NumTraits::dummy_precision()); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + COAL_UNUSED_VARIABLE(dummy_precision); FCL_REAL dist; if (dist1 >= dist2) { diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 450cc2f4b..863c37a43 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -58,8 +58,8 @@ void GJK::initialize() { void GJK::reset(size_t max_iterations_, FCL_REAL tolerance_) { max_iterations = max_iterations_; tolerance = tolerance_; - HPP_FCL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", - std::invalid_argument); + COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", + std::invalid_argument); status = DidNotRun; nfree = 0; simplex = nullptr; @@ -137,8 +137,8 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { vs[2]->w, vs[3]->w); break; default: - HPP_FCL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]", - std::logic_error); + COAL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]", + std::logic_error); } w0.setZero(); w1.setZero(); @@ -273,7 +273,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, break; default: - HPP_FCL_THROW_PRETTY("Invalid momentum variant.", std::logic_error); + COAL_THROW_PRETTY("Invalid momentum variant.", std::logic_error); } // see below, ray points away from origin @@ -345,7 +345,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, inside = projectTetrahedraOrigin(curr_simplex, next_simplex); break; default: - HPP_FCL_THROW_PRETTY("Invalid simplex rank", std::logic_error); + COAL_THROW_PRETTY("Invalid simplex rank", std::logic_error); } assert(nfree + next_simplex.rank == 4); current = next; @@ -395,8 +395,8 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", - std::logic_error); + COAL_THROW_PRETTY("Invalid convergence criterion type.", + std::logic_error); } } break; @@ -413,13 +413,13 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", - std::logic_error); + COAL_THROW_PRETTY("Invalid convergence criterion type.", + std::logic_error); } } break; default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error); + COAL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error); } } @@ -640,7 +640,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { const FCL_REAL dummy_precision( 3 * std::sqrt(std::numeric_limits::epsilon())); - HPP_FCL_UNUSED_VARIABLE(dummy_precision); + COAL_UNUSED_VARIABLE(dummy_precision); #define REGION_INSIDE() \ ray.setZero(); \ @@ -1527,8 +1527,8 @@ void ConvexBase::buildSupportWarmStart() { ConvexBase::num_support_warm_starts || this->support_warm_starts.indices.size() != ConvexBase::num_support_warm_starts) { - HPP_FCL_THROW_PRETTY("Wrong number of support warm starts.", - std::runtime_error); + COAL_THROW_PRETTY("Wrong number of support warm starts.", + std::runtime_error); } } diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index 636313c0c..52fbb09ff 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -150,7 +150,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( } } default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape.", std::logic_error); } } @@ -220,7 +220,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( break; } default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } } @@ -252,7 +252,7 @@ bool getNormalizeSupportDirection(const ShapeBase* shape) { return (bool)shape_traits::NeedNesterovNormalizeHeuristic; break; default: - HPP_FCL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); + COAL_THROW_PRETTY("Unsupported geometric shape", std::logic_error); } } diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp index 947ffb60e..ef9edf2f6 100644 --- a/src/narrowphase/support_functions.cpp +++ b/src/narrowphase/support_functions.cpp @@ -169,8 +169,8 @@ getShapeSupportTplInstantiation(Box) support.setZero(); } - HPP_FCL_UNUSED_VARIABLE(sphere); - HPP_FCL_UNUSED_VARIABLE(dir); + COAL_UNUSED_VARIABLE(sphere); + COAL_UNUSED_VARIABLE(dir); } // clang-format off getShapeSupportTplInstantiation(Sphere) @@ -877,7 +877,7 @@ void convexSupportSetRecurse( const Vec3f& support_dir, const FCL_REAL support_value, const Transform3f& tf, std::vector& visited, SupportSet::Polygon& polygon, FCL_REAL tol) { - HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius); + COAL_UNUSED_VARIABLE(swept_sphere_radius); if (visited[vertex_idx]) { return; diff --git a/src/octree.cpp b/src/octree.cpp index 2abfe3707..7c8abed28 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -166,7 +166,7 @@ void OcTree::exportAsObjFile(const std::string& filename) const { std::ofstream os; os.open(filename); if (!os.is_open()) - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( (std::string("failed to open file \"") + filename + std::string("\"")) .c_str(), std::runtime_error); diff --git a/src/serialization/serialization.cpp b/src/serialization/serialization.cpp index e97a00ba8..692e79ddf 100644 --- a/src/serialization/serialization.cpp +++ b/src/serialization/serialization.cpp @@ -44,41 +44,41 @@ using namespace coal; #include "coal/serialization/convex.h" #include "coal/serialization/hfield.h" #include "coal/serialization/BVH_model.h" -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP #include "coal/serialization/octree.h" #endif -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionResult) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(DistanceRequest) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(DistanceResult) +COAL_SERIALIZATION_DEFINE_EXPORT(CollisionRequest) +COAL_SERIALIZATION_DEFINE_EXPORT(CollisionResult) +COAL_SERIALIZATION_DEFINE_EXPORT(DistanceRequest) +COAL_SERIALIZATION_DEFINE_EXPORT(DistanceResult) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(ShapeBase) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(TriangleP) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Box) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Sphere) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Capsule) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cone) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Cylinder) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Halfspace) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Plane) +COAL_SERIALIZATION_DEFINE_EXPORT(ShapeBase) +COAL_SERIALIZATION_DEFINE_EXPORT(CollisionGeometry) +COAL_SERIALIZATION_DEFINE_EXPORT(TriangleP) +COAL_SERIALIZATION_DEFINE_EXPORT(Box) +COAL_SERIALIZATION_DEFINE_EXPORT(Sphere) +COAL_SERIALIZATION_DEFINE_EXPORT(Ellipsoid) +COAL_SERIALIZATION_DEFINE_EXPORT(Capsule) +COAL_SERIALIZATION_DEFINE_EXPORT(Cone) +COAL_SERIALIZATION_DEFINE_EXPORT(Cylinder) +COAL_SERIALIZATION_DEFINE_EXPORT(Halfspace) +COAL_SERIALIZATION_DEFINE_EXPORT(Plane) -#define EXPORT_AND_CAST(Derived, Base) \ - HPP_FCL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ - HPP_FCL_SERIALIZATION_DEFINE_EXPORT(Derived) \ +#define EXPORT_AND_CAST(Derived, Base) \ + COAL_SERIALIZATION_CAST_REGISTER(Derived, Base) \ + COAL_SERIALIZATION_DEFINE_EXPORT(Derived) \ /**/ -HPP_FCL_SERIALIZATION_CAST_REGISTER(ConvexBase, CollisionGeometry) +COAL_SERIALIZATION_CAST_REGISTER(ConvexBase, CollisionGeometry) EXPORT_AND_CAST(Convex, ConvexBase) EXPORT_AND_CAST(Convex, ConvexBase) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField) -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(HeightField) +COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) +COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) +COAL_SERIALIZATION_DEFINE_EXPORT(HeightField) -HPP_FCL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry) +COAL_SERIALIZATION_CAST_REGISTER(BVHModelBase, CollisionGeometry) EXPORT_AND_CAST(BVHModel, BVHModelBase) EXPORT_AND_CAST(BVHModel, BVHModelBase) @@ -89,6 +89,6 @@ EXPORT_AND_CAST(BVHModel>, BVHModelBase) EXPORT_AND_CAST(BVHModel>, BVHModelBase) EXPORT_AND_CAST(BVHModel>, BVHModelBase) -#ifdef HPP_FCL_HAS_OCTOMAP -HPP_FCL_SERIALIZATION_DEFINE_EXPORT(OcTree) +#ifdef COAL_HAS_OCTOMAP +COAL_SERIALIZATION_DEFINE_EXPORT(OcTree) #endif diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index dcfb586f6..98beb82a6 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -1,6 +1,6 @@ #include "coal/shape/convex.h" -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL #include #include #include @@ -44,19 +44,19 @@ void reorderTriangle(const Convex* convex_tri, Triangle& tri) { ConvexBase* ConvexBase::convexHull(std::shared_ptr>& pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS return ConvexBase::convexHull(pts->data(), num_points, keepTriangles, qhullCommand); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL if (num_points <= 3) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "You shouldn't use this function with less than" " 4 points.", std::invalid_argument); @@ -70,7 +70,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; - HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error); + COAL_THROW_PRETTY("Qhull failed", std::logic_error); } typedef std::size_t index_type; @@ -143,7 +143,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, } } else { if (keepTriangles) { // TODO I think there is a memory leak here. - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "You requested to keep triangles so you " "must pass option \"Qt\" to qhull via the qhull command argument.", std::invalid_argument); @@ -183,7 +183,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, for (size_t i = 0; i < static_cast(nvertex); ++i) { Neighbors& n = neighbors_[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) - HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error); + COAL_THROW_PRETTY("Too many neighbors.", std::logic_error); n.count_ = (unsigned char)nneighbors[i].size(); n.n_ = p_nneighbors; p_nneighbors = @@ -196,20 +196,20 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, convex->buildSupportWarmStart(); return convex; #else - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "Library built without qhull. Cannot build object of this type.", std::logic_error); - HPP_FCL_UNUSED_VARIABLE(pts); - HPP_FCL_UNUSED_VARIABLE(num_points); - HPP_FCL_UNUSED_VARIABLE(keepTriangles); - HPP_FCL_UNUSED_VARIABLE(qhullCommand); + COAL_UNUSED_VARIABLE(pts); + COAL_UNUSED_VARIABLE(num_points); + COAL_UNUSED_VARIABLE(keepTriangles); + COAL_UNUSED_VARIABLE(qhullCommand); #endif } -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL void ConvexBase::buildDoubleDescription() { if (num_points <= 3) { - HPP_FCL_THROW_PRETTY( + COAL_THROW_PRETTY( "You shouldn't use this function with a convex less than" " 4 points.", std::invalid_argument); @@ -222,7 +222,7 @@ void ConvexBase::buildDoubleDescription() { if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; - HPP_FCL_THROW_PRETTY("Qhull failed", std::logic_error); + COAL_THROW_PRETTY("Qhull failed", std::logic_error); } buildDoubleDescriptionFromQHullResult(qh); diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index 517ad3f35..b0b68e458 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -44,10 +44,10 @@ void ConvexBase::initialize(std::shared_ptr> points_, unsigned int num_points_) { this->points = points_; this->num_points = num_points_; - HPP_FCL_ASSERT(this->points->size() == this->num_points, - "The number of points is not consistent with the size of the " - "points vector", - std::logic_error); + COAL_ASSERT(this->points->size() == this->num_points, + "The number of points is not consistent with the size of the " + "points vector", + std::logic_error); this->num_normals_and_offsets = 0; this->normals.reset(); this->offsets.reset(); diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index f9bbd85f6..c03335b83 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -457,8 +457,8 @@ void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { template <> void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -471,8 +471,8 @@ void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { template <> void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } const Vec3f& T = tf.getTranslation(); @@ -484,8 +484,8 @@ void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { template <> void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -498,8 +498,8 @@ void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { template <> void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -513,8 +513,8 @@ template <> void computeBV(const Cylinder& s, const Transform3f& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -528,8 +528,8 @@ template <> void computeBV(const ConvexBase& s, const Transform3f& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); @@ -545,8 +545,8 @@ template <> void computeBV(const Halfspace& s, const Transform3f&, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } /// Half space can only have very rough OBB bv.axes.setIdentity(); @@ -558,8 +558,8 @@ template <> void computeBV(const Halfspace& s, const Transform3f&, RSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } /// Half space can only have very rough RSS bv.axes.setIdentity(); @@ -572,8 +572,8 @@ template <> void computeBV(const Halfspace& s, const Transform3f& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); @@ -583,8 +583,8 @@ template <> void computeBV(const Halfspace& s, const Transform3f& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } bv.num_spheres = 1; computeBV(s, tf, bv.obb); @@ -596,8 +596,8 @@ template <> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; @@ -656,8 +656,8 @@ template <> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; @@ -722,8 +722,8 @@ template <> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; @@ -802,8 +802,8 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, template <> void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Vec3f n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); @@ -820,8 +820,8 @@ void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { template <> void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Vec3f n = tf.getRotation() * s.n; @@ -841,8 +841,8 @@ template <> void computeBV(const Plane& s, const Transform3f& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); @@ -851,8 +851,8 @@ void computeBV(const Plane& s, const Transform3f& tf, template <> void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } bv.num_spheres = 1; computeBV(s, tf, bv.obb); @@ -864,8 +864,8 @@ template <> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; @@ -910,8 +910,8 @@ template <> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; @@ -958,8 +958,8 @@ template <> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { - HPP_FCL_THROW_PRETTY("Swept-sphere radius not yet supported.", - std::runtime_error); + COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", + std::runtime_error); } Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; diff --git a/src/traits_traversal.h b/src/traits_traversal.h index 45defd06d..8666d34a2 100644 --- a/src/traits_traversal.h +++ b/src/traits_traversal.h @@ -20,7 +20,7 @@ namespace coal { template struct COAL_LOCAL TraversalTraitsCollision {}; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template struct COAL_LOCAL TraversalTraitsCollision { @@ -64,7 +64,7 @@ struct COAL_LOCAL TraversalTraitsCollision, OcTree> { template struct COAL_LOCAL TraversalTraitsDistance {}; -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP template struct COAL_LOCAL TraversalTraitsDistance { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index adfdeb82b..57cfaf80c 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -14,8 +14,8 @@ macro(add_fcl_test test_name source) IF(NOT WIN32) target_compile_options(${test_name} PRIVATE "-Wno-c99-extensions") ENDIF(NOT WIN32) - if(HPP_FCL_HAS_QHULL) - target_compile_options(${test_name} PRIVATE -DHPP_FCL_HAS_QHULL) + if(COAL_HAS_QHULL) + target_compile_options(${test_name} PRIVATE -DCOAL_HAS_QHULL) endif() endmacro(add_fcl_test) @@ -59,9 +59,9 @@ add_fcl_test(profiling profiling.cpp) add_fcl_test(gjk gjk.cpp) add_fcl_test(accelerated_gjk accelerated_gjk.cpp) add_fcl_test(gjk_convergence_criterion gjk_convergence_criterion.cpp) -if(HPP_FCL_HAS_OCTOMAP) +if(COAL_HAS_OCTOMAP) add_fcl_test(octree octree.cpp) -endif(HPP_FCL_HAS_OCTOMAP) +endif(COAL_HAS_OCTOMAP) add_fcl_test(serialization serialization.cpp) diff --git a/test/convex.cpp b/test/convex.cpp index 52043cf95..dacaeaa2e 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -172,7 +172,7 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) { } } -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(convex_hull_throw) { std::shared_ptr> points( new std::vector({Vec3f(1, 1, 1), Vec3f(0, 0, 0), Vec3f(1, 0, 0)})); diff --git a/test/serialization.cpp b/test/serialization.cpp index 7c46dcb54..10bd283d1 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -59,7 +59,7 @@ COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #include "coal/serialization/archive.h" #include "coal/serialization/memory.h" -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP #include "coal/serialization/octree.h" #endif @@ -365,7 +365,7 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) { } } -#ifdef HPP_FCL_HAS_QHULL +#ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(test_Convex) { std::vector p1; std::vector t1; @@ -529,7 +529,7 @@ BOOST_AUTO_TEST_CASE(test_shapes) { } } -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP BOOST_AUTO_TEST_CASE(test_octree) { const FCL_REAL resolution = 1e-2; const Matrixx3f points = Matrixx3f::Random(1000, 3); diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index df7974e28..a109dd623 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -58,7 +58,7 @@ int line; node2_type = shape2.getNodeType(); \ line = __LINE__ -#define HPP_FCL_CHECK(cond) \ +#define COAL_CHECK(cond) \ BOOST_CHECK_MESSAGE( \ cond, "from line " << line << ", for collision pair: " \ << get_node_type_name(node1_type) << " - " \ @@ -67,17 +67,17 @@ int line; << ", ssr2 = " << shape2.getSweptSphereRadius() \ << ": " #cond) -#define HPP_FCL_CHECK_VECTOR_CLOSE(v1, v2, tol) \ - EIGEN_VECTOR_IS_APPROX(v1, v2, tol); \ - HPP_FCL_CHECK(((v1) - (v2)).isZero(tol)) +#define COAL_CHECK_VECTOR_CLOSE(v1, v2, tol) \ + EIGEN_VECTOR_IS_APPROX(v1, v2, tol); \ + COAL_CHECK(((v1) - (v2)).isZero(tol)) -#define HPP_FCL_CHECK_REAL_CLOSE(v1, v2, tol) \ - FCL_REAL_IS_APPROX(v1, v2, tol); \ - HPP_FCL_CHECK(std::abs((v1) - (v2)) < tol) +#define COAL_CHECK_REAL_CLOSE(v1, v2, tol) \ + FCL_REAL_IS_APPROX(v1, v2, tol); \ + COAL_CHECK(std::abs((v1) - (v2)) < tol) -#define HPP_FCL_CHECK_CONDITION(cond) \ - BOOST_CHECK(cond); \ - HPP_FCL_CHECK(cond) +#define COAL_CHECK_CONDITION(cond) \ + BOOST_CHECK(cond); \ + COAL_CHECK(cond) // Preambule: swept sphere radius allows to virually convolve geometric shapes // by a sphere with positive radius (Minkowski sum). @@ -188,16 +188,16 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { shape2.getSweptSphereRadius()); // Check that the distance is the same - HPP_FCL_CHECK_REAL_CLOSE(distance[0], distance[1], precision); + COAL_CHECK_REAL_CLOSE(distance[0], distance[1], precision); // Check that the normal is the same - HPP_FCL_CHECK_CONDITION(normal[0].dot(normal[1]) > 0); - HPP_FCL_CHECK_CONDITION(std::abs(1 - normal[0].dot(normal[1])) < - precision); + COAL_CHECK_CONDITION(normal[0].dot(normal[1]) > 0); + COAL_CHECK_CONDITION(std::abs(1 - normal[0].dot(normal[1])) < + precision); // Check that the witness points are the same - HPP_FCL_CHECK_VECTOR_CLOSE(p1[0], p1[1], precision); - HPP_FCL_CHECK_VECTOR_CLOSE(p2[0], p2[1], precision); + COAL_CHECK_VECTOR_CLOSE(p1[0], p1[1], precision); + COAL_CHECK_VECTOR_CLOSE(p2[0], p2[1], precision); } } } @@ -338,21 +338,20 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { const FCL_REAL ssr = ssr1 + ssr2; // Check that the distance is the same - HPP_FCL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr, - contact[1].penetration_depth, precision); + COAL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr, + contact[1].penetration_depth, precision); // Check that the normal is the same - HPP_FCL_CHECK_CONDITION((contact[0].normal).dot(contact[1].normal) > - 0); - HPP_FCL_CHECK_CONDITION( + COAL_CHECK_CONDITION((contact[0].normal).dot(contact[1].normal) > 0); + COAL_CHECK_CONDITION( std::abs(1 - (contact[0].normal).dot(contact[1].normal)) < precision); // Check that the witness points are the same - HPP_FCL_CHECK_VECTOR_CLOSE( + COAL_CHECK_VECTOR_CLOSE( contact[0].nearest_points[0] + ssr1 * contact[0].normal, contact[1].nearest_points[0], precision); - HPP_FCL_CHECK_VECTOR_CLOSE( + COAL_CHECK_VECTOR_CLOSE( contact[0].nearest_points[1] - ssr2 * contact[0].normal, contact[1].nearest_points[1], precision); } diff --git a/test/utility.cpp b/test/utility.cpp index 5efd2ca1e..113902690 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -181,7 +181,7 @@ void saveOBJFile(const char* filename, std::vector& points, os.close(); } -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) { octomap::OcTreePtr_t octree(new octomap::OcTree(filename)); if (octree->getResolution() != resolution) { @@ -606,10 +606,10 @@ Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size) { std::shared_ptr makeRandomGeometry(NODE_TYPE node_type) { switch (node_type) { case GEOM_TRIANGLE: - HPP_FCL_THROW_PRETTY(std::string(HPP_FCL_PRETTY_FUNCTION) + " for " + - std::string(get_node_type_name(node_type)) + - " is not yet implemented.", - std::invalid_argument); + COAL_THROW_PRETTY(std::string(COAL_PRETTY_FUNCTION) + " for " + + std::string(get_node_type_name(node_type)) + + " is not yet implemented.", + std::invalid_argument); break; case GEOM_BOX: return std::make_shared(makeRandomBox(0.1, 1.0)); @@ -641,9 +641,9 @@ std::shared_ptr makeRandomGeometry(NODE_TYPE node_type) { return std::make_shared(makeRandomHalfspace(0.1, 1.0)); break; default: - HPP_FCL_THROW_PRETTY(std::string(get_node_type_name(node_type)) + - " is not a primitive shape.", - std::invalid_argument); + COAL_THROW_PRETTY(std::string(get_node_type_name(node_type)) + + " is not a primitive shape.", + std::invalid_argument); break; } } diff --git a/test/utility.h b/test/utility.h index 444f8aa54..78f4b05b9 100644 --- a/test/utility.h +++ b/test/utility.h @@ -35,8 +35,8 @@ /** \author Jia Pan */ -#ifndef TEST_HPP_FCL_UTILITY_H -#define TEST_HPP_FCL_UTILITY_H +#ifndef TEST_COAL_UTILITY_H +#define TEST_COAL_UTILITY_H #include "coal/math/transform.h" #include "coal/collision_data.h" @@ -44,7 +44,7 @@ #include "coal/broadphase/default_broadphase_callbacks.h" #include "coal/shape/convex.h" -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP #include "coal/octree.h" #endif @@ -78,7 +78,7 @@ << Vb << "\n]") namespace octomap { -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP typedef coal::shared_ptr OcTreePtr_t; #endif } // namespace octomap @@ -137,7 +137,7 @@ void loadOBJFile(const char* filename, std::vector& points, void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); -#ifdef HPP_FCL_HAS_OCTOMAP +#ifdef COAL_HAS_OCTOMAP coal::OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution); #endif From 9708caa5d6ac76264985af8b63df756c640175e0 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 19 Jun 2024 12:33:39 +0200 Subject: [PATCH 07/57] python: rename `fcl.hh` to `coal.hh` --- python/CMakeLists.txt | 4 ++-- python/broadphase/broadphase.cc | 2 +- python/broadphase/broadphase_callbacks.hh | 2 +- python/broadphase/broadphase_collision_manager.hh | 2 +- python/{fcl.cc => coal.cc} | 2 +- python/{fcl.hh => coal.hh} | 0 python/collision-geometries.cc | 2 +- python/collision.cc | 2 +- python/contact_patch.cc | 2 +- python/distance.cc | 2 +- python/fwd.hh | 2 +- python/gjk.cc | 2 +- python/math.cc | 2 +- python/octree.cc | 2 +- python/version.cc | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) rename python/{fcl.cc => coal.cc} (99%) rename python/{fcl.hh => coal.hh} (100%) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 3fb00ae6a..c279a22d4 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -43,7 +43,7 @@ SET(LIBRARY_NAME hppfcl) SET(${LIBRARY_NAME}_HEADERS fwd.hh - fcl.hh + coal.hh deprecation.hh broadphase/fwd.hh broadphase/broadphase_collision_manager.hh @@ -108,7 +108,7 @@ SET(${LIBRARY_NAME}_SOURCES collision.cc contact_patch.cc distance.cc - fcl.cc + coal.cc gjk.cc broadphase/broadphase.cc ) diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index afcaab456..57da90761 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -33,7 +33,7 @@ // POSSIBILITY OF SUCH DAMAGE. #include "coal/fwd.hh" -#include "../fcl.hh" +#include "../coal.hh" #include "../utils/std-pair.hh" #include "coal/broadphase/broadphase_dynamic_AABB_tree.h" diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index 49b99d4fc..f591fee9f 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -40,7 +40,7 @@ #include "coal/fwd.hh" #include "coal/broadphase/broadphase_callbacks.h" -#include "../fcl.hh" +#include "../coal.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index 5ccf40264..7ea9669dc 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -41,7 +41,7 @@ #include "coal/broadphase/broadphase_collision_manager.h" #include "coal/broadphase/default_broadphase_callbacks.h" -#include "../fcl.hh" +#include "../coal.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" diff --git a/python/fcl.cc b/python/coal.cc similarity index 99% rename from python/fcl.cc rename to python/coal.cc index 08bdf4016..9b3761e45 100644 --- a/python/fcl.cc +++ b/python/coal.cc @@ -34,7 +34,7 @@ #include -#include "fcl.hh" +#include "coal.hh" #include "coal/fwd.hh" #include "coal/shape/geometric_shapes.h" diff --git a/python/fcl.hh b/python/coal.hh similarity index 100% rename from python/fcl.hh rename to python/coal.hh diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index db33967ed..b44c7213c 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -35,7 +35,7 @@ #include #include -#include "fcl.hh" +#include "coal.hh" #include "deprecation.hh" #include "coal/fwd.hh" diff --git a/python/collision.cc b/python/collision.cc index 7717943c4..88eb4d0b4 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -41,7 +41,7 @@ COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #include "coal/serialization/collision_data.h" COAL_COMPILER_DIAGNOSTIC_POP -#include "fcl.hh" +#include "coal.hh" #include "deprecation.hh" #include "serializable.hh" diff --git a/python/contact_patch.cc b/python/contact_patch.cc index ed6a650d0..8ac35cbdf 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -38,7 +38,7 @@ #include "coal/contact_patch.h" #include "coal/serialization/collision_data.h" -#include "fcl.hh" +#include "coal.hh" #include "deprecation.hh" #include "serializable.hh" diff --git a/python/distance.cc b/python/distance.cc index 3cd90b19b..4f229ff6a 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -34,7 +34,7 @@ #include -#include "fcl.hh" +#include "coal.hh" COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS diff --git a/python/fwd.hh b/python/fwd.hh index 0a688d67f..62a197e19 100644 --- a/python/fwd.hh +++ b/python/fwd.hh @@ -8,7 +8,7 @@ #include "coal/fwd.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC namespace doxygen { -using hpp::fcl::shared_ptr; +using coal::shared_ptr; } #endif diff --git a/python/gjk.cc b/python/gjk.cc index c69779561..256b32eec 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -34,7 +34,7 @@ #include -#include "fcl.hh" +#include "coal.hh" #include "coal/fwd.hh" #include "coal/narrowphase/gjk.h" diff --git a/python/math.cc b/python/math.cc index dd93e6e3d..b8ef731ff 100644 --- a/python/math.cc +++ b/python/math.cc @@ -39,7 +39,7 @@ #include "coal/math/transform.h" #include "coal/serialization/transform.h" -#include "fcl.hh" +#include "coal.hh" #include "pickle.hh" #include "serializable.hh" diff --git a/python/octree.cc b/python/octree.cc index f8cddc926..73781f458 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -1,5 +1,5 @@ -#include "fcl.hh" +#include "coal.hh" #include diff --git a/python/version.cc b/python/version.cc index 153391eb0..0b3b1d4ce 100644 --- a/python/version.cc +++ b/python/version.cc @@ -3,7 +3,7 @@ // #include "coal/config.hh" -#include "fcl.hh" +#include "coal.hh" #include namespace bp = boost::python; From 5abd6756247985f9fa04676acc7b30a82f1f1f08 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 19 Jun 2024 15:58:02 +0200 Subject: [PATCH 08/57] python: rename python lib to `coal` --- python/CMakeLists.txt | 56 ++++++------- .../broadphase_collision_manager.hh | 2 +- python/coal.cc | 2 +- python/{hppfcl => coal}/__init__.py | 4 +- python/{hppfcl => coal}/viewer.py | 20 ++--- test/python_unit/CMakeLists.txt | 2 +- test/python_unit/api.py | 26 +++--- test/python_unit/collision.py | 42 +++++----- test/python_unit/collision_manager.py | 18 ++--- test/python_unit/geometric_shapes.py | 80 +++++++++---------- test/python_unit/pickling.py | 30 +++---- 11 files changed, 142 insertions(+), 140 deletions(-) rename python/{hppfcl => coal}/__init__.py (95%) rename python/{hppfcl => coal}/viewer.py (87%) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c279a22d4..95560122d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -39,9 +39,9 @@ ADD_CUSTOM_TARGET(python) SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) # Name of the Python library -SET(LIBRARY_NAME hppfcl) +SET(PYTHON_LIB_NAME ${PROJECT_NAME}_pywrap) -SET(${LIBRARY_NAME}_HEADERS +SET(${PYTHON_LIB_NAME}_HEADERS fwd.hh coal.hh deprecation.hh @@ -96,12 +96,12 @@ IF(ENABLE_DOXYGEN_AUTODOC) ) ADD_DEPENDENCIES(generate_doxygen_cpp_doc doc) - LIST(APPEND ${LIBRARY_NAME}_HEADERS + LIST(APPEND ${PYTHON_LIB_NAME}_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh ) ENDIF() -SET(${LIBRARY_NAME}_SOURCES +SET(${PYTHON_LIB_NAME}_SOURCES version.cc math.cc collision-geometries.cc @@ -114,34 +114,34 @@ SET(${LIBRARY_NAME}_SOURCES ) IF(COAL_HAS_OCTOMAP) - LIST(APPEND ${LIBRARY_NAME}_SOURCES octree.cc) + LIST(APPEND ${PYTHON_LIB_NAME}_SOURCES octree.cc) ENDIF(COAL_HAS_OCTOMAP) -ADD_LIBRARY(${LIBRARY_NAME} MODULE ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_HEADERS}) -TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS}) -TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} PRIVATE "${CMAKE_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}") +ADD_LIBRARY(${PYTHON_LIB_NAME} MODULE ${${PYTHON_LIB_NAME}_SOURCES} ${${PYTHON_LIB_NAME}_HEADERS}) +TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS}) +TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} PRIVATE "${CMAKE_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}") IF(WIN32) - TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC ${PYTHON_LIBRARY}) + TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY}) ENDIF(WIN32) -ADD_DEPENDENCIES(python ${LIBRARY_NAME}) -ADD_HEADER_GROUP(${LIBRARY_NAME}_HEADERS) -ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES) +ADD_DEPENDENCIES(python ${PYTHON_LIB_NAME}) +ADD_HEADER_GROUP(${PYTHON_LIB_NAME}_HEADERS) +ADD_SOURCE_GROUP(${PYTHON_LIB_NAME}_SOURCES) IF(ENABLE_DOXYGEN_AUTODOC) - ADD_DEPENDENCIES(${LIBRARY_NAME} generate_doxygen_cpp_doc) - TARGET_COMPILE_DEFINITIONS(${LIBRARY_NAME} PRIVATE -DCOAL_HAS_DOXYGEN_AUTODOC) + ADD_DEPENDENCIES(${PYTHON_LIB_NAME} generate_doxygen_cpp_doc) + TARGET_COMPILE_DEFINITIONS(${PYTHON_LIB_NAME} PRIVATE -DCOAL_HAS_DOXYGEN_AUTODOC) ENDIF() -TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC +TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy Boost::system) -SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES +SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES PREFIX "" SUFFIX "${PYTHON_EXT_SUFFIX}" - LIBRARY_OUTPUT_NAME ${LIBRARY_NAME} - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${LIBRARY_NAME}" + OUTPUT_NAME "${PYTHON_LIB_NAME}" + LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}" ) IF(IS_ABSOLUTE ${PYTHON_SITELIB}) @@ -149,19 +149,21 @@ IF(IS_ABSOLUTE ${PYTHON_SITELIB}) ELSE() SET(ABSOLUTE_PYTHON_SITELIB ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}) ENDIF() -SET(${LIBRARY_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${LIBRARY_NAME}) +SET(${PYTHON_LIB_NAME}_INSTALL_DIR ${ABSOLUTE_PYTHON_SITELIB}/${PROJECT_NAME}) IF(UNIX) - GET_RELATIVE_RPATH(${${LIBRARY_NAME}_INSTALL_DIR} ${LIBRARY_NAME}_INSTALL_RPATH) - SET_TARGET_PROPERTIES(${LIBRARY_NAME} PROPERTIES INSTALL_RPATH "${${LIBRARY_NAME}_INSTALL_RPATH}") + GET_RELATIVE_RPATH(${${PYTHON_LIB_NAME}_INSTALL_DIR} ${PYTHON_LIB_NAME}_INSTALL_RPATH) + SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES INSTALL_RPATH "${${PYTHON_LIB_NAME}_INSTALL_RPATH}") ENDIF() -INSTALL(TARGETS ${LIBRARY_NAME} - DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR}) +INSTALL(TARGETS ${PYTHON_LIB_NAME} + EXPORT ${TARGETS_EXPORT_NAME} + DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR}) # --- GENERATE STUBS IF(GENERATE_PYTHON_STUBS) LOAD_STUBGEN() - GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${LIBRARY_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${LIBRARY_NAME} ${PROJECT_NAME}) + GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${PYTHON_LIB_NAME} + ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME} ${PROJECT_NAME}) ENDIF(GENERATE_PYTHON_STUBS) # --- INSTALL SCRIPTS @@ -171,8 +173,8 @@ SET(PYTHON_FILES ) FOREACH(python ${PYTHON_FILES}) - PYTHON_BUILD(${LIBRARY_NAME} ${python}) + PYTHON_BUILD(${PROJECT_NAME} ${python}) INSTALL(FILES - "${CMAKE_CURRENT_SOURCE_DIR}/${LIBRARY_NAME}/${python}" - DESTINATION ${${LIBRARY_NAME}_INSTALL_DIR}) + "${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}/${python}" + DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR}) ENDFOREACH(python) diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index 7ea9669dc..3dbcb5c1b 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -215,7 +215,7 @@ struct BroadPhaseCollisionManagerWrapper template static void exposeDerived() { std::string class_name = boost::typeindex::type_id().pretty_name(); - boost::algorithm::replace_all(class_name, "hpp::fcl::", ""); + boost::algorithm::replace_all(class_name, "coal::", ""); #if defined(WIN32) boost::algorithm::replace_all(class_name, "class ", ""); #endif diff --git a/python/coal.cc b/python/coal.cc index 9b3761e45..b792f0c70 100644 --- a/python/coal.cc +++ b/python/coal.cc @@ -83,7 +83,7 @@ void exposeMeshLoader() { } } -BOOST_PYTHON_MODULE(hppfcl) { +BOOST_PYTHON_MODULE(coal_pywrap) { namespace bp = boost::python; PyImport_ImportModule("warnings"); diff --git a/python/hppfcl/__init__.py b/python/coal/__init__.py similarity index 95% rename from python/hppfcl/__init__.py rename to python/coal/__init__.py index b93784303..e26c55ab9 100644 --- a/python/hppfcl/__init__.py +++ b/python/coal/__init__.py @@ -32,5 +32,5 @@ # POSSIBILITY OF SUCH DAMAGE. # ruff: noqa: F401, F403 -from .hppfcl import * -from .hppfcl import __raw_version__, __version__ +from .coal_pywrap import * +from .coal_pywrap import __raw_version__, __version__ diff --git a/python/hppfcl/viewer.py b/python/coal/viewer.py similarity index 87% rename from python/hppfcl/viewer.py rename to python/coal/viewer.py index 15bf53c2d..bef54e2c0 100644 --- a/python/hppfcl/viewer.py +++ b/python/coal/viewer.py @@ -8,7 +8,7 @@ import numpy as np from gepetto import Color -import hppfcl +import coal def applyConfiguration(gui, name, tf): @@ -18,18 +18,18 @@ def applyConfiguration(gui, name, tf): def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)): - if isinstance(geom, hppfcl.Capsule): + if isinstance(geom, coal.Capsule): return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color) - elif isinstance(geom, hppfcl.Cylinder): + elif isinstance(geom, coal.Cylinder): return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color) - elif isinstance(geom, hppfcl.Box): + elif isinstance(geom, coal.Box): w, h, d = (2.0 * geom.halfSide).tolist() return gui.addBox(name, w, h, d, color) - elif isinstance(geom, hppfcl.Sphere): + elif isinstance(geom, coal.Sphere): return gui.addSphere(name, geom.radius, color) - elif isinstance(geom, hppfcl.Cone): + elif isinstance(geom, coal.Cone): return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color) - elif isinstance(geom, hppfcl.Convex): + elif isinstance(geom, coal.Convex): pts = [ geom.points(geom.polygons(f)[i]).tolist() for f in range(geom.num_polygons) @@ -40,7 +40,7 @@ def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)): gui.setLightingMode(name, "ON") gui.setBoolProperty(name, "BackfaceDrawing", True) return True - elif isinstance(geom, hppfcl.ConvexBase): + elif isinstance(geom, coal.ConvexBase): pts = [geom.points(i).tolist() for i in range(geom.num_points)] gui.addCurve(name, pts, color) gui.setCurveMode(name, "POINTS") @@ -73,7 +73,7 @@ def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True gui.applyConfiguration( n, res.getNearestPoint1().tolist() - + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal) + + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal) .coeffs() .tolist(), ) @@ -101,7 +101,7 @@ def displayCollisionResult(gui, group_name, res, color=Color.green): gui.applyConfiguration( n, (P - depth * N / 2).tolist() - + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N) + + coal.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N) .coeffs() .tolist(), ) diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt index 8f7e098e0..4c59925a0 100644 --- a/test/python_unit/CMakeLists.txt +++ b/test/python_unit/CMakeLists.txt @@ -6,7 +6,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS pickling ) -ADD_DEPENDENCIES(build_tests hppfcl) +ADD_DEPENDENCIES(build_tests ${PROJECT_NAME}_pywrap) FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) ADD_PYTHON_UNIT_TEST("py-${TEST}" "test/python_unit/${TEST}.py" "python") ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) diff --git a/test/python_unit/api.py b/test/python_unit/api.py index d982dc611..ac7331d46 100644 --- a/test/python_unit/api.py +++ b/test/python_unit/api.py @@ -1,30 +1,30 @@ import unittest from test_case import TestCase -import hppfcl +import coal import numpy as np class TestMainAPI(TestCase): def test_collision(self): - capsule = hppfcl.Capsule(1.0, 2.0) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0])) + capsule = coal.Capsule(1.0, 2.0) + M1 = coal.Transform3f() + M2 = coal.Transform3f(np.eye(3), np.array([3, 0, 0])) - req = hppfcl.CollisionRequest() - res = hppfcl.CollisionResult() + req = coal.CollisionRequest() + res = coal.CollisionResult() - self.assertTrue(not hppfcl.collide(capsule, M1, capsule, M2, req, res)) + self.assertTrue(not coal.collide(capsule, M1, capsule, M2, req, res)) def test_distance(self): - capsule = hppfcl.Capsule(1.0, 2.0) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([3, 0, 0])) + capsule = coal.Capsule(1.0, 2.0) + M1 = coal.Transform3f() + M2 = coal.Transform3f(np.eye(3), np.array([3, 0, 0])) - req = hppfcl.DistanceRequest() - res = hppfcl.DistanceResult() + req = coal.DistanceRequest() + res = coal.DistanceResult() - self.assertTrue(hppfcl.distance(capsule, M1, capsule, M2, req, res) > 0) + self.assertTrue(coal.distance(capsule, M1, capsule, M2, req, res) > 0) if __name__ == "__main__": diff --git a/test/python_unit/collision.py b/test/python_unit/collision.py index d14ece98f..61cdcd243 100644 --- a/test/python_unit/collision.py +++ b/test/python_unit/collision.py @@ -1,47 +1,47 @@ import unittest from test_case import TestCase -import hppfcl +import coal import numpy as np def tetahedron(): - pts = hppfcl.StdVec_Vec3f() + pts = coal.StdVec_Vec3f() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) pts.append(np.array((0, 0, 1))) - tri = hppfcl.StdVec_Triangle() - tri.append(hppfcl.Triangle(0, 1, 2)) - tri.append(hppfcl.Triangle(0, 1, 3)) - tri.append(hppfcl.Triangle(0, 2, 3)) - tri.append(hppfcl.Triangle(1, 2, 3)) - return hppfcl.Convex(pts, tri) + tri = coal.StdVec_Triangle() + tri.append(coal.Triangle(0, 1, 2)) + tri.append(coal.Triangle(0, 1, 3)) + tri.append(coal.Triangle(0, 2, 3)) + tri.append(coal.Triangle(1, 2, 3)) + return coal.Convex(pts, tri) class TestMainAPI(TestCase): def test_convex_halfspace(self): convex = tetahedron() - halfspace = hppfcl.Halfspace(np.array((0, 0, 1)), 0) + halfspace = coal.Halfspace(np.array((0, 0, 1)), 0) - req = hppfcl.CollisionRequest() - res = hppfcl.CollisionResult() + req = coal.CollisionRequest() + res = coal.CollisionResult() - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, -0.1])) + M1 = coal.Transform3f() + M2 = coal.Transform3f(np.eye(3), np.array([0, 0, -0.1])) res.clear() - hppfcl.collide(convex, M1, halfspace, M2, req, res) - self.assertFalse(hppfcl.collide(convex, M1, halfspace, M2, req, res)) + coal.collide(convex, M1, halfspace, M2, req, res) + self.assertFalse(coal.collide(convex, M1, halfspace, M2, req, res)) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 0.1])) + M1 = coal.Transform3f() + M2 = coal.Transform3f(np.eye(3), np.array([0, 0, 0.1])) res.clear() - self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res)) + self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) - M1 = hppfcl.Transform3f() - M2 = hppfcl.Transform3f(np.eye(3), np.array([0, 0, 2])) + M1 = coal.Transform3f() + M2 = coal.Transform3f(np.eye(3), np.array([0, 0, 2])) res.clear() - self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res)) + self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) if __name__ == "__main__": diff --git a/test/python_unit/collision_manager.py b/test/python_unit/collision_manager.py index 46f7fa925..9dfe8474f 100644 --- a/test/python_unit/collision_manager.py +++ b/test/python_unit/collision_manager.py @@ -1,21 +1,21 @@ -import hppfcl as fcl +import coal import numpy as np -sphere = fcl.Sphere(0.5) -sphere_obj = fcl.CollisionObject(sphere) +sphere = coal.Sphere(0.5) +sphere_obj = coal.CollisionObject(sphere) -M_sphere = fcl.Transform3f.Identity() +M_sphere = coal.Transform3f.Identity() M_sphere.setTranslation(np.array([-0.6, 0.0, 0.0])) sphere_obj.setTransform(M_sphere) -box = fcl.Box(np.array([0.5, 0.5, 0.5])) -box_obj = fcl.CollisionObject(box) +box = coal.Box(np.array([0.5, 0.5, 0.5])) +box_obj = coal.CollisionObject(box) -M_box = fcl.Transform3f.Identity() +M_box = coal.Transform3f.Identity() M_box.setTranslation(np.array([-0.6, 0.0, 0.0])) box_obj.setTransform(M_box) -collision_manager = fcl.DynamicAABBTreeCollisionManager() +collision_manager = coal.DynamicAABBTreeCollisionManager() collision_manager.registerObject(sphere_obj) collision_manager.registerObject(box_obj) @@ -24,7 +24,7 @@ collision_manager.setup() # Perform collision detection -callback = fcl.CollisionCallBackDefault() +callback = coal.CollisionCallBackDefault() collision_manager.collide(sphere_obj, callback) assert callback.data.result.numContacts() == 1 diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py index 4d6735b10..57e03e21c 100644 --- a/test/python_unit/geometric_shapes.py +++ b/test/python_unit/geometric_shapes.py @@ -1,16 +1,16 @@ import unittest from test_case import TestCase -import hppfcl +import coal import numpy as np class TestGeometricShapes(TestCase): def test_capsule(self): - capsule = hppfcl.Capsule(1.0, 2.0) - self.assertIsInstance(capsule, hppfcl.Capsule) - self.assertIsInstance(capsule, hppfcl.ShapeBase) - self.assertIsInstance(capsule, hppfcl.CollisionGeometry) - self.assertEqual(capsule.getNodeType(), hppfcl.NODE_TYPE.GEOM_CAPSULE) + capsule = coal.Capsule(1.0, 2.0) + self.assertIsInstance(capsule, coal.Capsule) + self.assertIsInstance(capsule, coal.ShapeBase) + self.assertIsInstance(capsule, coal.CollisionGeometry) + self.assertEqual(capsule.getNodeType(), coal.NODE_TYPE.GEOM_CAPSULE) self.assertEqual(capsule.radius, 1.0) self.assertEqual(capsule.halfLength, 1.0) capsule.radius = 3.0 @@ -47,11 +47,11 @@ def test_capsule(self): self.assertApprox(Ic, I0_ref) def test_box1(self): - box = hppfcl.Box(np.array([1.0, 2.0, 3.0])) - self.assertIsInstance(box, hppfcl.Box) - self.assertIsInstance(box, hppfcl.ShapeBase) - self.assertIsInstance(box, hppfcl.CollisionGeometry) - self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX) + box = coal.Box(np.array([1.0, 2.0, 3.0])) + self.assertIsInstance(box, coal.Box) + self.assertIsInstance(box, coal.ShapeBase) + self.assertIsInstance(box, coal.CollisionGeometry) + self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX) self.assertTrue(np.array_equal(box.halfSide, np.array([0.5, 1.0, 1.5]))) box.halfSide = np.array([4.0, 5.0, 6.0]) self.assertTrue(np.array_equal(box.halfSide, np.array([4.0, 5.0, 6.0]))) @@ -73,21 +73,21 @@ def test_box1(self): self.assertApprox(Ic, I0_ref) def test_box2(self): - box = hppfcl.Box(1.0, 2.0, 3) - self.assertIsInstance(box, hppfcl.Box) - self.assertIsInstance(box, hppfcl.ShapeBase) - self.assertIsInstance(box, hppfcl.CollisionGeometry) - self.assertEqual(box.getNodeType(), hppfcl.NODE_TYPE.GEOM_BOX) + box = coal.Box(1.0, 2.0, 3) + self.assertIsInstance(box, coal.Box) + self.assertIsInstance(box, coal.ShapeBase) + self.assertIsInstance(box, coal.CollisionGeometry) + self.assertEqual(box.getNodeType(), coal.NODE_TYPE.GEOM_BOX) self.assertEqual(box.halfSide[0], 0.5) self.assertEqual(box.halfSide[1], 1.0) self.assertEqual(box.halfSide[2], 1.5) def test_sphere(self): - sphere = hppfcl.Sphere(1.0) - self.assertIsInstance(sphere, hppfcl.Sphere) - self.assertIsInstance(sphere, hppfcl.ShapeBase) - self.assertIsInstance(sphere, hppfcl.CollisionGeometry) - self.assertEqual(sphere.getNodeType(), hppfcl.NODE_TYPE.GEOM_SPHERE) + sphere = coal.Sphere(1.0) + self.assertIsInstance(sphere, coal.Sphere) + self.assertIsInstance(sphere, coal.ShapeBase) + self.assertIsInstance(sphere, coal.CollisionGeometry) + self.assertEqual(sphere.getNodeType(), coal.NODE_TYPE.GEOM_SPHERE) self.assertEqual(sphere.radius, 1.0) sphere.radius = 2.0 self.assertEqual(sphere.radius, 2.0) @@ -103,11 +103,11 @@ def test_sphere(self): self.assertApprox(Ic, I0_ref) def test_cylinder(self): - cylinder = hppfcl.Cylinder(1.0, 2.0) - self.assertIsInstance(cylinder, hppfcl.Cylinder) - self.assertIsInstance(cylinder, hppfcl.ShapeBase) - self.assertIsInstance(cylinder, hppfcl.CollisionGeometry) - self.assertEqual(cylinder.getNodeType(), hppfcl.NODE_TYPE.GEOM_CYLINDER) + cylinder = coal.Cylinder(1.0, 2.0) + self.assertIsInstance(cylinder, coal.Cylinder) + self.assertIsInstance(cylinder, coal.ShapeBase) + self.assertIsInstance(cylinder, coal.CollisionGeometry) + self.assertEqual(cylinder.getNodeType(), coal.NODE_TYPE.GEOM_CYLINDER) self.assertEqual(cylinder.radius, 1.0) self.assertEqual(cylinder.halfLength, 1.0) cylinder.radius = 3.0 @@ -128,11 +128,11 @@ def test_cylinder(self): self.assertApprox(Ic, I0_ref) def test_cone(self): - cone = hppfcl.Cone(1.0, 2.0) - self.assertIsInstance(cone, hppfcl.Cone) - self.assertIsInstance(cone, hppfcl.ShapeBase) - self.assertIsInstance(cone, hppfcl.CollisionGeometry) - self.assertEqual(cone.getNodeType(), hppfcl.NODE_TYPE.GEOM_CONE) + cone = coal.Cone(1.0, 2.0) + self.assertIsInstance(cone, coal.Cone) + self.assertIsInstance(cone, coal.ShapeBase) + self.assertIsInstance(cone, coal.CollisionGeometry) + self.assertEqual(cone.getNodeType(), coal.NODE_TYPE.GEOM_CONE) self.assertEqual(cone.radius, 1.0) self.assertEqual(cone.halfLength, 1.0) cone.radius = 3.0 @@ -155,13 +155,13 @@ def test_cone(self): self.assertApprox(Ic, Ic_ref) def test_BVH(self): - bvh = hppfcl.BVHModelOBBRSS() + bvh = coal.BVHModelOBBRSS() self.assertEqual(bvh.num_vertices, 0) self.assertEqual(bvh.vertices().shape, (0, 3)) def test_convex(self): - verts = hppfcl.StdVec_Vec3f() - faces = hppfcl.StdVec_Triangle() + verts = coal.StdVec_Vec3f() + faces = coal.StdVec_Triangle() verts.extend( [ np.array([0, 0, 0]), @@ -169,12 +169,12 @@ def test_convex(self): np.array([1, 0, 0]), ] ) - faces.append(hppfcl.Triangle(0, 1, 2)) - hppfcl.Convex(verts, faces) + faces.append(coal.Triangle(0, 1, 2)) + coal.Convex(verts, faces) verts.append(np.array([0, 0, 1])) try: - hppfcl.Convex.convexHull(verts, False, None) + coal.Convex.convexHull(verts, False, None) qhullAvailable = True except Exception as e: self.assertIn( @@ -183,11 +183,11 @@ def test_convex(self): qhullAvailable = False if qhullAvailable: - hppfcl.Convex.convexHull(verts, False, "") - hppfcl.Convex.convexHull(verts, True, "") + coal.Convex.convexHull(verts, False, "") + coal.Convex.convexHull(verts, True, "") try: - hppfcl.Convex.convexHull(verts[:3], False, None) + coal.Convex.convexHull(verts[:3], False, None) except Exception as e: self.assertIn( "You shouldn't use this function with less than 4 points.", str(e) diff --git a/test/python_unit/pickling.py b/test/python_unit/pickling.py index 1e20fe393..8a94bde07 100644 --- a/test/python_unit/pickling.py +++ b/test/python_unit/pickling.py @@ -1,23 +1,23 @@ import unittest from test_case import TestCase -import hppfcl +import coal import pickle import numpy as np def tetahedron(): - pts = hppfcl.StdVec_Vec3f() + pts = coal.StdVec_Vec3f() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) pts.append(np.array((0, 0, 1))) - tri = hppfcl.StdVec_Triangle() - tri.append(hppfcl.Triangle(0, 1, 2)) - tri.append(hppfcl.Triangle(0, 1, 3)) - tri.append(hppfcl.Triangle(0, 2, 3)) - tri.append(hppfcl.Triangle(1, 2, 3)) - return hppfcl.Convex(pts, tri) + tri = coal.StdVec_Triangle() + tri.append(coal.Triangle(0, 1, 2)) + tri.append(coal.Triangle(0, 1, 3)) + tri.append(coal.Triangle(0, 2, 3)) + tri.append(coal.Triangle(1, 2, 3)) + return coal.Convex(pts, tri) class TestGeometryPickling(TestCase): @@ -30,28 +30,28 @@ def pickling(self, obj): self.assertTrue(obj == obj2) def test_all_shapes(self): - box = hppfcl.Box(1.0, 2.0, 3.0) + box = coal.Box(1.0, 2.0, 3.0) self.pickling(box) - sphere = hppfcl.Sphere(1.0) + sphere = coal.Sphere(1.0) self.pickling(sphere) - ellipsoid = hppfcl.Ellipsoid(1.0, 2.0, 3.0) + ellipsoid = coal.Ellipsoid(1.0, 2.0, 3.0) self.pickling(ellipsoid) convex = tetahedron() self.pickling(convex) - capsule = hppfcl.Capsule(1.0, 2.0) + capsule = coal.Capsule(1.0, 2.0) self.pickling(capsule) - cylinder = hppfcl.Cylinder(1.0, 2.0) + cylinder = coal.Cylinder(1.0, 2.0) self.pickling(cylinder) - plane = hppfcl.Plane(np.array([0.0, 0.0, 1.0]), 2.0) + plane = coal.Plane(np.array([0.0, 0.0, 1.0]), 2.0) self.pickling(plane) - half_space = hppfcl.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0) + half_space = coal.Halfspace(np.array([0.0, 0.0, 1.0]), 2.0) self.pickling(half_space) From 71db039ebc3bbf87c46f7404063ee14049c07581 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 19 Jun 2024 16:08:56 +0200 Subject: [PATCH 09/57] python: update doxygen doc to new `coal` lib name --- python/broadphase/broadphase.cc | 16 ++++++++-------- python/broadphase/broadphase_callbacks.hh | 2 +- .../broadphase/broadphase_collision_manager.hh | 2 +- python/coal.cc | 2 +- python/collision-geometries.cc | 10 +++++----- python/collision.cc | 2 +- python/contact_patch.cc | 2 +- python/distance.cc | 2 +- python/gjk.cc | 2 +- python/math.cc | 2 +- python/octree.cc | 2 +- 11 files changed, 22 insertions(+), 22 deletions(-) diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index 57da90761..788fb73ed 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -49,15 +49,15 @@ COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" COAL_COMPILER_DIAGNOSTIC_POP -#include "doxygen_autodoc/hpp/fcl/broadphase/default_broadphase_callbacks.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" +#include "doxygen_autodoc/coal/broadphase/default_broadphase_callbacks.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree.h" // #include -//"doxygen_autodoc/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_bruteforce.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SaP.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_SSaP.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_interval_tree.h" -// #include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_spatialhash.h" +//"doxygen_autodoc/coal/broadphase/broadphase_dynamic_AABB_tree_array.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_bruteforce.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_SaP.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_SSaP.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_interval_tree.h" +// #include "doxygen_autodoc/coal/broadphase/broadphase_spatialhash.h" #endif #include "broadphase_callbacks.hh" diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index f591fee9f..03790438f 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -44,7 +44,7 @@ #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_callbacks.h" +#include "doxygen_autodoc/coal/broadphase/broadphase_callbacks.h" #endif namespace coal { diff --git a/python/broadphase/broadphase_collision_manager.hh b/python/broadphase/broadphase_collision_manager.hh index 3dbcb5c1b..2624c0dcb 100644 --- a/python/broadphase/broadphase_collision_manager.hh +++ b/python/broadphase/broadphase_collision_manager.hh @@ -45,7 +45,7 @@ #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/broadphase/broadphase_collision_manager.h" +#include "doxygen_autodoc/coal/broadphase/broadphase_collision_manager.h" #endif #include diff --git a/python/coal.cc b/python/coal.cc index b792f0c70..dee8825d1 100644 --- a/python/coal.cc +++ b/python/coal.cc @@ -45,7 +45,7 @@ #include "coal/collision.h" #ifdef COAL_HAS_DOXYGEN_AUTODOC -#include "doxygen_autodoc/hpp/fcl/mesh_loader/loader.h" +#include "doxygen_autodoc/coal/mesh_loader/loader.h" #endif #include "../doc/python/doxygen.hh" diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index b44c7213c..32aa74d51 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -56,14 +56,14 @@ #ifdef COAL_HAS_DOXYGEN_AUTODOC // FIXME for a reason I do not understand, doxygen fails to understand that -// BV_splitter is not defined in hpp/fcl/BVH/BVH_model.h +// BV_splitter is not defined in coal/BVH/BVH_model.h #include "coal/internal/BV_splitter.h" #include "coal/broadphase/detail/hierarchy_tree.h" -#include "doxygen_autodoc/hpp/fcl/BVH/BVH_model.h" -#include "doxygen_autodoc/hpp/fcl/BV/AABB.h" -#include "doxygen_autodoc/hpp/fcl/hfield.h" -#include "doxygen_autodoc/hpp/fcl/shape/geometric_shapes.h" +#include "doxygen_autodoc/coal/BVH/BVH_model.h" +#include "doxygen_autodoc/coal/BV/AABB.h" +#include "doxygen_autodoc/coal/hfield.h" +#include "doxygen_autodoc/coal/shape/geometric_shapes.h" #include "doxygen_autodoc/functions.h" #endif diff --git a/python/collision.cc b/python/collision.cc index 88eb4d0b4..71b292689 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -47,7 +47,7 @@ COAL_COMPILER_DIAGNOSTIC_POP #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/collision_data.h" +#include "doxygen_autodoc/coal/collision_data.h" #endif #include "../doc/python/doxygen.hh" diff --git a/python/contact_patch.cc b/python/contact_patch.cc index 8ac35cbdf..e79721125 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -44,7 +44,7 @@ #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/collision_data.h" +#include "doxygen_autodoc/coal/collision_data.h" #endif #include "../doc/python/doxygen.hh" diff --git a/python/distance.cc b/python/distance.cc index 4f229ff6a..e6080a8b7 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -49,7 +49,7 @@ COAL_COMPILER_DIAGNOSTIC_POP #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/collision_data.h" +#include "doxygen_autodoc/coal/collision_data.h" #endif using namespace boost::python; diff --git a/python/gjk.cc b/python/gjk.cc index 256b32eec..c5dbb0d10 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -41,7 +41,7 @@ #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/narrowphase/gjk.h" +#include "doxygen_autodoc/coal/narrowphase/gjk.h" #endif using namespace boost::python; diff --git a/python/math.cc b/python/math.cc index b8ef731ff..1622eb340 100644 --- a/python/math.cc +++ b/python/math.cc @@ -44,7 +44,7 @@ #include "serializable.hh" #ifdef COAL_HAS_DOXYGEN_AUTODOC -#include "doxygen_autodoc/hpp/fcl/math/transform.h" +#include "doxygen_autodoc/coal/math/transform.h" #endif using namespace boost::python; diff --git a/python/octree.cc b/python/octree.cc index 73781f458..c18c7d64b 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -8,7 +8,7 @@ #ifdef COAL_HAS_DOXYGEN_AUTODOC #include "doxygen_autodoc/functions.h" -#include "doxygen_autodoc/hpp/fcl/octree.h" +#include "doxygen_autodoc/coal/octree.h" #endif bp::object toPyBytes(std::vector& bytes) { From d0f4e90f03e20912e9134effc2267468a5f0abbe Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 19 Jun 2024 16:13:07 +0200 Subject: [PATCH 10/57] cmake/test: change function name to reflect library name --- test/CMakeLists.txt | 74 ++++++++++++++++++++++----------------------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 57cfaf80c..39d2d31b2 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -3,7 +3,7 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework filesystem) config_files(fcl_resources/config.h) -macro(add_fcl_test test_name source) +macro(add_coal_test test_name source) ADD_UNIT_TEST(${test_name} ${source}) target_link_libraries(${test_name} PUBLIC @@ -17,60 +17,60 @@ macro(add_fcl_test test_name source) if(COAL_HAS_QHULL) target_compile_options(${test_name} PRIVATE -DCOAL_HAS_QHULL) endif() -endmacro(add_fcl_test) +endmacro(add_coal_test) include_directories(${CMAKE_CURRENT_BINARY_DIR}) add_library(utility STATIC utility.cpp) target_link_libraries(utility PUBLIC ${PROJECT_NAME}) -add_fcl_test(math math.cpp) +add_coal_test(math math.cpp) -add_fcl_test(collision collision.cpp) -add_fcl_test(contact_patch contact_patch.cpp) -add_fcl_test(distance distance.cpp) -add_fcl_test(swept_sphere_radius swept_sphere_radius.cpp) -add_fcl_test(normal_and_nearest_points normal_and_nearest_points.cpp) -add_fcl_test(distance_lower_bound distance_lower_bound.cpp) -add_fcl_test(security_margin security_margin.cpp) -add_fcl_test(geometric_shapes geometric_shapes.cpp) -add_fcl_test(shape_inflation shape_inflation.cpp) -#add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp) -add_fcl_test(gjk_asserts gjk_asserts.cpp) -add_fcl_test(frontlist frontlist.cpp) +add_coal_test(collision collision.cpp) +add_coal_test(contact_patch contact_patch.cpp) +add_coal_test(distance distance.cpp) +add_coal_test(swept_sphere_radius swept_sphere_radius.cpp) +add_coal_test(normal_and_nearest_points normal_and_nearest_points.cpp) +add_coal_test(distance_lower_bound distance_lower_bound.cpp) +add_coal_test(security_margin security_margin.cpp) +add_coal_test(geometric_shapes geometric_shapes.cpp) +add_coal_test(shape_inflation shape_inflation.cpp) +#add_coal_test(shape_mesh_consistency shape_mesh_consistency.cpp) +add_coal_test(gjk_asserts gjk_asserts.cpp) +add_coal_test(frontlist frontlist.cpp) SET_TESTS_PROPERTIES(frontlist PROPERTIES TIMEOUT 7200) -# add_fcl_test(sphere_capsule sphere_capsule.cpp) -add_fcl_test(capsule_capsule capsule_capsule.cpp) -add_fcl_test(box_box_distance box_box_distance.cpp) -add_fcl_test(box_box_collision box_box_collision.cpp) -add_fcl_test(simple simple.cpp) -add_fcl_test(capsule_box_1 capsule_box_1.cpp) -add_fcl_test(capsule_box_2 capsule_box_2.cpp) -add_fcl_test(obb obb.cpp) -add_fcl_test(convex convex.cpp) +# add_coal_test(sphere_capsule sphere_capsule.cpp) +add_coal_test(capsule_capsule capsule_capsule.cpp) +add_coal_test(box_box_distance box_box_distance.cpp) +add_coal_test(box_box_collision box_box_collision.cpp) +add_coal_test(simple simple.cpp) +add_coal_test(capsule_box_1 capsule_box_1.cpp) +add_coal_test(capsule_box_2 capsule_box_2.cpp) +add_coal_test(obb obb.cpp) +add_coal_test(convex convex.cpp) -add_fcl_test(bvh_models bvh_models.cpp) -add_fcl_test(collision_node_asserts collision_node_asserts.cpp) -add_fcl_test(hfields hfields.cpp) +add_coal_test(bvh_models bvh_models.cpp) +add_coal_test(collision_node_asserts collision_node_asserts.cpp) +add_coal_test(hfields hfields.cpp) -add_fcl_test(profiling profiling.cpp) +add_coal_test(profiling profiling.cpp) -add_fcl_test(gjk gjk.cpp) -add_fcl_test(accelerated_gjk accelerated_gjk.cpp) -add_fcl_test(gjk_convergence_criterion gjk_convergence_criterion.cpp) +add_coal_test(gjk gjk.cpp) +add_coal_test(accelerated_gjk accelerated_gjk.cpp) +add_coal_test(gjk_convergence_criterion gjk_convergence_criterion.cpp) if(COAL_HAS_OCTOMAP) - add_fcl_test(octree octree.cpp) + add_coal_test(octree octree.cpp) endif(COAL_HAS_OCTOMAP) -add_fcl_test(serialization serialization.cpp) +add_coal_test(serialization serialization.cpp) # Broadphase -add_fcl_test(broadphase broadphase.cpp) +add_coal_test(broadphase broadphase.cpp) set_tests_properties(broadphase PROPERTIES WILL_FAIL TRUE) -add_fcl_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp) -add_fcl_test(broadphase_collision_1 broadphase_collision_1.cpp) -add_fcl_test(broadphase_collision_2 broadphase_collision_2.cpp) +add_coal_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp) +add_coal_test(broadphase_collision_1 broadphase_collision_1.cpp) +add_coal_test(broadphase_collision_2 broadphase_collision_2.cpp) ## Benchmark add_executable(test-benchmark benchmark.cpp) From 37db2311b227b667435e2bf39f0f1a0cd46f2c0d Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 19 Jun 2024 16:23:43 +0200 Subject: [PATCH 11/57] test: rename BOOST_TEST_MODULE --- test/accelerated_gjk.cpp | 2 +- test/box_box_collision.cpp | 2 +- test/box_box_distance.cpp | 2 +- test/broadphase.cpp | 2 +- test/broadphase_collision_1.cpp | 2 +- test/broadphase_collision_2.cpp | 2 +- test/broadphase_dynamic_AABB_tree.cpp | 2 +- test/bvh_models.cpp | 2 +- test/capsule_box_1.cpp | 2 +- test/capsule_box_2.cpp | 2 +- test/capsule_capsule.cpp | 2 +- test/collision.cpp | 2 +- test/collision_node_asserts.cpp | 2 +- test/contact_patch.cpp | 2 +- test/convex.cpp | 2 +- test/distance.cpp | 2 +- test/distance_lower_bound.cpp | 2 +- test/frontlist.cpp | 2 +- test/geometric_shapes.cpp | 2 +- test/gjk.cpp | 2 +- test/gjk_asserts.cpp | 2 +- test/gjk_convergence_criterion.cpp | 2 +- test/hfields.cpp | 2 +- test/math.cpp | 2 +- test/normal_and_nearest_points.cpp | 2 +- test/octree.cpp | 2 +- test/security_margin.cpp | 2 +- test/serialization.cpp | 2 +- test/shape_inflation.cpp | 2 +- test/shape_mesh_consistency.cpp | 2 +- test/simple.cpp | 2 +- test/swept_sphere_radius.cpp | 2 +- 32 files changed, 32 insertions(+), 32 deletions(-) diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp index be71edefc..ed744b014 100644 --- a/test/accelerated_gjk.cpp +++ b/test/accelerated_gjk.cpp @@ -34,7 +34,7 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE FCL_NESTEROV_GJK +#define BOOST_TEST_MODULE COAL_NESTEROV_GJK #include #include diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp index af49add91..fdbb4dda1 100644 --- a/test/box_box_collision.cpp +++ b/test/box_box_collision.cpp @@ -1,4 +1,4 @@ -#define BOOST_TEST_MODULE BOX_BOX_COLLISION +#define BOOST_TEST_MODULE COAL_BOX_BOX_COLLISION #include #include diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp index f1a8c1144..e559ab2c1 100644 --- a/test/box_box_distance.cpp +++ b/test/box_box_distance.cpp @@ -37,7 +37,7 @@ #define _USE_MATH_DEFINES #include -#define BOOST_TEST_MODULE FCL_BOX_BOX +#define BOOST_TEST_MODULE COAL_BOX_BOX #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) diff --git a/test/broadphase.cpp b/test/broadphase.cpp index b0ed10f06..a7d18d070 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_BROADPHASE +#define BOOST_TEST_MODULE COAL_BROADPHASE #include #include "coal/config.hh" diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp index 3ea150af4..1fc940362 100644 --- a/test/broadphase_collision_1.cpp +++ b/test/broadphase_collision_1.cpp @@ -35,7 +35,7 @@ */ /** @author Jia Pan */ -#define BOOST_TEST_MODULE BROADPHASE_COLLISION_1 +#define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_1 #include #include "coal/broadphase/broadphase_bruteforce.h" diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp index 72f477422..7f6f5ca5f 100644 --- a/test/broadphase_collision_2.cpp +++ b/test/broadphase_collision_2.cpp @@ -35,7 +35,7 @@ /** @author Jia Pan */ -#define BOOST_TEST_MODULE BROADPHASE_COLLISION_2 +#define BOOST_TEST_MODULE COAL_BROADPHASE_COLLISION_2 #include #include "coal/broadphase/broadphase_bruteforce.h" diff --git a/test/broadphase_dynamic_AABB_tree.cpp b/test/broadphase_dynamic_AABB_tree.cpp index 4db353737..25c64d43d 100644 --- a/test/broadphase_dynamic_AABB_tree.cpp +++ b/test/broadphase_dynamic_AABB_tree.cpp @@ -36,7 +36,7 @@ /** Tests the dynamic axis-aligned bounding box tree.*/ -#define BOOST_TEST_MODULE BROADPHASE_DYNAMIC_AABB_TREE +#define BOOST_TEST_MODULE COAL_BROADPHASE_DYNAMIC_AABB_TREE #include // #include "coal/data_types.h" diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index b966e98c3..2d3f5e502 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -35,7 +35,7 @@ /** \author Jeongseok Lee */ -#define BOOST_TEST_MODULE FCL_BVH_MODELS +#define BOOST_TEST_MODULE COAL_BVH_MODELS #include #include diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp index c3a094735..568bfe3ee 100644 --- a/test/capsule_box_1.cpp +++ b/test/capsule_box_1.cpp @@ -34,7 +34,7 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp index ed94fc22a..b16d523cf 100644 --- a/test/capsule_box_2.cpp +++ b/test/capsule_box_2.cpp @@ -34,7 +34,7 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index d7f8af59f..1f429914d 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -35,7 +35,7 @@ /** \author Karsten Knese */ -#define BOOST_TEST_MODULE FCL_CAPSULE_CAPSULE +#define BOOST_TEST_MODULE COAL_CAPSULE_CAPSULE #include #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE((x + 1.0), (1.0), (eps)) diff --git a/test/collision.cpp b/test/collision.cpp index 6484d8765..de61e1a8b 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -37,7 +37,7 @@ #include -#define BOOST_TEST_MODULE FCL_COLLISION +#define BOOST_TEST_MODULE COAL_COLLISION #include #include diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp index e81d75b51..57506e595 100644 --- a/test/collision_node_asserts.cpp +++ b/test/collision_node_asserts.cpp @@ -1,4 +1,4 @@ -#define BOOST_TEST_MODULE FCL_COLLISION_NODE_ASSERT +#define BOOST_TEST_MODULE COAL_COLLISION_NODE_ASSERT #include #include diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp index fdc1a258d..36725fd03 100644 --- a/test/contact_patch.cpp +++ b/test/contact_patch.cpp @@ -34,7 +34,7 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE FCL_CONTACT_PATCH +#define BOOST_TEST_MODULE COAL_CONTACT_PATCH #include #include "coal/contact_patch.h" diff --git a/test/convex.cpp b/test/convex.cpp index dacaeaa2e..ebcc4a093 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -34,7 +34,7 @@ /** \author Joseph Mirabel */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #include "coal/shape/convex.h" diff --git a/test/distance.cpp b/test/distance.cpp index 89db86065..8066dfa7b 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_DISTANCE +#define BOOST_TEST_MODULE COAL_DISTANCE #include #include diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index 598f1a33e..50695a27d 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_DISTANCE_LOWER_BOUND +#define BOOST_TEST_MODULE COAL_DISTANCE_LOWER_BOUND #include #include diff --git a/test/frontlist.cpp b/test/frontlist.cpp index 8c63fb1e2..0728f6bb9 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_FRONT_LIST +#define BOOST_TEST_MODULE COAL_FRONT_LIST #include #include "coal/internal/traversal_node_bvhs.h" diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index 150a43f30..ec07c2d50 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -36,7 +36,7 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_GEOMETRIC_SHAPES +#define BOOST_TEST_MODULE COAL_GEOMETRIC_SHAPES #include #include "coal/narrowphase/narrowphase.h" diff --git a/test/gjk.cpp b/test/gjk.cpp index 75c96efa1..ee54597bd 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -34,7 +34,7 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_GJK +#define BOOST_TEST_MODULE COAL_GJK #include #include diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp index b65b5d111..926ed28a3 100644 --- a/test/gjk_asserts.cpp +++ b/test/gjk_asserts.cpp @@ -1,4 +1,4 @@ -#define BOOST_TEST_MODULE FCL_GJK_ASSERTS +#define BOOST_TEST_MODULE COAL_GJK_ASSERTS #include #include diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index 2cbb124fb..6fccdb200 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -34,7 +34,7 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE FCL_NESTEROV_GJK +#define BOOST_TEST_MODULE COAL_NESTEROV_GJK #include #include diff --git a/test/hfields.cpp b/test/hfields.cpp index 92bd16f49..7492bd9d4 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -34,7 +34,7 @@ /** \author Justin Carpentier */ -#define BOOST_TEST_MODULE FCL_HEIGHT_FIELDS +#define BOOST_TEST_MODULE COAL_HEIGHT_FIELDS #include #include diff --git a/test/math.cpp b/test/math.cpp index 251f49873..03827c6ff 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -36,7 +36,7 @@ #define _USE_MATH_DEFINES #include -#define BOOST_TEST_MODULE FCL_MATH +#define BOOST_TEST_MODULE COAL_MATH #include #include "coal/data_types.h" diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp index 5b31c7aea..81176f5ee 100644 --- a/test/normal_and_nearest_points.cpp +++ b/test/normal_and_nearest_points.cpp @@ -34,7 +34,7 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE NORMAL_AND_NEAREST_POINTS +#define BOOST_TEST_MODULE COAL_NORMAL_AND_NEAREST_POINTS #include #include "coal/fwd.hh" diff --git a/test/octree.cpp b/test/octree.cpp index 4d51ad612..87bf39c64 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -35,7 +35,7 @@ /** \author Florent Lamiraux */ -#define BOOST_TEST_MODULE FCL_OCTREE +#define BOOST_TEST_MODULE COAL_OCTREE #include #include #include diff --git a/test/security_margin.cpp b/test/security_margin.cpp index b9a825045..59ef596f8 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN +#define BOOST_TEST_MODULE COAL_SECURITY_MARGIN #include #include diff --git a/test/serialization.cpp b/test/serialization.cpp index 10bd283d1..58e1ac889 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_SERIALIZATION +#define BOOST_TEST_MODULE COAL_SERIALIZATION #include #include diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index 9c1abd985..5eae40610 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN +#define BOOST_TEST_MODULE COAL_SECURITY_MARGIN #include #include diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index 1e45c3451..2bfd3fa92 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -35,7 +35,7 @@ /** \author Jia Pan */ -#define BOOST_TEST_MODULE FCL_SHAPE_MESH_CONSISTENCY +#define BOOST_TEST_MODULE COAL_SHAPE_MESH_CONSISTENCY #include #include "coal/narrowphase/narrowphase.h" diff --git a/test/simple.cpp b/test/simple.cpp index 401053714..94527919b 100644 --- a/test/simple.cpp +++ b/test/simple.cpp @@ -1,4 +1,4 @@ -#define BOOST_TEST_MODULE FCL_SIMPLE +#define BOOST_TEST_MODULE COAL_SIMPLE #include #include "coal/internal/intersect.h" diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index a109dd623..f9ef8a1d8 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -34,7 +34,7 @@ /** \author Louis Montaut */ -#define BOOST_TEST_MODULE SWEPT_SPHERE_RADIUS +#define BOOST_TEST_MODULE COAL_SWEPT_SPHERE_RADIUS #include #include "coal/narrowphase/narrowphase.h" From 64015bb8e9caf401911015a175262d2a54b6b0ba Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 20 Jun 2024 16:50:38 +0200 Subject: [PATCH 12/57] misc: change library name to `coal` in doc --- include/coal/collision_data.h | 18 +++++++++--------- include/coal/collision_utility.h | 8 ++++---- include/coal/contact_patch_func_matrix.h | 4 ++-- include/coal/doc.hh | 22 +++++++++++----------- include/coal/logging.h | 2 +- include/coal/shape/geometric_shapes.h | 2 +- python/version.cc | 2 +- src/collision_utility.cpp | 8 ++++---- src/mesh_loader/assimp.cpp | 2 +- src/mesh_loader/loader.cpp | 2 +- src/narrowphase/gjk.cpp | 2 +- test/benchmark.cpp | 8 ++++---- test/profiling.cpp | 8 ++++---- test/swept_sphere_radius.cpp | 2 +- 14 files changed, 45 insertions(+), 45 deletions(-) diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index 26c12ad79..863372d5d 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -321,14 +321,14 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { bool enable_distance_lower_bound; /// @brief Distance below which objects are considered in collision. - /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation + /// See \ref coal_collision_and_distance_lower_bound_computation /// @note If set to -inf, the objects tested for collision are considered /// as collision free and no test is actually performed by functions - /// hpp::fcl::collide of class hpp::fcl::ComputeCollision. + /// coal::collide of class coal::ComputeCollision. FCL_REAL security_margin; /// @brief Distance below which bounding volumes are broken down. - /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation + /// See \ref coal_collision_and_distance_lower_bound_computation FCL_REAL break_distance; /// @brief Distance above which GJK solver makes an early stopping. @@ -394,7 +394,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult { public: /// Lower bound on distance between objects if they are disjoint. - /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation + /// See \ref coal_collision_and_distance_lower_bound_computation /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is /// set to infinity, distance_lower_bound is the actual distance between the /// shapes. @@ -499,14 +499,14 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// and second shape of a collision pair, a contact patch is represented as a /// polytope which vertices all belong to `P & S1 & S2`, where `&` denotes the /// set-intersection. Since a contact patch is a subset of a plane supported by -/// `n`, it has a preferred direction. In HPP-FCL, the `Contact::normal` points +/// `n`, it has a preferred direction. In Coal, the `Contact::normal` points /// from S1 to S2. In the same way, a contact patch points by default from S1 /// to S2. /// /// @note For now (April 2024), a `ContactPatch` is a polygon (2D polytope), /// so the points of the set, forming the convex-hull of the polytope, are /// stored in a counter-clockwise fashion. -/// @note If needed, the internal algorithms of hpp-fcl can easily be extended +/// @note If needed, the internal algorithms of Coal can easily be extended /// to compute a contact volume instead of a contact patch. struct COAL_DLLAPI ContactPatch { public: @@ -517,12 +517,12 @@ struct COAL_DLLAPI ContactPatch { Transform3f tf; /// @brief Direction of ContactPatch. - /// When doing collision detection, the convention of HPP-FCL is that the + /// When doing collision detection, the convention of Coal is that the /// normal always points from the first to the second shape of the collision /// pair i.e. from shape1 to shape2 when calling `collide(shape1, shape2)`. /// The PatchDirection enum allows to identify if the patch points from /// shape 1 to shape 2 (Default type) or from shape 2 to shape 1 (Inverted - /// type). The Inverted type should only be used for internal HPP-FCL + /// type). The Inverted type should only be used for internal Coal /// computations (it allows to properly define two separate contact patches in /// the same frame). enum PatchDirection { DEFAULT = 0, INVERTED = 1 }; @@ -555,7 +555,7 @@ struct COAL_DLLAPI ContactPatch { /// @brief Default constructor. /// Note: the preallocated size does not determine the maximum number of /// points in the patch, it only serves as preallocation if the maximum size - /// of the patch is known in advance. HPP-FCL will automatically expand/shrink + /// of the patch is known in advance. Coal will automatically expand/shrink /// the contact patch if needed. explicit ContactPatch(size_t preallocated_size = default_preallocated_size) : tf(Transform3f::Identity()), diff --git a/include/coal/collision_utility.h b/include/coal/collision_utility.h index 7f72d0e9c..19aa534a8 100644 --- a/include/coal/collision_utility.h +++ b/include/coal/collision_utility.h @@ -2,18 +2,18 @@ // Copyright (c) 2022 INRIA // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it +// This file is part of Coal. +// Coal is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // -// hpp-fcl is distributed in the hope that it will be +// Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . +// Coal. If not, see . #ifndef COAL_COLLISION_UTILITY_H #define COAL_COLLISION_UTILITY_H diff --git a/include/coal/contact_patch_func_matrix.h b/include/coal/contact_patch_func_matrix.h index 50b08d291..ec2aeb496 100644 --- a/include/coal/contact_patch_func_matrix.h +++ b/include/coal/contact_patch_func_matrix.h @@ -52,12 +52,12 @@ struct COAL_DLLAPI ContactPatchFunctionMatrix { /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 /// and tf2; /// 2. the collision result that generated contact patches candidates - /// (`hpp::fcl::Contact`), from which contact patches will be expanded; + /// (`coal::Contact`), from which contact patches will be expanded; /// 3. the solver for computation of contact patches; /// 4. the request setting for contact patches (e.g. maximum amount of /// patches, patch tolerance etc.) /// 5. the structure to return contact patches - /// (`hpp::fcl::ContactPatchResult`). + /// (`coal::ContactPatchResult`). /// /// Note: we pass a GJKSolver, because it allows to reuse internal computation /// that was made during the narrow phase. It also allows to experiment with diff --git a/include/coal/doc.hh b/include/coal/doc.hh index ade896a29..7b86b9d73 100644 --- a/include/coal/doc.hh +++ b/include/coal/doc.hh @@ -35,26 +35,26 @@ namespace coal { /// \mainpage -/// \anchor hpp_fcl_documentation +/// \anchor coal_documentation /// -/// \section hpp_fcl_introduction Introduction +/// \section coal_introduction Introduction /// -/// hpp-fcl is a modified version the FCL libraries. +/// Coal is a modified version the FCL libraries. /// /// It is a library for collision detection and distance computation between /// various types of geometric shapes reprensented either by -/// \li basic shapes (hpp::fcl::ShapeBase) like box, sphere, cylinders, ... -/// \li or by bounding volume hierarchies of various types (hpp::fcl::BVHModel) +/// \li basic shapes (coal::ShapeBase) like box, sphere, cylinders, ... +/// \li or by bounding volume hierarchies of various types (coal::BVHModel) /// -/// \par Using hpp-fcl +/// \par Using Coal /// /// The main entry points to the library are functions -/// \li hpp::fcl::collide(const CollisionObject*, const CollisionObject*, const -/// CollisionRequest&, CollisionResult&) \li hpp::fcl::distance(const +/// \li coal::collide(const CollisionObject*, const CollisionObject*, const +/// CollisionRequest&, CollisionResult&) \li coal::distance(const /// CollisionObject*, const CollisionObject*, const DistanceRequest&, /// DistanceResult&) /// -/// \section hpp_fcl_collision_and_distance_lower_bound_computation Collision +/// \section coal_collision_and_distance_lower_bound_computation Collision /// detection and distance lower bound /// /// Collision queries can return a distance lower bound between the two objects, @@ -71,8 +71,8 @@ namespace coal { /// is only guaranted that it will be inferior to /// distance - security_margin and superior to \em break_distance. /// \note If CollisionRequest::security_margin is set to -inf, no collision test -/// is performed by function hpp::fcl::collide or class -/// hpp::fcl::ComputeCollision and the objects are considered as not +/// is performed by function coal::collide or class +/// coal::ComputeCollision and the objects are considered as not /// colliding. } // namespace coal diff --git a/include/coal/logging.h b/include/coal/logging.h index c75db2fbe..235b475c5 100644 --- a/include/coal/logging.h +++ b/include/coal/logging.h @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -/// This file defines basic logging macros for HPP-FCL, based on Boost.Log. +/// This file defines basic logging macros for Coal, based on Boost.Log. /// To enable logging, define the preprocessor macro `COAL_ENABLE_LOGGING`. #ifndef COAL_LOGGING_H diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h index ea3ad9882..9d6a62f2e 100644 --- a/include/coal/shape/geometric_shapes.h +++ b/include/coal/shape/geometric_shapes.h @@ -646,7 +646,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// "Qt". If \c NULL, "Qt" is passed to Qhull. /// - if \c keepTriangles is \c false, an empty string is passed to /// Qhull. - /// \note hpp-fcl must have been compiled with option \c COAL_HAS_QHULL set + /// \note Coal must have been compiled with option \c COAL_HAS_QHULL set /// to \c ON. static ConvexBase* convexHull(std::shared_ptr>& points, unsigned int num_points, bool keepTriangles, diff --git a/python/version.cc b/python/version.cc index 0b3b1d4ce..658a94297 100644 --- a/python/version.cc +++ b/python/version.cc @@ -17,7 +17,7 @@ inline bool checkVersionAtMost(int major, int minor, int patch) { } void exposeVersion() { - // Define release numbers of the current hpp-fcl version. + // Define release numbers of the current Coal version. bp::scope().attr("__version__") = BOOST_PP_STRINGIZE(COAL_MAJOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_MINOR_VERSION) "." BOOST_PP_STRINGIZE(COAL_PATCH_VERSION); bp::scope().attr("__raw_version__") = COAL_VERSION; diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp index 213075724..8fe84680e 100644 --- a/src/collision_utility.cpp +++ b/src/collision_utility.cpp @@ -1,18 +1,18 @@ // Copyright (c) 2017, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it +// This file is part of Coal. +// Coal is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // -// hpp-fcl is distributed in the hope that it will be +// Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . +// Coal. If not, see . #include "coal/collision_utility.h" #include "coal/BVH/BVH_utility.h" diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp index 2fe749808..dc947ab0c 100644 --- a/src/mesh_loader/assimp.cpp +++ b/src/mesh_loader/assimp.cpp @@ -39,7 +39,7 @@ // Assimp >= 5.0 is forcing the use of C++11 keywords. A fix has been submitted // https://github.com/assimp/assimp/pull/2758. The next lines fixes the bug for -// current version of hpp-fcl. +// current version of Coal. #include #if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600)) && \ defined(AI_NO_EXCEPT) diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp index e7e275d4e..5ebe872d2 100644 --- a/src/mesh_loader/loader.cpp +++ b/src/mesh_loader/loader.cpp @@ -94,7 +94,7 @@ CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) { shared_ptr octree(new octomap::OcTree(filename)); return CollisionGeometryPtr_t(new coal::OcTree(octree)); #else - COAL_THROW_PRETTY("hpp-fcl compiled without OctoMap. Cannot create OcTrees.", + COAL_THROW_PRETTY("Coal compiled without OctoMap. Cannot create OcTrees.", std::logic_error); #endif } diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 863c37a43..f6959bbdd 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -152,7 +152,7 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { /// Inflate the points along a normal. /// The normal is typically the normal of the separating plane found by GJK /// or the normal found by EPA. -/// The normal should follow hpp-fcl convention: it points from shape0 to +/// The normal should follow coal convention: it points from shape0 to /// shape1. template void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0, diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 372c8f06f..6cca3275c 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -1,18 +1,18 @@ // Copyright (c) 2016, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it +// This file is part of Coal. +// Coal is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // -// hpp-fcl is distributed in the hope that it will be +// Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . +// Coal. If not, see . #include diff --git a/test/profiling.cpp b/test/profiling.cpp index f75566149..5ac747714 100644 --- a/test/profiling.cpp +++ b/test/profiling.cpp @@ -1,18 +1,18 @@ // Copyright (c) 2017, Joseph Mirabel // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) // -// This file is part of hpp-fcl. -// hpp-fcl is free software: you can redistribute it +// This file is part of Coal. +// Coal is free software: you can redistribute it // and/or modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation, either version // 3 of the License, or (at your option) any later version. // -// hpp-fcl is distributed in the hope that it will be +// Coal is distributed in the hope that it will be // useful, but WITHOUT ANY WARRANTY; without even the implied warranty // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // General Lesser Public License for more details. You should have // received a copy of the GNU Lesser General Public License along with -// hpp-fcl. If not, see . +// Coal. If not, see . #include diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index f9ef8a1d8..6d2c33642 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -99,7 +99,7 @@ int line; // So we can also easily recover the witness points of the swept sphere shapes. // // This suite of test is designed to verify that property and generally test for -// swept-sphere radius support in hpp-fcl. +// swept-sphere radius support in Coal. // Notes: // - not all collision pairs use GJK/EPA, so this test makes sure that // swept-sphere radius is taken into account even for specialized collision From 62ee93d8b9c4ee5b47687871551391c91538442c Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 17:09:46 +0200 Subject: [PATCH 13/57] types: remove collision with `Scalar` --- include/coal/internal/tools.h | 12 ++++++------ include/coal/serialization/eigen.h | 12 ++++++------ src/BV/OBB.cpp | 2 +- src/BV/RSS.cpp | 2 +- src/BVH/BV_fitter.cpp | 24 ++++++++++++------------ test/shape_inflation.cpp | 6 +++--- 6 files changed, 29 insertions(+), 29 deletions(-) diff --git a/include/coal/internal/tools.h b/include/coal/internal/tools.h index cb3eb0e0e..02cf695ee 100644 --- a/include/coal/internal/tools.h +++ b/include/coal/internal/tools.h @@ -102,16 +102,16 @@ void relativeTransform(const Eigen::MatrixBase& R1, template void eigen(const Eigen::MatrixBase& m, typename Derived::Scalar dout[3], Vector* vout) { - typedef typename Derived::Scalar Scalar; + typedef typename Derived::Scalar S; Derived R(m.derived()); int n = 3; int j, iq, ip, i; - Scalar tresh, theta, tau, t, sm, s, h, g, c; + S tresh, theta, tau, t, sm, s, h, g, c; - Scalar b[3]; - Scalar z[3]; - Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; - Scalar d[3]; + S b[3]; + S z[3]; + S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + S d[3]; for (ip = 0; ip < n; ++ip) { b[ip] = d[ip] = R(ip, ip); diff --git a/include/coal/serialization/eigen.h b/include/coal/serialization/eigen.h index 8a1eea87b..e2defdc61 100644 --- a/include/coal/serialization/eigen.h +++ b/include/coal/serialization/eigen.h @@ -42,10 +42,10 @@ struct traits { namespace boost { namespace serialization { -template void save(Archive& ar, - const Eigen::Matrix& m, + const Eigen::Matrix& m, const unsigned int /*version*/) { Eigen::DenseIndex rows(m.rows()), cols(m.cols()); if (Rows == Eigen::Dynamic) ar& BOOST_SERIALIZATION_NVP(rows); @@ -53,10 +53,10 @@ void save(Archive& ar, ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); } -template void load(Archive& ar, - Eigen::Matrix& m, + Eigen::Matrix& m, const unsigned int /*version*/) { Eigen::DenseIndex rows = Rows, cols = Cols; if (Rows == Eigen::Dynamic) ar >> BOOST_SERIALIZATION_NVP(rows); @@ -65,10 +65,10 @@ void load(Archive& ar, ar >> make_nvp("data", make_array(m.data(), (size_t)m.size())); } -template void serialize(Archive& ar, - Eigen::Matrix& m, + Eigen::Matrix& m, const unsigned int version) { split_free(ar, m, version); } diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 8fee27530..e69b49ede 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -67,7 +67,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) { computeVertices(b2, vertex + 8); Matrix3f M; Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; + FCL_REAL s[3] = {0, 0, 0}; // obb axes b.axes.col(0).noalias() = (b1.To - b2.To).normalized(); diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index ead66ce7a..1738f8abd 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -942,7 +942,7 @@ RSS RSS::operator+(const RSS& other) const { Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3] = {0, 0, 0}; + FCL_REAL s[3] = {0, 0, 0}; getCovariance(v, NULL, NULL, NULL, 16, M); eigen(M, s, E); diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp index 3c0a80bf0..61006a385 100644 --- a/src/BVH/BV_fitter.cpp +++ b/src/BVH/BV_fitter.cpp @@ -47,7 +47,7 @@ static const double kIOS_RATIO = 1.5; static const double invSinA = 2; static const double cosA = sqrt(3.0) / 2.0; -static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], +static inline void axisFromEigen(Vec3f eigenV[3], FCL_REAL eigenS[3], Matrix3f& axes) { int min, mid, max; if (eigenS[0] > eigenS[1]) { @@ -131,7 +131,7 @@ void fit6(Vec3f* ps, OBB& bv) { void fitn(Vec3f* ps, unsigned int n, OBB& bv) { Matrix3f M; Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values + FCL_REAL s[3] = {0, 0, 0}; // three eigen values getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -202,7 +202,7 @@ void fit6(Vec3f* ps, RSS& bv) { void fitn(Vec3f* ps, unsigned int n, RSS& bv) { Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3] = {0, 0, 0}; + FCL_REAL s[3] = {0, 0, 0}; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -308,7 +308,7 @@ void fit3(Vec3f* ps, kIOS& bv) { void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { Matrix3f M; Vec3f E[3]; - Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values; + FCL_REAL s[3] = {0, 0, 0}; // three eigen values; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -481,9 +481,9 @@ OBB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBB bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; // three eigen values + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors + FCL_REAL s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -503,7 +503,7 @@ OBBRSS BVFitter::fit(unsigned int* primitive_indices, OBBRSS bv; Matrix3f M; Vec3f E[3]; - Matrix3f::Scalar s[3]; + FCL_REAL s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -534,9 +534,9 @@ RSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { RSS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; // three eigen values + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors + FCL_REAL s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); @@ -565,7 +565,7 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors - Matrix3f::Scalar s[3]; + FCL_REAL s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index 5eae40610..33f285da3 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -62,10 +62,10 @@ template bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol); bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) { - typedef Eigen::Matrix ScalarMatrix; - ScalarMatrix m1; + typedef Eigen::Matrix Matrix; + Matrix m1; m1 << v1; - ScalarMatrix m2; + Matrix m2; m2 << v2; return m1.isApprox(m2, tol); } From e78aa64e1cf5331e5a056004cd6b771a069717d5 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 17:18:01 +0200 Subject: [PATCH 14/57] all: rename `FCL_REAL` to `CoalScalar` --- doc/gjk.py | 10 +- doc/mesh-mesh-collision-call.dot | 50 +-- doc/shape-mesh-collision-call.dot | 42 +-- doc/shape-shape-collision-call.dot | 10 +- include/coal/BV/AABB.h | 22 +- include/coal/BV/BV.h | 10 +- include/coal/BV/BV_node.h | 6 +- include/coal/BV/OBB.h | 16 +- include/coal/BV/OBBRSS.h | 23 +- include/coal/BV/RSS.h | 28 +- include/coal/BV/kDOP.h | 24 +- include/coal/BV/kIOS.h | 29 +- include/coal/BVH/BVH_model.h | 10 +- include/coal/BVH/BVH_utility.h | 10 +- include/coal/broadphase/broadphase_SSaP.h | 4 +- include/coal/broadphase/broadphase_SaP.h | 6 +- .../coal/broadphase/broadphase_callbacks.h | 4 +- .../broadphase_continuous_collision_manager.h | 2 +- .../broadphase_dynamic_AABB_tree-inl.h | 8 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 8 +- .../broadphase/broadphase_interval_tree.h | 8 +- .../broadphase/broadphase_spatialhash-inl.h | 18 +- .../coal/broadphase/broadphase_spatialhash.h | 6 +- .../broadphase/default_broadphase_callbacks.h | 4 +- .../broadphase/detail/hierarchy_tree-inl.h | 36 +-- .../coal/broadphase/detail/hierarchy_tree.h | 2 +- .../detail/hierarchy_tree_array-inl.h | 30 +- .../broadphase/detail/hierarchy_tree_array.h | 2 +- .../coal/broadphase/detail/interval_tree.h | 2 +- .../broadphase/detail/interval_tree_node.h | 6 +- include/coal/broadphase/detail/morton.h | 2 +- .../coal/broadphase/detail/simple_interval.h | 2 +- .../coal/broadphase/detail/spatial_hash-inl.h | 2 +- include/coal/broadphase/detail/spatial_hash.h | 4 +- include/coal/collision_data.h | 95 +++--- include/coal/collision_object.h | 14 +- .../coal/contact_patch/contact_patch_solver.h | 7 +- .../contact_patch/contact_patch_solver.hxx | 26 +- include/coal/data_types.h | 12 +- include/coal/distance.h | 34 +- include/coal/distance_func_matrix.h | 14 +- include/coal/hfield.h | 61 ++-- include/coal/internal/BV_splitter.h | 6 +- include/coal/internal/intersect.h | 46 +-- include/coal/internal/shape_shape_func.h | 64 ++-- include/coal/internal/tools.h | 4 +- include/coal/internal/traversal_node_base.h | 12 +- .../coal/internal/traversal_node_bvh_hfield.h | 62 ++-- .../coal/internal/traversal_node_bvh_shape.h | 36 +-- include/coal/internal/traversal_node_bvhs.h | 60 ++-- .../internal/traversal_node_hfield_shape.h | 69 +++-- include/coal/internal/traversal_node_octree.h | 113 +++---- include/coal/internal/traversal_node_shapes.h | 6 +- include/coal/internal/traversal_recurse.h | 5 +- include/coal/math/transform.h | 36 +-- include/coal/narrowphase/gjk.h | 32 +- .../coal/narrowphase/minkowski_difference.h | 2 +- include/coal/narrowphase/narrowphase.h | 71 ++--- .../coal/narrowphase/narrowphase_defaults.h | 8 +- include/coal/narrowphase/support_functions.h | 26 +- include/coal/octree.h | 51 +-- include/coal/serialization/contact_patch.h | 6 +- include/coal/serialization/convex.h | 14 +- include/coal/serialization/geometric_shapes.h | 2 +- include/coal/serialization/kIOS.h | 4 +- include/coal/shape/convex.h | 2 +- include/coal/shape/details/convex.hxx | 10 +- .../coal/shape/geometric_shape_to_BVH_model.h | 66 ++-- include/coal/shape/geometric_shapes.h | 178 ++++++----- python/broadphase/broadphase.cc | 2 +- python/broadphase/broadphase_callbacks.hh | 2 +- python/collision-geometries.cc | 42 +-- python/collision.cc | 2 +- python/contact_patch.cc | 4 +- python/distance.cc | 14 +- python/gjk.cc | 2 +- python/math.cc | 2 +- python/octree.cc | 2 +- src/BV/AABB.cpp | 58 ++-- src/BV/OBB.cpp | 63 ++-- src/BV/OBB.h | 2 +- src/BV/RSS.cpp | 134 ++++---- src/BV/kDOP.cpp | 39 +-- src/BV/kIOS.cpp | 42 +-- src/BVH/BVH_model.cpp | 2 +- src/BVH/BVH_utility.cpp | 105 +++---- src/BVH/BV_fitter.cpp | 78 ++--- src/BVH/BV_splitter.cpp | 11 +- src/broadphase/broadphase_SSaP.cpp | 30 +- src/broadphase/broadphase_SaP.cpp | 52 ++-- src/broadphase/broadphase_bruteforce.cpp | 6 +- .../broadphase_dynamic_AABB_tree.cpp | 36 +-- .../broadphase_dynamic_AABB_tree_array.cpp | 36 +-- src/broadphase/broadphase_interval_tree.cpp | 26 +- .../default_broadphase_callbacks.cpp | 4 +- src/broadphase/detail/interval_tree.cpp | 11 +- src/broadphase/detail/spatial_hash.cpp | 2 +- src/collision.cpp | 4 +- src/collision_node.cpp | 8 +- src/contact_patch/contact_patch_solver.cpp | 2 +- src/distance.cpp | 30 +- src/distance/box_halfspace.cpp | 6 +- src/distance/box_plane.cpp | 26 +- src/distance/box_sphere.cpp | 22 +- src/distance/capsule_capsule.cpp | 36 +-- src/distance/capsule_halfspace.cpp | 6 +- src/distance/capsule_plane.cpp | 6 +- src/distance/cone_halfspace.cpp | 6 +- src/distance/cone_plane.cpp | 22 +- src/distance/convex_halfspace.cpp | 6 +- src/distance/convex_plane.cpp | 6 +- src/distance/cylinder_halfspace.cpp | 6 +- src/distance/cylinder_plane.cpp | 6 +- src/distance/ellipsoid_halfspace.cpp | 6 +- src/distance/ellipsoid_plane.cpp | 6 +- src/distance/halfspace_halfspace.cpp | 2 +- src/distance/halfspace_plane.cpp | 6 +- src/distance/plane_plane.cpp | 10 +- src/distance/sphere_capsule.cpp | 6 +- src/distance/sphere_cylinder.cpp | 6 +- src/distance/sphere_halfspace.cpp | 6 +- src/distance/sphere_plane.cpp | 6 +- src/distance/sphere_sphere.cpp | 2 +- src/distance/triangle_halfspace.cpp | 6 +- src/distance/triangle_plane.cpp | 6 +- src/distance/triangle_sphere.cpp | 6 +- src/distance/triangle_triangle.cpp | 6 +- src/distance_func_matrix.cpp | 135 ++++---- src/intersect.cpp | 104 +++---- src/narrowphase/details.h | 249 +++++++-------- src/narrowphase/gjk.cpp | 142 ++++----- src/narrowphase/minkowski_difference.cpp | 6 +- src/narrowphase/support_functions.cpp | 161 ++++------ src/octree.cpp | 32 +- src/shape/geometric_shapes.cpp | 28 +- src/shape/geometric_shapes_utility.cpp | 292 +++++++++--------- src/traversal/traversal_recurse.cpp | 20 +- test/accelerated_gjk.cpp | 6 +- test/benchmark.cpp | 4 +- test/box_box_collision.cpp | 2 +- test/broadphase.cpp | 25 +- test/broadphase_collision_1.cpp | 81 ++--- test/broadphase_collision_2.cpp | 12 +- test/broadphase_dynamic_AABB_tree.cpp | 6 +- test/bvh_models.cpp | 2 +- test/capsule_capsule.cpp | 2 +- test/collision.cpp | 16 +- test/collision_node_asserts.cpp | 2 +- test/contact_patch.cpp | 88 +++--- test/convex.cpp | 10 +- test/distance.cpp | 4 +- test/distance_lower_bound.cpp | 26 +- test/frontlist.cpp | 4 +- test/geometric_shapes.cpp | 155 +++++----- test/gjk.cpp | 32 +- test/gjk_asserts.cpp | 2 +- test/gjk_convergence_criterion.cpp | 6 +- test/hfields.cpp | 55 ++-- test/math.cpp | 2 +- test/normal_and_nearest_points.cpp | 115 +++---- test/obb.cpp | 148 ++++----- test/octree.cpp | 22 +- test/profiling.cpp | 6 +- test/security_margin.cpp | 18 +- test/serialization.cpp | 12 +- test/shape_inflation.cpp | 33 +- test/shape_mesh_consistency.cpp | 2 +- test/simple.cpp | 6 +- test/swept_sphere_radius.cpp | 46 +-- test/utility.cpp | 133 ++++---- test/utility.h | 48 +-- 171 files changed, 2556 insertions(+), 2531 deletions(-) diff --git a/doc/gjk.py b/doc/gjk.py index b69bcc2b3..2b7019f99 100644 --- a/doc/gjk.py +++ b/doc/gjk.py @@ -447,26 +447,26 @@ def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]): + "const Vec3f& {} (current.vertex[{}]->w);".format(v.upper(), v), file=file, ) - print(indent + "const FCL_REAL aa = A.squaredNorm();".format(), file=file) + print(indent + "const CoalScalar aa = A.squaredNorm();".format(), file=file) for v in "dcb": for m in "abcd": if m <= v: print( indent - + "const FCL_REAL {0}{1} = {2}.dot({3});".format( + + "const CoalScalar {0}{1} = {2}.dot({3});".format( v, m, v.upper(), m.upper() ), file=file, ) else: print( - indent + "const FCL_REAL& {0}{1} = {1}{0};".format(v, m), + indent + "const CoalScalar& {0}{1} = {1}{0};".format(v, m), file=file, ) - print(indent + "const FCL_REAL {0}a_aa = {0}a - aa;".format(v), file=file) + print(indent + "const CoalScalar {0}a_aa = {0}a - aa;".format(v), file=file) for l0, l1 in zip("bcd", "cdb"): print( - indent + "const FCL_REAL {0}a_{1}a = {0}a - {1}a;".format(l0, l1), + indent + "const CoalScalar {0}a_{1}a = {0}a - {1}a;".format(l0, l1), file=file, ) for v in "bc": diff --git a/doc/mesh-mesh-collision-call.dot b/doc/mesh-mesh-collision-call.dot index 55c455482..8c91a3821 100644 --- a/doc/mesh-mesh-collision-call.dot +++ b/doc/mesh-mesh-collision-call.dot @@ -5,34 +5,34 @@ digraph CD { size = 11.7 "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box] - "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box] - "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" [shape = box] - "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" [shape = box] - "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] - "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] + "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] + "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] + "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] + "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] + "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" [shape = box] - "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" [shape = box] + "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" [shape = box] "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" [shape = box] - "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] - "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [shape = box] - "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)" [shape = box] + "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] + "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box] "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" - "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)" - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n FCL_REAL& squaredLowerBoundDistance)" - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" + "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)" + "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)" + "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" - "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" - "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" - "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" - "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" - "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)" - "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [color=red] - "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" [color = red] - "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" - "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" - "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound) const" + "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" + "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" + "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" + "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" + "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" +"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" + "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [color=red] + "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [color = red] + "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" + "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" + "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" } \ No newline at end of file diff --git a/doc/shape-mesh-collision-call.dot b/doc/shape-mesh-collision-call.dot index 839f575a9..c08fbdfef 100644 --- a/doc/shape-mesh-collision-call.dot +++ b/doc/shape-mesh-collision-call.dot @@ -8,29 +8,29 @@ digraph CD { "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" [shape = box] "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" [shape = box] - "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box] - "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box] - "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box] - "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] - "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] - "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] - "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] + "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box] + "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" [shape = box] + "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box] + "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] + "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] + "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] + "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] + "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] + "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" - "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" - "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" - "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" - "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" - "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red] - "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red] - "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nFCL_REAL& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, FCL_REAL& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nFCL_REAL& squaredLowerBoundDistance)\nBV/OBB.cpp" - "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nFCL_REAL& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" + "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" + "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" + "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" + "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" + "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red] + "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red] + "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" + "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" + "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" + "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" } diff --git a/doc/shape-shape-collision-call.dot b/doc/shape-shape-collision-call.dot index 6690f4eb3..be01df254 100644 --- a/doc/shape-shape-collision-call.dot +++ b/doc/shape-shape-collision-call.dot @@ -5,15 +5,15 @@ digraph CD { size = 11.7 "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box] - "template\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box] + "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box] "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box] "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box] "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box] - "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" [shape = box] + "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" [shape = box] - "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" - "template\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" + "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" + "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" - "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nFCL_REAL* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" + "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" } diff --git a/include/coal/BV/AABB.h b/include/coal/BV/AABB.h index c0aa81b67..22ae0ab0d 100644 --- a/include/coal/BV/AABB.h +++ b/include/coal/BV/AABB.h @@ -128,14 +128,14 @@ class COAL_DLLAPI AABB { /// @brief Check whether two AABB are overlap bool overlap(const AABB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const; + CoalScalar& sqrDistLowerBound) const; /// @brief Distance between two AABBs - FCL_REAL distance(const AABB& other) const; + CoalScalar distance(const AABB& other) const; /// @brief Distance between two AABBs; P and Q, should not be NULL, return the /// nearest points - FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const; + CoalScalar distance(const AABB& other, Vec3f* P, Vec3f* Q) const; /// @brief Merge the AABB and a point inline AABB& operator+=(const Vec3f& p) { @@ -158,22 +158,22 @@ class COAL_DLLAPI AABB { } /// @brief Size of the AABB (used in BV_Splitter to order two AABBs) - inline FCL_REAL size() const { return (max_ - min_).squaredNorm(); } + inline CoalScalar size() const { return (max_ - min_).squaredNorm(); } /// @brief Center of the AABB inline Vec3f center() const { return (min_ + max_) * 0.5; } /// @brief Width of the AABB - inline FCL_REAL width() const { return max_[0] - min_[0]; } + inline CoalScalar width() const { return max_[0] - min_[0]; } /// @brief Height of the AABB - inline FCL_REAL height() const { return max_[1] - min_[1]; } + inline CoalScalar height() const { return max_[1] - min_[1]; } /// @brief Depth of the AABB - inline FCL_REAL depth() const { return max_[2] - min_[2]; } + inline CoalScalar depth() const { return max_[2] - min_[2]; } /// @brief Volume of the AABB - inline FCL_REAL volume() const { return width() * height() * depth(); } + inline CoalScalar volume() const { return width() * height() * depth(); } /// @} @@ -213,14 +213,14 @@ class COAL_DLLAPI AABB { /// @brief expand the half size of the AABB by a scalar delta, and keep the /// center unchanged. - inline AABB& expand(const FCL_REAL delta) { + inline AABB& expand(const CoalScalar delta) { min_.array() -= delta; max_.array() += delta; return *this; } /// @brief expand the aabb by increase the thickness of the plate by a ratio - inline AABB& expand(const AABB& core, FCL_REAL ratio) { + inline AABB& expand(const AABB& core, CoalScalar ratio) { min_ = min_ * ratio - core.min_; max_ = max_ * ratio - core.max_; return *this; @@ -260,7 +260,7 @@ COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, /// and b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); + CoalScalar& sqrDistLowerBound); } // namespace coal #endif diff --git a/include/coal/BV/BV.h b/include/coal/BV/BV.h index 7cca20489..d6c6a7e08 100644 --- a/include/coal/BV/BV.h +++ b/include/coal/BV/BV.h @@ -65,7 +65,7 @@ template <> struct Converter { static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) { const Vec3f& center = bv1.center(); - FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5; + CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5; const Vec3f center2 = tf1.transform(center); bv2.min_ = center2 - Vec3f::Constant(r); bv2.max_ = center2 + Vec3f::Constant(r); @@ -132,7 +132,7 @@ template struct Converter { static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) { const Vec3f& center = bv1.center(); - FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + CoalScalar r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; const Vec3f center2 = tf1.transform(center); bv2.min_ = center2 - Vec3f::Constant(r); bv2.max_ = center2 + Vec3f::Constant(r); @@ -140,7 +140,7 @@ struct Converter { static void convert(const BV1& bv1, AABB& bv2) { const Vec3f& center = bv1.center(); - FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + CoalScalar r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; bv2.min_ = center - Vec3f::Constant(r); bv2.max_ = center + Vec3f::Constant(r); } @@ -213,14 +213,14 @@ struct Converter { bv2.Tr = tf1.transform(bv1.center()); /// Sort the AABB edges so that AABB extents are ordered. - FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth()}; + CoalScalar d[3] = {bv1.width(), bv1.height(), bv1.depth()}; Eigen::DenseIndex id[3] = {0, 1, 2}; for (Eigen::DenseIndex i = 1; i < 3; ++i) { for (Eigen::DenseIndex j = i; j > 0; --j) { if (d[j] > d[j - 1]) { { - FCL_REAL tmp = d[j]; + CoalScalar tmp = d[j]; d[j] = d[j - 1]; d[j - 1] = tmp; } diff --git a/include/coal/BV/BV_node.h b/include/coal/BV/BV_node.h index 9e7adfad6..43f33863b 100644 --- a/include/coal/BV/BV_node.h +++ b/include/coal/BV/BV_node.h @@ -121,14 +121,14 @@ struct COAL_DLLAPI BVNode : public BVNodeBase { bool overlap(const BVNode& other) const { return bv.overlap(other.bv); } /// @brief Check whether two BVNode collide bool overlap(const BVNode& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { return bv.overlap(other.bv, request, sqrDistLowerBound); } /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and /// the underlying BV supports distance, return the nearest points. - FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, - Vec3f* P2 = NULL) const { + CoalScalar distance(const BVNode& other, Vec3f* P1 = NULL, + Vec3f* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } diff --git a/include/coal/BV/OBB.h b/include/coal/BV/OBB.h index d9bd0c503..b16f3d01f 100644 --- a/include/coal/BV/OBB.h +++ b/include/coal/BV/OBB.h @@ -86,10 +86,10 @@ struct COAL_DLLAPI OBB { /// @retval sqrDistLowerBound squared lower bound on distance between boxes if /// they do not overlap. bool overlap(const OBB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const; + CoalScalar& sqrDistLowerBound) const; /// @brief Distance between two OBBs, not implemented. - FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; + CoalScalar distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; /// @brief A simple way to merge the OBB and a point (the result is not /// compact). @@ -106,22 +106,22 @@ struct COAL_DLLAPI OBB { OBB operator+(const OBB& other) const; /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) - inline FCL_REAL size() const { return extent.squaredNorm(); } + inline CoalScalar size() const { return extent.squaredNorm(); } /// @brief Center of the OBB inline const Vec3f& center() const { return To; } /// @brief Width of the OBB. - inline FCL_REAL width() const { return 2 * extent[0]; } + inline CoalScalar width() const { return 2 * extent[0]; } /// @brief Height of the OBB. - inline FCL_REAL height() const { return 2 * extent[1]; } + inline CoalScalar height() const { return 2 * extent[1]; } /// @brief Depth of the OBB - inline FCL_REAL depth() const { return 2 * extent[2]; } + inline CoalScalar depth() const { return 2 * extent[2]; } /// @brief Volume of the OBB - inline FCL_REAL volume() const { return width() * height() * depth(); } + inline CoalScalar volume() const { return width() * height() * depth(); } }; /// @brief Translate the OBB bv @@ -136,7 +136,7 @@ COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, /// b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); + CoalScalar& sqrDistLowerBound); /// Check collision between two boxes /// @param B, T orientation and position of first box, diff --git a/include/coal/BV/OBBRSS.h b/include/coal/BV/OBBRSS.h index 5e1da89fa..003b396e1 100644 --- a/include/coal/BV/OBBRSS.h +++ b/include/coal/BV/OBBRSS.h @@ -77,14 +77,14 @@ struct COAL_DLLAPI OBBRSS { /// @retval sqrDistLowerBound squared lower bound on distance between /// objects if they do not overlap. bool overlap(const OBBRSS& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { return obb.overlap(other.obb, request, sqrDistLowerBound); } /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the /// nearest points - FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, - Vec3f* Q = NULL) const { + CoalScalar distance(const OBBRSS& other, Vec3f* P = NULL, + Vec3f* Q = NULL) const { return rss.distance(other.rss, P, Q); } @@ -110,22 +110,22 @@ struct COAL_DLLAPI OBBRSS { } /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) - inline FCL_REAL size() const { return obb.size(); } + inline CoalScalar size() const { return obb.size(); } /// @brief Center of the OBBRSS inline const Vec3f& center() const { return obb.center(); } /// @brief Width of the OBRSS - inline FCL_REAL width() const { return obb.width(); } + inline CoalScalar width() const { return obb.width(); } /// @brief Height of the OBBRSS - inline FCL_REAL height() const { return obb.height(); } + inline CoalScalar height() const { return obb.height(); } /// @brief Depth of the OBBRSS - inline FCL_REAL depth() const { return obb.depth(); } + inline CoalScalar depth() const { return obb.depth(); } /// @brief Volume of the OBBRSS - inline FCL_REAL volume() const { return obb.volume(); } + inline CoalScalar volume() const { return obb.volume(); } }; /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) @@ -143,14 +143,15 @@ inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, /// not overlap. inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) { + CoalScalar& sqrDistLowerBound) { return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound); } /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) /// and b2 is in indentity; P and Q, is not NULL, returns the nearest points -inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, - const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL) { +inline CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, + const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, + Vec3f* Q = NULL) { return distance(R0, T0, b1.rss, b2.rss, P, Q); } diff --git a/include/coal/BV/RSS.h b/include/coal/BV/RSS.h index e0916fe29..8d9cfba52 100644 --- a/include/coal/BV/RSS.h +++ b/include/coal/BV/RSS.h @@ -64,10 +64,10 @@ struct COAL_DLLAPI RSS { Vec3f Tr; /// @brief Side lengths of rectangle - FCL_REAL length[2]; + CoalScalar length[2]; /// @brief Radius of sphere summed with rectangle to form RSS - FCL_REAL radius; + CoalScalar radius; ///  @brief Default constructor with default values RSS() : axes(Matrix3f::Zero()), Tr(Vec3f::Zero()), radius(-1) { @@ -93,14 +93,14 @@ struct COAL_DLLAPI RSS { /// Not implemented bool overlap(const RSS& other, const CollisionRequest&, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { sqrDistLowerBound = sqrt(-1); return overlap(other); } /// @brief the distance between two RSS; P and Q, if not NULL, return the /// nearest points - FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; + CoalScalar distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; /// @brief A simple way to merge the RSS and a point, not compact. /// @todo This function may have some bug. @@ -116,7 +116,7 @@ struct COAL_DLLAPI RSS { RSS operator+(const RSS& other) const; /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) - inline FCL_REAL size() const { + inline CoalScalar size() const { return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + 2 * radius); } @@ -125,18 +125,18 @@ struct COAL_DLLAPI RSS { inline const Vec3f& center() const { return Tr; } /// @brief Width of the RSS - inline FCL_REAL width() const { return length[0] + 2 * radius; } + inline CoalScalar width() const { return length[0] + 2 * radius; } /// @brief Height of the RSS - inline FCL_REAL height() const { return length[1] + 2 * radius; } + inline CoalScalar height() const { return length[1] + 2 * radius; } /// @brief Depth of the RSS - inline FCL_REAL depth() const { return 2 * radius; } + inline CoalScalar depth() const { return 2 * radius; } /// @brief Volume of the RSS - inline FCL_REAL volume() const { + inline CoalScalar volume() const { return (length[0] * length[1] * 2 * radius + - 4 * boost::math::constants::pi() * radius * radius * + 4 * boost::math::constants::pi() * radius * radius * radius); } @@ -153,9 +153,9 @@ struct COAL_DLLAPI RSS { /// not the RSS. But the direction P - Q is the correct direction for cloest /// points Notice that P and Q are both in the local frame of the first RSS (not /// global frame and not even the local frame of object 1) -COAL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, - const RSS& b1, const RSS& b2, Vec3f* P = NULL, - Vec3f* Q = NULL); +COAL_DLLAPI CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, + const RSS& b1, const RSS& b2, Vec3f* P = NULL, + Vec3f* Q = NULL); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. @@ -166,7 +166,7 @@ COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, /// b2 is in identity. COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); + CoalScalar& sqrDistLowerBound); } // namespace coal diff --git a/include/coal/BV/kDOP.h b/include/coal/BV/kDOP.h index cb35f7838..4803cd4b6 100644 --- a/include/coal/BV/kDOP.h +++ b/include/coal/BV/kDOP.h @@ -91,7 +91,7 @@ template class COAL_DLLAPI KDOP { protected: /// @brief Origin's distances to N KDOP planes - Eigen::Array dist_; + Eigen::Array dist_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -123,11 +123,11 @@ class COAL_DLLAPI KDOP { /// @retval sqrDistLowerBound squared lower bound on distance between boxes if /// they do not overlap. bool overlap(const KDOP& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const; + CoalScalar& sqrDistLowerBound) const; /// @brief The distance between two KDOP. Not implemented. - FCL_REAL distance(const KDOP& other, Vec3f* P = NULL, - Vec3f* Q = NULL) const; + CoalScalar distance(const KDOP& other, Vec3f* P = NULL, + Vec3f* Q = NULL) const; /// @brief Merge the point and the KDOP KDOP& operator+=(const Vec3f& p); @@ -139,7 +139,7 @@ class COAL_DLLAPI KDOP { KDOP operator+(const KDOP& other) const; /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) - inline FCL_REAL size() const { + inline CoalScalar size() const { return width() * width() + height() * height() + depth() * depth(); } @@ -149,20 +149,20 @@ class COAL_DLLAPI KDOP { } /// @brief The (AABB) width - inline FCL_REAL width() const { return dist_[N / 2] - dist_[0]; } + inline CoalScalar width() const { return dist_[N / 2] - dist_[0]; } /// @brief The (AABB) height - inline FCL_REAL height() const { return dist_[N / 2 + 1] - dist_[1]; } + inline CoalScalar height() const { return dist_[N / 2 + 1] - dist_[1]; } /// @brief The (AABB) depth - inline FCL_REAL depth() const { return dist_[N / 2 + 2] - dist_[2]; } + inline CoalScalar depth() const { return dist_[N / 2 + 2] - dist_[2]; } /// @brief The (AABB) volume - inline FCL_REAL volume() const { return width() * height() * depth(); } + inline CoalScalar volume() const { return width() * height() * depth(); } - inline FCL_REAL dist(short i) const { return dist_[i]; } + inline CoalScalar dist(short i) const { return dist_[i]; } - inline FCL_REAL& dist(short i) { return dist_[i]; } + inline CoalScalar& dist(short i) { return dist_[i]; } //// @brief Check whether one point is inside the KDOP bool inside(const Vec3f& p) const; @@ -177,7 +177,7 @@ bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, template bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/, const CollisionRequest& /*request*/, - FCL_REAL& /*sqrDistLowerBound*/) { + CoalScalar& /*sqrDistLowerBound*/) { COAL_THROW_PRETTY("not implemented", std::logic_error); } diff --git a/include/coal/BV/kIOS.h b/include/coal/BV/kIOS.h index 36eaee616..c663bd2c3 100644 --- a/include/coal/BV/kIOS.h +++ b/include/coal/BV/kIOS.h @@ -55,7 +55,7 @@ class COAL_DLLAPI kIOS { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3f o; - FCL_REAL r; + CoalScalar r; bool operator==(const kIOS_Sphere& other) const { return o == other.o && r == other.r; @@ -70,8 +70,8 @@ class COAL_DLLAPI kIOS { static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1) { Vec3f d = s1.o - s0.o; - FCL_REAL dist2 = d.squaredNorm(); - FCL_REAL diff_r = s1.r - s0.r; + CoalScalar dist2 = d.squaredNorm(); + CoalScalar diff_r = s1.r - s0.r; /** The sphere with the larger radius encloses the other */ if (diff_r * diff_r >= dist2) { @@ -129,10 +129,11 @@ class COAL_DLLAPI kIOS { /// @brief Check collision between two kIOS bool overlap(const kIOS& other, const CollisionRequest&, - FCL_REAL& sqrDistLowerBound) const; + CoalScalar& sqrDistLowerBound) const; /// @brief The distance between two kIOS - FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; + CoalScalar distance(const kIOS& other, Vec3f* P = NULL, + Vec3f* Q = NULL) const; /// @brief A simple way to merge the kIOS and a point kIOS& operator+=(const Vec3f& p); @@ -147,22 +148,22 @@ class COAL_DLLAPI kIOS { kIOS operator+(const kIOS& other) const; /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) - FCL_REAL size() const; + CoalScalar size() const; /// @brief Center of the kIOS const Vec3f& center() const { return spheres[0].o; } /// @brief Width of the kIOS - FCL_REAL width() const; + CoalScalar width() const; /// @brief Height of the kIOS - FCL_REAL height() const; + CoalScalar height() const; /// @brief Depth of the kIOS - FCL_REAL depth() const; + CoalScalar depth() const; /// @brief Volume of the kIOS - FCL_REAL volume() const; + CoalScalar volume() const; }; /// @brief Translate the kIOS BV @@ -179,13 +180,13 @@ COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, /// @todo Not efficient COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound); + CoalScalar& sqrDistLowerBound); /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation -COAL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, - const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, - Vec3f* Q = NULL); +COAL_DLLAPI CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, + const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, + Vec3f* Q = NULL); } // namespace coal diff --git a/include/coal/BVH/BVH_model.h b/include/coal/BVH/BVH_model.h index 71b830188..de80f330a 100644 --- a/include/coal/BVH/BVH_model.h +++ b/include/coal/BVH/BVH_model.h @@ -199,7 +199,7 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { virtual void makeParentRelative() = 0; Vec3f computeCOM() const { - FCL_REAL vol = 0; + CoalScalar vol = 0; Vec3f com(0, 0, 0); if (!(vertices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " @@ -218,7 +218,7 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { for (unsigned int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices_[i]; - FCL_REAL d_six_vol = + CoalScalar d_six_vol = (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); vol += d_six_vol; com += (vertices_[tri[0]] + vertices_[tri[1]] + vertices_[tri[2]]) * @@ -228,8 +228,8 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { return com / (vol * 4); } - FCL_REAL computeVolume() const { - FCL_REAL vol = 0; + CoalScalar computeVolume() const { + CoalScalar vol = 0; if (!(vertices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "vertices." @@ -246,7 +246,7 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices_[i]; - FCL_REAL d_six_vol = + CoalScalar d_six_vol = (vertices_[tri[0]].cross(vertices_[tri[1]])).dot(vertices_[tri[2]]); vol += d_six_vol; } diff --git a/include/coal/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h index 21434736c..a3687ab42 100644 --- a/include/coal/BVH/BVH_utility.h +++ b/include/coal/BVH/BVH_utility.h @@ -90,7 +90,7 @@ COAL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, /// and the origin, given the BV axises. COAL_DLLAPI void getRadiusAndOriginAndRectangleSize( Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r); + const Matrix3f& axes, Vec3f& origin, CoalScalar l[2], CoalScalar& r); /// @brief Compute the bounding volume extent and center for a set or subset of /// points, given the BV axises. @@ -102,13 +102,13 @@ COAL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, /// @brief Compute the center and radius for a triangle's circumcircle COAL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, - FCL_REAL& radius); + CoalScalar& radius); /// @brief Compute the maximum distance from a given center point to a point /// cloud -COAL_DLLAPI FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - const Vec3f& query); +COAL_DLLAPI CoalScalar maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Vec3f& query); } // namespace coal diff --git a/include/coal/broadphase/broadphase_SSaP.h b/include/coal/broadphase/broadphase_SSaP.h index fdb46cb21..9ca05050c 100644 --- a/include/coal/broadphase/broadphase_SSaP.h +++ b/include/coal/broadphase/broadphase_SSaP.h @@ -113,12 +113,12 @@ class COAL_DLLAPI SSaPCollisionManager : public BroadPhaseCollisionManager { typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; + CoalScalar& min_dist) const; bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; + CoalScalar& min_dist) const; static int selectOptimalAxis( const std::vector& objs_x, diff --git a/include/coal/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h index 209ad6af3..28c06e57c 100644 --- a/include/coal/broadphase/broadphase_SaP.h +++ b/include/coal/broadphase/broadphase_SaP.h @@ -151,9 +151,9 @@ class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { /// @brief set the value of the end point Vec3f& getVal(); - FCL_REAL getVal(int i) const; + CoalScalar getVal(int i) const; - FCL_REAL& getVal(int i); + CoalScalar& getVal(int i); }; /// @brief A pair of objects that are not culling away and should further @@ -210,7 +210,7 @@ class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { std::map obj_aabb_map; bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; + CoalScalar& min_dist) const; bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; diff --git a/include/coal/broadphase/broadphase_callbacks.h b/include/coal/broadphase/broadphase_callbacks.h index 56fe4f1cd..5a132d106 100644 --- a/include/coal/broadphase/broadphase_callbacks.h +++ b/include/coal/broadphase/broadphase_callbacks.h @@ -82,11 +82,11 @@ struct COAL_DLLAPI DistanceCallBackBase { /// @param[in] o2 Collision object #2. /// @param[out] dist Distance between the two collision geometries. virtual bool distance(CollisionObject* o1, CollisionObject* o2, - FCL_REAL& dist) = 0; + CoalScalar& dist) = 0; /// @brief Functor call associated to the distance operation. virtual bool operator()(CollisionObject* o1, CollisionObject* o2, - FCL_REAL& dist) { + CoalScalar& dist) { return distance(o1, o2, dist); } }; diff --git a/include/coal/broadphase/broadphase_continuous_collision_manager.h b/include/coal/broadphase/broadphase_continuous_collision_manager.h index c2cec955e..c2e36e746 100644 --- a/include/coal/broadphase/broadphase_continuous_collision_manager.h +++ b/include/coal/broadphase/broadphase_continuous_collision_manager.h @@ -134,7 +134,7 @@ class COAL_DLLAPI BroadPhaseContinuousCollisionManager { using BroadPhaseContinuousCollisionManagerf = BroadPhaseContinuousCollisionManager; using BroadPhaseContinuousCollisionManagerd = - BroadPhaseContinuousCollisionManager; + BroadPhaseContinuousCollisionManager; } // namespace coal diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h index 05d8881a6..870e39b06 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -155,7 +155,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Eigen::MatrixBase& translation2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); @@ -175,8 +175,8 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, translation2); - FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); - FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); + CoalScalar d1 = aabb2.distance(root1->children[0]->bv); + CoalScalar d2 = aabb2.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -211,7 +211,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, computeChildBV(root2_bv, i, child_bv); const AABB& aabb2 = translate(child_bv, translation2); - FCL_REAL d = root1->bv.distance(aabb2); + CoalScalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(root1, tree2, child, child_bv, translation2, diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index caafe0096..296555166 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -151,7 +151,7 @@ bool distanceRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Eigen::MatrixBase& translation2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { @@ -174,8 +174,8 @@ bool distanceRecurse_( (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, translation2); - FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); + CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv); + CoalScalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -210,7 +210,7 @@ bool distanceRecurse_( computeChildBV(root2_bv, i, child_bv); const AABB& aabb2 = translate(child_bv, translation2); - FCL_REAL d = root1->bv.distance(aabb2); + CoalScalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, diff --git a/include/coal/broadphase/broadphase_interval_tree.h b/include/coal/broadphase/broadphase_interval_tree.h index a97799c29..9950cf82e 100644 --- a/include/coal/broadphase/broadphase_interval_tree.h +++ b/include/coal/broadphase/broadphase_interval_tree.h @@ -119,7 +119,7 @@ class COAL_DLLAPI IntervalTreeCollisionManager CollisionObject* obj; /// @brief end point value - FCL_REAL value; + CoalScalar value; /// @brief tag for whether it is a lower bound or higher bound of an /// interval, 0 for lo, and 1 for hi @@ -133,7 +133,7 @@ class COAL_DLLAPI IntervalTreeCollisionManager struct COAL_DLLAPI SAPInterval : public detail::SimpleInterval { CollisionObject* obj; - SAPInterval(FCL_REAL low_, FCL_REAL high_, CollisionObject* obj_); + SAPInterval(CoalScalar low_, CoalScalar high_, CollisionObject* obj_); }; bool checkColl( @@ -145,12 +145,12 @@ class COAL_DLLAPI IntervalTreeCollisionManager typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; + CoalScalar& min_dist) const; bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const; bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; + CoalScalar& min_dist) const; /// @brief vector stores all the end points std::vector endpoints[3]; diff --git a/include/coal/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h index fd4ffa0bb..1509bcd70 100644 --- a/include/coal/broadphase/broadphase_spatialhash-inl.h +++ b/include/coal/broadphase/broadphase_spatialhash-inl.h @@ -45,7 +45,7 @@ namespace coal { //============================================================================== template SpatialHashingCollisionManager::SpatialHashingCollisionManager( - FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, + CoalScalar cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size) : scene_limit(AABB(scene_min, scene_max)), hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) { @@ -262,7 +262,7 @@ template void SpatialHashingCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } @@ -309,10 +309,10 @@ bool SpatialHashingCollisionManager::collide_( template bool SpatialHashingCollisionManager::distance_( CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; auto aabb = obj->getAABB(); - if (min_dist < (std::numeric_limits::max)()) { + if (min_dist < (std::numeric_limits::max)()) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } @@ -320,7 +320,7 @@ bool SpatialHashingCollisionManager::distance_( AABB overlap_aabb; auto status = 1; - FCL_REAL old_min_distance; + CoalScalar old_min_distance; while (1) { old_min_distance = min_dist; @@ -350,7 +350,7 @@ bool SpatialHashingCollisionManager::distance_( } if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) { + if (old_min_distance < (std::numeric_limits::max)()) { break; } else { if (min_dist < old_min_distance) { @@ -422,7 +422,7 @@ void SpatialHashingCollisionManager::distance( this->enable_tested_set_ = true; this->tested_set.clear(); - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (const auto& obj : objs) { if (distance_(obj, callback, min_dist)) break; @@ -473,7 +473,7 @@ void SpatialHashingCollisionManager::distance( return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (const auto& obj : objs) @@ -512,7 +512,7 @@ template template bool SpatialHashingCollisionManager::distanceObjectToObjects( CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { for (auto& obj2 : objs) { if (obj == obj2) continue; diff --git a/include/coal/broadphase/broadphase_spatialhash.h b/include/coal/broadphase/broadphase_spatialhash.h index 17f37e052..e2828a53d 100644 --- a/include/coal/broadphase/broadphase_spatialhash.h +++ b/include/coal/broadphase/broadphase_spatialhash.h @@ -56,7 +56,7 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { typedef BroadPhaseCollisionManager Base; using Base::getObjects; - SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, + SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000); @@ -128,7 +128,7 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { /// @brief perform distance computation between one object and all the objects /// belonging ot the manager bool distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; + CoalScalar& min_dist) const; /// @brief all objects in the scene std::list objs; @@ -157,7 +157,7 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { template bool distanceObjectToObjects(CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const; + CoalScalar& min_dist) const; }; } // namespace coal diff --git a/include/coal/broadphase/default_broadphase_callbacks.h b/include/coal/broadphase/default_broadphase_callbacks.h index a124cd8d0..711b6c708 100644 --- a/include/coal/broadphase/default_broadphase_callbacks.h +++ b/include/coal/broadphase/default_broadphase_callbacks.h @@ -189,7 +189,7 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, /// @return `true` if the broadphase evaluation should stop. /// @tparam S The scalar type with which the computation will be performed. bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* data, FCL_REAL& dist); + void* data, CoalScalar& dist); /// @brief Default collision callback to check collision between collision /// objects. @@ -212,7 +212,7 @@ struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase { /// Clears the distance result and sets the done boolean to false. void init() { data.clear(); } - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist); + bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist); DistanceData data; diff --git a/include/coal/broadphase/detail/hierarchy_tree-inl.h b/include/coal/broadphase/detail/hierarchy_tree-inl.h index 85d0c8899..b771e77e5 100644 --- a/include/coal/broadphase/detail/hierarchy_tree-inl.h +++ b/include/coal/broadphase/detail/hierarchy_tree-inl.h @@ -159,7 +159,7 @@ template struct UpdateImpl { static bool run(const HierarchyTree& tree, typename HierarchyTree::Node* leaf, const BV& bv, - const Vec3f& /*vel*/, FCL_REAL /*margin*/) { + const Vec3f& /*vel*/, CoalScalar /*margin*/) { if (leaf->bv.contain(bv)) return false; tree.update_(leaf, bv); return true; @@ -177,14 +177,14 @@ struct UpdateImpl { //============================================================================== template bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel, - FCL_REAL margin) { - return UpdateImpl::run(*this, leaf, bv, vel, margin); + CoalScalar margin) { + return UpdateImpl::run(*this, leaf, bv, vel, margin); } //============================================================================== template bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel) { - return UpdateImpl::run(*this, leaf, bv, vel); + return UpdateImpl::run(*this, leaf, bv, vel); } //============================================================================== @@ -302,10 +302,10 @@ void HierarchyTree::bottomup(const NodeVecIterator lbeg, while (lbeg < lcur_end - 1) { NodeVecIterator min_it1 = lbeg; NodeVecIterator min_it2 = lbeg + 1; - FCL_REAL min_size = (std::numeric_limits::max)(); + CoalScalar min_size = (std::numeric_limits::max)(); for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { - FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size(); + CoalScalar cur_size = ((*it1)->bv + (*it2)->bv).size(); if (cur_size < min_size) { min_size = cur_size; min_it1 = it1; @@ -377,7 +377,7 @@ typename HierarchyTree::Node* HierarchyTree::topdown_0( for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; int best_axis = 0; - FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; + CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()}; if (extent[1] > extent[0]) best_axis = 1; if (extent[2] > extent[best_axis]) best_axis = 2; @@ -415,7 +415,7 @@ typename HierarchyTree::Node* HierarchyTree::topdown_1( split_p += (*it)->bv.center(); vol += (*it)->bv; } - split_p /= static_cast(num_leaves); + split_p /= static_cast(num_leaves); int best_axis = -1; long bestmidp = num_leaves; int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; @@ -436,7 +436,7 @@ typename HierarchyTree::Node* HierarchyTree::topdown_1( if (best_axis < 0) best_axis = 0; - FCL_REAL split_value = split_p[best_axis]; + CoalScalar split_value = split_p[best_axis]; NodeVecIterator lcenter = lbeg; for (it = lbeg; it < lend; ++it) { if ((*it)->bv.center()[best_axis] < split_value) { @@ -480,7 +480,7 @@ void HierarchyTree::init_1(std::vector& leaves) { if (leaves.size() > 0) bound_bv = leaves[0]->bv; for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; - morton_functor coder(bound_bv); + morton_functor coder(bound_bv); for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); @@ -504,7 +504,7 @@ void HierarchyTree::init_2(std::vector& leaves) { if (leaves.size() > 0) bound_bv = leaves[0]->bv; for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; - morton_functor coder(bound_bv); + morton_functor coder(bound_bv); for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); @@ -528,7 +528,7 @@ void HierarchyTree::init_3(std::vector& leaves) { if (leaves.size() > 0) bound_bv = leaves[0]->bv; for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; - morton_functor coder(bound_bv); + morton_functor coder(bound_bv); for (size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); @@ -951,14 +951,14 @@ struct SelectImpl { template size_t select(const NodeBase& query, const NodeBase& node1, const NodeBase& node2) { - return SelectImpl::run(query, node1, node2); + return SelectImpl::run(query, node1, node2); } //============================================================================== template size_t select(const BV& query, const NodeBase& node1, const NodeBase& node2) { - return SelectImpl::run(query, node1, node2); + return SelectImpl::run(query, node1, node2); } //============================================================================== @@ -973,8 +973,8 @@ struct SelectImpl { Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } @@ -986,8 +986,8 @@ struct SelectImpl { Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } }; diff --git a/include/coal/broadphase/detail/hierarchy_tree.h b/include/coal/broadphase/detail/hierarchy_tree.h index 8eb2f46da..67a6b7ec9 100644 --- a/include/coal/broadphase/detail/hierarchy_tree.h +++ b/include/coal/broadphase/detail/hierarchy_tree.h @@ -100,7 +100,7 @@ class HierarchyTree { bool update(Node* leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction - bool update(Node* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); + bool update(Node* leaf, const BV& bv, const Vec3f& vel, CoalScalar margin); /// @brief update one leaf's bounding volume, with prediction bool update(Node* leaf, const BV& bv, const Vec3f& vel); diff --git a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h index 7fafa1281..9ac67f89c 100644 --- a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h +++ b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h @@ -137,7 +137,7 @@ void HierarchyTree::init_1(Node* leaves, int n_leaves_) { if (n_leaves > 0) bound_bv = nodes[0].bv; for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; - morton_functor coder(bound_bv); + morton_functor coder(bound_bv); for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); @@ -175,7 +175,7 @@ void HierarchyTree::init_2(Node* leaves, int n_leaves_) { if (n_leaves > 0) bound_bv = nodes[0].bv; for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; - morton_functor coder(bound_bv); + morton_functor coder(bound_bv); for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); @@ -213,7 +213,7 @@ void HierarchyTree::init_3(Node* leaves, int n_leaves_) { if (n_leaves > 0) bound_bv = nodes[0].bv; for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; - morton_functor coder(bound_bv); + morton_functor coder(bound_bv); for (size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); @@ -296,7 +296,7 @@ bool HierarchyTree::update(size_t leaf, const BV& bv) { //============================================================================== template bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, - FCL_REAL margin) { + CoalScalar margin) { COAL_UNUSED_VARIABLE(bv); COAL_UNUSED_VARIABLE(vel); COAL_UNUSED_VARIABLE(margin); @@ -477,10 +477,10 @@ void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) { size_t* lcur_end = lend; while (lbeg < lcur_end - 1) { size_t *min_it1 = nullptr, *min_it2 = nullptr; - FCL_REAL min_size = (std::numeric_limits::max)(); + CoalScalar min_size = (std::numeric_limits::max)(); for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) { for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) { - FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); + CoalScalar cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); if (cur_size < min_size) { min_size = cur_size; min_it1 = it1; @@ -528,7 +528,7 @@ size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) { for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv; size_t best_axis = 0; - FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; + CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()}; if (extent[1] > extent[0]) best_axis = 1; if (extent[2] > extent[best_axis]) best_axis = 2; @@ -562,7 +562,7 @@ size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { split_p += nodes[*i].bv.center(); vol += nodes[*i].bv; } - split_p /= static_cast(num_leaves); + split_p /= static_cast(num_leaves); int best_axis = -1; int bestmidp = (int)num_leaves; int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; @@ -583,7 +583,7 @@ size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { if (best_axis < 0) best_axis = 0; - FCL_REAL split_value = split_p[best_axis]; + CoalScalar split_value = split_p[best_axis]; size_t* lcenter = lbeg; for (size_t* i = lbeg; i < lend; ++i) { if (nodes[*i].bv.center()[best_axis] < split_value) { @@ -931,14 +931,14 @@ struct SelectImpl { //============================================================================== template size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { - return SelectImpl::run(query, node1, node2, nodes); + return SelectImpl::run(query, node1, node2, nodes); } //============================================================================== template size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes) { - return SelectImpl::run(query, node1, node2, nodes); + return SelectImpl::run(query, node1, node2, nodes); } //============================================================================== @@ -952,8 +952,8 @@ struct SelectImpl { Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } @@ -965,8 +965,8 @@ struct SelectImpl { Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); - FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); - FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); + CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); + CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } }; diff --git a/include/coal/broadphase/detail/hierarchy_tree_array.h b/include/coal/broadphase/detail/hierarchy_tree_array.h index 23e2c2bcc..a7edf618c 100644 --- a/include/coal/broadphase/detail/hierarchy_tree_array.h +++ b/include/coal/broadphase/detail/hierarchy_tree_array.h @@ -114,7 +114,7 @@ class HierarchyTree { bool update(size_t leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction - bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); + bool update(size_t leaf, const BV& bv, const Vec3f& vel, CoalScalar margin); /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vec3f& vel); diff --git a/include/coal/broadphase/detail/interval_tree.h b/include/coal/broadphase/detail/interval_tree.h index b17c8b238..e2f8485c6 100644 --- a/include/coal/broadphase/detail/interval_tree.h +++ b/include/coal/broadphase/detail/interval_tree.h @@ -85,7 +85,7 @@ class COAL_DLLAPI IntervalTree { IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const; /// @brief Return result for a given query - std::deque query(FCL_REAL low, FCL_REAL high); + std::deque query(CoalScalar low, CoalScalar high); protected: IntervalTreeNode* root; diff --git a/include/coal/broadphase/detail/interval_tree_node.h b/include/coal/broadphase/detail/interval_tree_node.h index b053a2f9c..01c5de4bd 100644 --- a/include/coal/broadphase/detail/interval_tree_node.h +++ b/include/coal/broadphase/detail/interval_tree_node.h @@ -68,11 +68,11 @@ class COAL_DLLAPI IntervalTreeNode { /// @brief interval stored in the node SimpleInterval* stored_interval; - FCL_REAL key; + CoalScalar key; - FCL_REAL high; + CoalScalar high; - FCL_REAL max_high; + CoalScalar max_high; /// @brief red or black node: if red = false then the node is black bool red; diff --git a/include/coal/broadphase/detail/morton.h b/include/coal/broadphase/detail/morton.h index 549e7df0b..f2e8073bb 100644 --- a/include/coal/broadphase/detail/morton.h +++ b/include/coal/broadphase/detail/morton.h @@ -80,7 +80,7 @@ struct morton_functor { }; using morton_functoru32f = morton_functor; -using morton_functoru32d = morton_functor; +using morton_functoru32d = morton_functor; /// @brief Functor to compute 60 bit morton code for a given AABB template diff --git a/include/coal/broadphase/detail/simple_interval.h b/include/coal/broadphase/detail/simple_interval.h index 7b654048c..efd9cbe55 100644 --- a/include/coal/broadphase/detail/simple_interval.h +++ b/include/coal/broadphase/detail/simple_interval.h @@ -53,7 +53,7 @@ struct COAL_DLLAPI SimpleInterval { virtual void print(); /// @brief interval is defined as [low, high] - FCL_REAL low, high; + CoalScalar low, high; }; } // namespace detail diff --git a/include/coal/broadphase/detail/spatial_hash-inl.h b/include/coal/broadphase/detail/spatial_hash-inl.h index 499aee848..cb9d6bd9a 100644 --- a/include/coal/broadphase/detail/spatial_hash-inl.h +++ b/include/coal/broadphase/detail/spatial_hash-inl.h @@ -45,7 +45,7 @@ namespace coal { namespace detail { //============================================================================== -SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) +SpatialHash::SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_) : cell_size(cell_size_), scene_limit(scene_limit_) { width[0] = std::ceil(scene_limit.width() / cell_size); width[1] = std::ceil(scene_limit.height() / cell_size); diff --git a/include/coal/broadphase/detail/spatial_hash.h b/include/coal/broadphase/detail/spatial_hash.h index a5301a793..6d8646e38 100644 --- a/include/coal/broadphase/detail/spatial_hash.h +++ b/include/coal/broadphase/detail/spatial_hash.h @@ -47,12 +47,12 @@ namespace detail { /// @brief Spatial hash function: hash an AABB to a set of integer values struct COAL_DLLAPI SpatialHash { - SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_); + SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_); std::vector operator()(const AABB& aabb) const; private: - FCL_REAL cell_size; + CoalScalar cell_size; AABB scene_limit; unsigned int width[3]; }; diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index 863372d5d..488211f6d 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -102,28 +102,28 @@ struct COAL_DLLAPI Contact { Vec3f pos; /// @brief penetration depth - FCL_REAL penetration_depth; + CoalScalar penetration_depth; /// @brief invalid contact primitive information static const int NONE = -1; /// @brief Default constructor Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) { - penetration_depth = (std::numeric_limits::max)(); + penetration_depth = (std::numeric_limits::max)(); nearest_points[0] = nearest_points[1] = normal = pos = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_), o2(o2_), b1(b1_), b2(b2_) { - penetration_depth = (std::numeric_limits::max)(); + penetration_depth = (std::numeric_limits::max)(); nearest_points[0] = nearest_points[1] = normal = pos = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, - int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) + int b2_, const Vec3f& pos_, const Vec3f& normal_, CoalScalar depth_) : o1(o1_), o2(o2_), b1(b1_), @@ -136,7 +136,7 @@ struct COAL_DLLAPI Contact { Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_, - FCL_REAL depth_) + CoalScalar depth_) : o1(o1_), o2(o2_), b1(b1_), @@ -161,7 +161,7 @@ struct COAL_DLLAPI Contact { bool operator!=(const Contact& other) const { return !(*this == other); } - FCL_REAL getDistanceToCollision(const CollisionRequest& request) const; + CoalScalar getDistanceToCollision(const CollisionRequest& request) const; }; struct QueryResult; @@ -189,7 +189,7 @@ struct COAL_DLLAPI QueryRequest { /// Note: This tolerance determines the precision on the estimated distance /// between two geometries which are not in collision. /// It is recommended to not set this tolerance to less than 1e-6. - FCL_REAL gjk_tolerance; + CoalScalar gjk_tolerance; /// @brief whether to enable the Nesterov accleration of GJK GJKVariant gjk_variant; @@ -208,13 +208,13 @@ struct COAL_DLLAPI QueryRequest { /// between two geometries which are in collision. /// It is recommended to not set this tolerance to less than 1e-6. /// Also, setting EPA's tolerance to less than GJK's is not recommended. - FCL_REAL epa_tolerance; + CoalScalar epa_tolerance; /// @brief enable timings when performing collision/distance request bool enable_timings; /// @brief threshold below which a collision is considered. - FCL_REAL collision_distance_threshold; + CoalScalar collision_distance_threshold; COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS @@ -233,7 +233,7 @@ struct COAL_DLLAPI QueryRequest { epa_tolerance(EPA_DEFAULT_TOLERANCE), enable_timings(false), collision_distance_threshold( - Eigen::NumTraits::dummy_precision()) {} + Eigen::NumTraits::dummy_precision()) {} /// @brief Copy constructor. QueryRequest(const QueryRequest& other) = default; @@ -325,11 +325,11 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { /// @note If set to -inf, the objects tested for collision are considered /// as collision free and no test is actually performed by functions /// coal::collide of class coal::ComputeCollision. - FCL_REAL security_margin; + CoalScalar security_margin; /// @brief Distance below which bounding volumes are broken down. /// See \ref coal_collision_and_distance_lower_bound_computation - FCL_REAL break_distance; + CoalScalar break_distance; /// @brief Distance above which GJK solver makes an early stopping. /// GJK stops searching for the closest points when it proves that the @@ -337,7 +337,7 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { /// /// @remarks Consequently, the closest points might be incorrect, but allows /// to save computational resources. - FCL_REAL distance_upper_bound; + CoalScalar distance_upper_bound; /// @brief Constructor from a flag and a maximal number of contacts. /// @@ -352,7 +352,7 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND), security_margin(0), break_distance(1e-3), - distance_upper_bound((std::numeric_limits::max)()) {} + distance_upper_bound((std::numeric_limits::max)()) {} /// @brief Default constructor. CollisionRequest() @@ -361,7 +361,7 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { enable_distance_lower_bound(false), security_margin(0), break_distance(1e-3), - distance_upper_bound((std::numeric_limits::max)()) {} + distance_upper_bound((std::numeric_limits::max)()) {} COAL_COMPILER_DIAGNOSTIC_POP bool isSatisfied(const CollisionResult& result) const; @@ -381,7 +381,7 @@ struct COAL_DLLAPI CollisionRequest : QueryRequest { } }; -inline FCL_REAL Contact::getDistanceToCollision( +inline CoalScalar Contact::getDistanceToCollision( const CollisionRequest& request) const { return penetration_depth - request.security_margin; } @@ -398,7 +398,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// @note Always computed. If \ref CollisionRequest::distance_upper_bound is /// set to infinity, distance_lower_bound is the actual distance between the /// shapes. - FCL_REAL distance_lower_bound; + CoalScalar distance_lower_bound; /// @brief normal associated to nearest_points. /// Same as `CollisionResult::nearest_points` but for the normal. @@ -415,13 +415,14 @@ struct COAL_DLLAPI CollisionResult : QueryResult { public: CollisionResult() - : distance_lower_bound((std::numeric_limits::max)()) { + : distance_lower_bound((std::numeric_limits::max)()) { nearest_points[0] = nearest_points[1] = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } /// @brief Update the lower bound only if the distance is inferior. - inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) { + inline void updateDistanceLowerBound( + const CoalScalar& distance_lower_bound_) { if (distance_lower_bound_ < distance_lower_bound) distance_lower_bound = distance_lower_bound_; } @@ -480,11 +481,11 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// @brief clear the results obtained void clear() { - distance_lower_bound = (std::numeric_limits::max)(); + distance_lower_bound = (std::numeric_limits::max)(); contacts.clear(); timings.clear(); nearest_points[0] = nearest_points[1] = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } /// @brief reposition Contact objects when fcl inverts them @@ -541,7 +542,7 @@ struct COAL_DLLAPI ContactPatch { /// @note Although there may exist multiple minimum separation vectors between /// two shapes, the term "minimum" comes from the fact that it's impossible to /// find a different separation vector which has a smaller norm than `d * n`. - FCL_REAL penetration_depth; + CoalScalar penetration_depth; /// @brief Default maximum size of the polygon representing the contact patch. /// Used to pre-allocate memory for the patch. @@ -655,8 +656,8 @@ struct COAL_DLLAPI ContactPatch { /// @brief Whether two contact patches are the same or not. /// Checks for different order of the points. bool isSame(const ContactPatch& other, - const FCL_REAL tol = - Eigen::NumTraits::dummy_precision()) const { + const CoalScalar tol = + Eigen::NumTraits::dummy_precision()) const { // The x and y axis of the set are arbitrary, but the z axis is // always the normal. The position of the origin of the frame is also // arbitrary. So we only check if the normals are the same. @@ -740,7 +741,7 @@ struct COAL_DLLAPI ContactPatchRequest { /// plane, it is taken into account in the computation of the contact patch. /// Otherwise, it is not used for the computation. /// @note Needs to be positive. - FCL_REAL m_patch_tolerance; + CoalScalar m_patch_tolerance; public: /// @brief Default constructor. @@ -760,7 +761,7 @@ struct COAL_DLLAPI ContactPatchRequest { explicit ContactPatchRequest(size_t max_num_patch = 1, size_t num_samples_curved_shapes = ContactPatch::default_preallocated_size, - FCL_REAL patch_tolerance = 1e-3) + CoalScalar patch_tolerance = 1e-3) : max_num_patch(max_num_patch) { this->setNumSamplesCurvedShapes(num_samples_curved_shapes); this->setPatchTolerance(patch_tolerance); @@ -770,7 +771,7 @@ struct COAL_DLLAPI ContactPatchRequest { explicit ContactPatchRequest(const CollisionRequest& collision_request, size_t num_samples_curved_shapes = ContactPatch::default_preallocated_size, - FCL_REAL patch_tolerance = 1e-3) + CoalScalar patch_tolerance = 1e-3) : max_num_patch(collision_request.num_max_contacts) { this->setNumSamplesCurvedShapes(num_samples_curved_shapes); this->setPatchTolerance(patch_tolerance); @@ -794,19 +795,19 @@ struct COAL_DLLAPI ContactPatchRequest { } /// @copydoc m_patch_tolerance - void setPatchTolerance(const FCL_REAL patch_tolerance) { + void setPatchTolerance(const CoalScalar patch_tolerance) { if (patch_tolerance < 0) { COAL_LOG_WARNING( "`patch_tolerance` cannot be negative. Setting it to 0 to prevent " "bugs."); - this->m_patch_tolerance = Eigen::NumTraits::dummy_precision(); + this->m_patch_tolerance = Eigen::NumTraits::dummy_precision(); } else { this->m_patch_tolerance = patch_tolerance; } } /// @copydoc m_patch_tolerance - FCL_REAL getPatchTolerance() const { return this->m_patch_tolerance; } + CoalScalar getPatchTolerance() const { return this->m_patch_tolerance; } /// @brief Whether two ContactPatchRequest are identical or not. bool operator==(const ContactPatchRequest& other) const { @@ -1009,8 +1010,8 @@ struct COAL_DLLAPI DistanceRequest : QueryRequest { bool enable_signed_distance; /// @brief error threshold for approximate distance - FCL_REAL rel_err; // relative error, between 0 and 1 - FCL_REAL abs_err; // absolute error + CoalScalar rel_err; // relative error, between 0 and 1 + CoalScalar abs_err; // absolute error /// \param enable_nearest_points_ enables the nearest points computation. /// \param enable_signed_distance_ allows to compute the penetration depth @@ -1019,8 +1020,8 @@ struct COAL_DLLAPI DistanceRequest : QueryRequest { COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS DistanceRequest(bool enable_nearest_points_ = true, - bool enable_signed_distance_ = true, FCL_REAL rel_err_ = 0.0, - FCL_REAL abs_err_ = 0.0) + bool enable_signed_distance_ = true, + CoalScalar rel_err_ = 0.0, CoalScalar abs_err_ = 0.0) : enable_nearest_points(enable_nearest_points_), enable_signed_distance(enable_signed_distance_), rel_err(rel_err_), @@ -1054,7 +1055,7 @@ struct COAL_DLLAPI DistanceResult : QueryResult { /// DistanceRequest::enable_signed_distance is activated, min_distance <= 0. /// @note The nearest points are the points of the shapes that achieve a /// distance of `DistanceResult::min_distance`. - FCL_REAL min_distance; + CoalScalar min_distance; /// @brief normal. Vec3f normal; @@ -1085,15 +1086,15 @@ struct COAL_DLLAPI DistanceResult : QueryResult { static const int NONE = -1; DistanceResult( - FCL_REAL min_distance_ = (std::numeric_limits::max)()) + CoalScalar min_distance_ = (std::numeric_limits::max)()) : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { const Vec3f nan( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); + Vec3f::Constant(std::numeric_limits::quiet_NaN())); nearest_points[0] = nearest_points[1] = normal = nan; } /// @brief add distance information into the result - void update(FCL_REAL distance, const CollisionGeometry* o1_, + void update(CoalScalar distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) { if (min_distance > distance) { min_distance = distance; @@ -1105,7 +1106,7 @@ struct COAL_DLLAPI DistanceResult : QueryResult { } /// @brief add distance information into the result - void update(FCL_REAL distance, const CollisionGeometry* o1_, + void update(CoalScalar distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_) { if (min_distance > distance) { @@ -1137,8 +1138,8 @@ struct COAL_DLLAPI DistanceResult : QueryResult { /// @brief clear the result void clear() { const Vec3f nan( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); - min_distance = (std::numeric_limits::max)(); + Vec3f::Constant(std::numeric_limits::quiet_NaN())); + min_distance = (std::numeric_limits::max)(); o1 = NULL; o2 = NULL; b1 = NONE; @@ -1173,16 +1174,16 @@ struct COAL_DLLAPI DistanceResult : QueryResult { namespace internal { inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, CollisionResult& res, - const FCL_REAL sqrDistLowerBound) { + const CoalScalar sqrDistLowerBound) { // BV cannot find negative distance. if (res.distance_lower_bound <= 0) return; - FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; + CoalScalar new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; } inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, CollisionResult& res, - const FCL_REAL& distance, + const CoalScalar& distance, const Vec3f& p0, const Vec3f& p1, const Vec3f& normal) { if (distance < res.distance_lower_bound) { diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h index 5e7900ff0..1088ed620 100644 --- a/include/coal/collision_object.h +++ b/include/coal/collision_object.h @@ -94,7 +94,7 @@ enum NODE_TYPE { class COAL_DLLAPI CollisionGeometry { public: CollisionGeometry() - : aabb_center(Vec3f::Constant((std::numeric_limits::max)())), + : aabb_center(Vec3f::Constant((std::numeric_limits::max)())), aabb_radius(-1), cost_density(1), threshold_occupied(1), @@ -151,7 +151,7 @@ class COAL_DLLAPI CollisionGeometry { Vec3f aabb_center; /// @brief AABB radius - FCL_REAL aabb_radius; + CoalScalar aabb_radius; /// @brief AABB in local coordinate, used for tight AABB when only translation /// transform @@ -161,13 +161,13 @@ class COAL_DLLAPI CollisionGeometry { void* user_data; /// @brief collision cost for unit volume - FCL_REAL cost_density; + CoalScalar cost_density; /// @brief threshold for occupied ( >= is occupied) - FCL_REAL threshold_occupied; + CoalScalar threshold_occupied; /// @brief threshold for free (<= is free) - FCL_REAL threshold_free; + CoalScalar threshold_free; /// @brief compute center of mass virtual Vec3f computeCOM() const { return Vec3f::Zero(); } @@ -178,13 +178,13 @@ class COAL_DLLAPI CollisionGeometry { } /// @brief compute the volume - virtual FCL_REAL computeVolume() const { return 0; } + virtual CoalScalar computeVolume() const { return 0; } /// @brief compute the inertia matrix, related to the com virtual Matrix3f computeMomentofInertiaRelatedToCOM() const { Matrix3f C = computeMomentofInertia(); Vec3f com = computeCOM(); - FCL_REAL V = computeVolume(); + CoalScalar V = computeVolume(); return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], diff --git a/include/coal/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h index 2d230c141..09ae43c13 100644 --- a/include/coal/contact_patch/contact_patch_solver.h +++ b/include/coal/contact_patch/contact_patch_solver.h @@ -84,7 +84,8 @@ struct COAL_DLLAPI ContactPatchSolver { typedef void (*SupportSetFunction)(const ShapeBase* shape, SupportSet& support_set, int& hint, ShapeSupportData& support_data, - size_t num_sampled_supports, FCL_REAL tol); + size_t num_sampled_supports, + CoalScalar tol); /// @brief Number of vectors to pre-allocate in the `m_clipping_sets` vectors. static constexpr size_t default_num_preallocated_supports = 16; @@ -97,7 +98,7 @@ struct COAL_DLLAPI ContactPatchSolver { /// @brief Tolerance below which points are added to the shapes support sets. /// See @ref ContactPatchRequest::m_patch_tolerance for more details. - FCL_REAL patch_tolerance; + CoalScalar patch_tolerance; /// @brief Support set function for shape s1. mutable SupportSetFunction supportFuncShape1; @@ -133,7 +134,7 @@ struct COAL_DLLAPI ContactPatchSolver { const size_t num_contact_patch = 1; const size_t preallocated_patch_size = ContactPatch::default_preallocated_size; - const FCL_REAL patch_tolerance = 1e-3; + const CoalScalar patch_tolerance = 1e-3; const ContactPatchRequest request(num_contact_patch, preallocated_patch_size, patch_tolerance); this->set(request); diff --git a/include/coal/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx index 9eecdd071..f730b79da 100644 --- a/include/coal/contact_patch/contact_patch_solver.hxx +++ b/include/coal/contact_patch/contact_patch_solver.hxx @@ -129,7 +129,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, } // `eps` is be used to check strict positivity of determinants. - const FCL_REAL eps = Eigen::NumTraits::dummy_precision(); + const CoalScalar eps = Eigen::NumTraits::dummy_precision(); using Polygon = SupportSet::Polygon; if ((this->support_set_shape1.size() == 2) && @@ -145,7 +145,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, const Vec2f& c = pts2[0]; const Vec2f& d = pts2[1]; - const FCL_REAL det = + const CoalScalar det = (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0)); if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) || ((b - a).squaredNorm() < eps)) { @@ -154,17 +154,17 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, } const Vec2f cd = (d - c); - const FCL_REAL l = cd.squaredNorm(); + const CoalScalar l = cd.squaredNorm(); Polygon& patch = contact_patch.points(); // Project a onto [c, d] - FCL_REAL t1 = (a - c).dot(cd); + CoalScalar t1 = (a - c).dot(cd); t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l)); const Vec2f p1 = c + t1 * cd; patch.emplace_back(p1); // Project b onto [c, d] - FCL_REAL t2 = (b - c).dot(cd); + CoalScalar t2 = (b - c).dot(cd); t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l)); const Vec2f p2 = c + t2 * cd; if ((p1 - p2).squaredNorm() >= eps) { @@ -236,8 +236,8 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, const Vec2f ap1 = p1 - a; const Vec2f ap2 = p2 - a; - const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0); - const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0); + const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); + const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); if (det1 < 0 && det2 < 0) { // Both p1 and p2 are outside the clipping polygon, i.e. there is no @@ -295,8 +295,8 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, const Vec2f ap1 = p1 - a; const Vec2f ap2 = p2 - a; - const FCL_REAL det1 = ab(0) * ap1(1) - ab(1) * ap1(0); - const FCL_REAL det2 = ab(0) * ap2(1) - ab(1) * ap2(0); + const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); + const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); if (det1 < 0 && det2 < 0) { // No intersection. Continue to next segment of previous. @@ -410,13 +410,13 @@ inline Vec2f ContactPatchSolver::computeLineSegmentIntersection( const Vec2f& a, const Vec2f& b, const Vec2f& c, const Vec2f& d) { const Vec2f ab = b - a; const Vec2f n(-ab(1), ab(0)); - const FCL_REAL denominator = n.dot(c - d); + const CoalScalar denominator = n.dot(c - d); if (std::abs(denominator) < std::numeric_limits::epsilon()) { return d; } - const FCL_REAL nominator = n.dot(a - d); - FCL_REAL alpha = nominator / denominator; - alpha = std::min(1.0, std::max(0.0, alpha)); + const CoalScalar nominator = n.dot(a - d); + CoalScalar alpha = nominator / denominator; + alpha = std::min(1.0, std::max(0.0, alpha)); return alpha * c + (1 - alpha) * d; } diff --git a/include/coal/data_types.h b/include/coal/data_types.h index 0f69930ee..b9b64bb76 100644 --- a/include/coal/data_types.h +++ b/include/coal/data_types.h @@ -67,9 +67,19 @@ typedef Eigen::Matrix VecXf; typedef Eigen::Matrix Matrix3f; typedef Eigen::Matrix Matrixx3f; typedef Eigen::Matrix Matrixx2f; +typedef Eigen::Matrix MatrixXf; + +typedef double CoalScalar; +typedef Eigen::Matrix Vec3s; +typedef Eigen::Matrix Vec2s; +typedef Eigen::Matrix Vec6s; +typedef Eigen::Matrix VecXs; +typedef Eigen::Matrix Matrix3s; +typedef Eigen::Matrix Matrixx3s; +typedef Eigen::Matrix Matrixx2s; typedef Eigen::Matrix Matrixx3i; -typedef Eigen::Matrix MatrixXf; +typedef Eigen::Matrix MatrixXs; typedef Eigen::Vector2i support_func_guess_t; /// @brief Initial guess to use for the GJK algorithm diff --git a/include/coal/distance.h b/include/coal/distance.h index bf4beef7a..21da64876 100644 --- a/include/coal/distance.h +++ b/include/coal/distance.h @@ -49,34 +49,34 @@ namespace coal { /// requirements for contacts, including whether return the nearest points, this /// function performs the distance between them. Return value is the minimum /// distance generated between the two objects. -COAL_DLLAPI FCL_REAL distance(const CollisionObject* o1, - const CollisionObject* o2, - const DistanceRequest& request, - DistanceResult& result); +COAL_DLLAPI CoalScalar distance(const CollisionObject* o1, + const CollisionObject* o2, + const DistanceRequest& request, + DistanceResult& result); /// @copydoc distance(const CollisionObject*, const CollisionObject*, const /// DistanceRequest&, DistanceResult&) -COAL_DLLAPI FCL_REAL distance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result); +COAL_DLLAPI CoalScalar distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result); /// This class reduces the cost of identifying the geometry pair. /// This is mostly useful for repeated shape-shape queries. /// /// \code /// ComputeDistance calc_distance (o1, o2); -/// FCL_REAL distance = calc_distance(tf1, tf2, request, result); +/// CoalScalar distance = calc_distance(tf1, tf2, request, result); /// \endcode class COAL_DLLAPI ComputeDistance { public: ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2); - FCL_REAL operator()(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const; + CoalScalar operator()(const Transform3f& tf1, const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) const; bool operator==(const ComputeDistance& other) const { return o1 == other.o1 && o2 == other.o2 && swap_geoms == other.swap_geoms && @@ -102,9 +102,9 @@ class COAL_DLLAPI ComputeDistance { DistanceFunctionMatrix::DistanceFunc func; bool swap_geoms; - virtual FCL_REAL run(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const; + virtual CoalScalar run(const Transform3f& tf1, const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) const; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/coal/distance_func_matrix.h b/include/coal/distance_func_matrix.h index d6bd5b7e6..febcc663d 100644 --- a/include/coal/distance_func_matrix.h +++ b/include/coal/distance_func_matrix.h @@ -54,13 +54,13 @@ struct COAL_DLLAPI DistanceFunctionMatrix { /// between geometric shapes; /// 3. the request setting for distance (e.g., whether need to return nearest /// points); - typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result); + typedef CoalScalar (*DistanceFunc)(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result); /// @brief each item in the distance matrix is a function to handle distance /// between objects of type1 and type2 diff --git a/include/coal/hfield.h b/include/coal/hfield.h index 0e3269bf6..4204efc22 100644 --- a/include/coal/hfield.h +++ b/include/coal/hfield.h @@ -71,7 +71,7 @@ struct COAL_DLLAPI HFNodeBase { Eigen::DenseIndex x_id, x_size; Eigen::DenseIndex y_id, y_size; - FCL_REAL max_height; + CoalScalar max_height; int contact_active_faces; /// @brief Default constructor @@ -81,7 +81,7 @@ struct COAL_DLLAPI HFNodeBase { x_size(0), y_id(-1), y_size(0), - max_height(std::numeric_limits::lowest()), + max_height(std::numeric_limits::lowest()), contact_active_faces(0) {} /// @brief Comparison operator @@ -145,14 +145,14 @@ struct COAL_DLLAPI HFNode : public HFNodeBase { bool overlap(const HFNode& other) const { return bv.overlap(other.bv); } /// @brief Check whether two BVNode collide bool overlap(const HFNode& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { return bv.overlap(other.bv, request, sqrDistLowerBound); } /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and /// the underlying BV supports distance, return the nearest points. - FCL_REAL distance(const HFNode& other, Vec3f* P1 = NULL, - Vec3f* P2 = NULL) const { + CoalScalar distance(const HFNode& other, Vec3f* P1 = NULL, + Vec3f* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } @@ -211,8 +211,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// @brief Constructing an empty HeightField HeightField() : CollisionGeometry(), - min_height((std::numeric_limits::min)()), - max_height((std::numeric_limits::max)()) {} + min_height((std::numeric_limits::min)()), + max_height((std::numeric_limits::max)()) {} /// @brief Constructing an HeightField from its base dimensions and the set of /// heights points. @@ -225,8 +225,9 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// 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) + HeightField(const CoalScalar x_dim, const CoalScalar y_dim, + const MatrixXf& heights, + const CoalScalar min_height = (CoalScalar)0) : CollisionGeometry() { init(x_dim, y_dim, heights, min_height); } @@ -256,14 +257,14 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { const MatrixXf& getHeights() const { return heights; } /// @brief Returns the dimension of the Height Field along the X direction. - FCL_REAL getXDim() const { return x_dim; } + CoalScalar getXDim() const { return x_dim; } /// @brief Returns the dimension of the Height Field along the Y direction. - FCL_REAL getYDim() const { return y_dim; } + CoalScalar getYDim() const { return y_dim; } /// @brief Returns the minimal height value of the Height Field. - FCL_REAL getMinHeight() const { return min_height; } + CoalScalar getMinHeight() const { return min_height; } /// @brief Returns the maximal height value of the Height Field. - FCL_REAL getMaxHeight() const { return max_height; } + CoalScalar getMaxHeight() const { return max_height; } virtual HeightField* clone() const { return new HeightField(*this); } @@ -304,8 +305,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { } protected: - void init(const FCL_REAL x_dim, const FCL_REAL y_dim, const MatrixXf& heights, - const FCL_REAL min_height) { + void init(const CoalScalar x_dim, const CoalScalar y_dim, + const MatrixXf& heights, const CoalScalar min_height) { this->x_dim = x_dim; this->y_dim = y_dim; this->heights = heights.cwiseMax(min_height); @@ -334,20 +335,20 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { Vec3f computeCOM() const { return Vec3f::Zero(); } - FCL_REAL computeVolume() const { return 0; } + CoalScalar computeVolume() const { return 0; } Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); } protected: /// @brief Dimensions in meters along X and Y directions - FCL_REAL x_dim, y_dim; + CoalScalar x_dim, y_dim; /// @brief Elevation values in meters of the Height Field MatrixXf heights; /// @brief Minimal height of the Height Field: all values bellow min_height /// will be discarded. - FCL_REAL min_height, max_height; + CoalScalar min_height, max_height; /// @brief Grids along the X and Y directions. Useful for plotting or other /// related things. @@ -360,7 +361,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// @brief Build the bounding volume hierarchy int buildTree() { num_bvs = 1; - const FCL_REAL max_recursive_height = + const CoalScalar max_recursive_height = recursiveBuildTree(0, 0, heights.cols() - 1, 0, heights.rows() - 1); assert(max_recursive_height == max_height && "the maximal height is not correct"); @@ -370,16 +371,15 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { return BVH_OK; } - FCL_REAL recursiveUpdateHeight(const size_t bv_id) { + CoalScalar recursiveUpdateHeight(const size_t bv_id) { HFNode& bv_node = bvs[bv_id]; - FCL_REAL max_height; + CoalScalar max_height; if (bv_node.isLeaf()) { max_height = heights.block<2, 2>(bv_node.y_id, bv_node.x_id).maxCoeff(); } else { - FCL_REAL - max_left_height = recursiveUpdateHeight(bv_node.leftChild()), - max_right_height = recursiveUpdateHeight(bv_node.rightChild()); + CoalScalar max_left_height = recursiveUpdateHeight(bv_node.leftChild()), + max_right_height = recursiveUpdateHeight(bv_node.rightChild()); max_height = (std::max)(max_left_height, max_right_height); } @@ -395,10 +395,11 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { return max_height; } - FCL_REAL recursiveBuildTree(const size_t bv_id, const Eigen::DenseIndex x_id, - const Eigen::DenseIndex x_size, - const Eigen::DenseIndex y_id, - const Eigen::DenseIndex y_size) { + CoalScalar recursiveBuildTree(const size_t bv_id, + const Eigen::DenseIndex x_id, + const Eigen::DenseIndex x_size, + const Eigen::DenseIndex y_id, + const Eigen::DenseIndex y_size) { assert(x_id < heights.cols() && "x_id is out of bounds"); assert(y_id < heights.rows() && "y_id is out of bounds"); assert(x_size >= 0 && y_size >= 0 && @@ -406,7 +407,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); HFNode& bv_node = bvs[bv_id]; - FCL_REAL max_height; + CoalScalar max_height; if (x_size == 1 && y_size == 1) // don't build any BV for the current child node { @@ -415,7 +416,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { bv_node.first_child = num_bvs; num_bvs += 2; - FCL_REAL max_left_height = min_height, max_right_height = min_height; + CoalScalar max_left_height = min_height, max_right_height = min_height; if (x_size >= y_size) // splitting along the X axis { Eigen::DenseIndex x_size_half = x_size / 2; diff --git a/include/coal/internal/BV_splitter.h b/include/coal/internal/BV_splitter.h index 01fabf1c7..6106a203f 100644 --- a/include/coal/internal/BV_splitter.h +++ b/include/coal/internal/BV_splitter.h @@ -110,7 +110,7 @@ class BVSplitter { /// @brief The split threshold, different primitives are splitted according /// whether their projection on the split_axis is larger or smaller than the /// threshold - FCL_REAL split_value; + CoalScalar split_value; /// @brief The mesh vertices or points handled by the splitter Vec3f* vertices; @@ -150,7 +150,7 @@ class BVSplitter { axis = 1; split_axis = axis; - FCL_REAL sum = 0; + CoalScalar sum = 0; if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { @@ -181,7 +181,7 @@ class BVSplitter { axis = 1; split_axis = axis; - std::vector proj((size_t)num_primitives); + std::vector proj((size_t)num_primitives); if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { diff --git a/include/coal/internal/intersect.h b/include/coal/internal/intersect.h index 0353df055..acbf532f2 100644 --- a/include/coal/internal/intersect.h +++ b/include/coal/internal/intersect.h @@ -48,7 +48,7 @@ namespace coal { class COAL_DLLAPI Intersect { public: static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, - const Vec3f& v3, Vec3f* n, FCL_REAL* t); + const Vec3f& v3, Vec3f* n, CoalScalar* t); }; // class Intersect /// @brief Project functions @@ -57,10 +57,10 @@ class COAL_DLLAPI Project { struct COAL_DLLAPI ProjectResult { /// @brief Parameterization of the projected point (based on the simplex to /// be projected, use 2 or 3 or 4 of the array) - FCL_REAL parameterization[4]; + CoalScalar parameterization[4]; /// @brief square distance from the query point to the projected simplex - FCL_REAL sqr_distance; + CoalScalar sqr_distance; /// @brief the code of the projection type unsigned int encode; @@ -113,13 +113,13 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, + Vec3f& Q); - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, Vec3f& P, + Vec3f& Q); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -131,9 +131,9 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Matrix3f& R, const Vec3f& Tl, Vec3f& P, + Vec3f& Q); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -145,8 +145,8 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, Vec3f& P, Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Transform3f& tf, Vec3f& P, Vec3f& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices @@ -158,11 +158,11 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Matrix3f& R, const Vec3f& Tl, Vec3f& P, + Vec3f& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices @@ -174,10 +174,10 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, Vec3f& P, Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Transform3f& tf, Vec3f& P, Vec3f& Q); }; } // namespace coal diff --git a/include/coal/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h index 64b11d3b8..07a4a97e6 100644 --- a/include/coal/internal/shape_shape_func.h +++ b/include/coal/internal/shape_shape_func.h @@ -49,18 +49,20 @@ namespace coal { template struct ShapeShapeDistancer { - static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result) { + static CoalScalar run(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; // Witness points on shape1 and shape2, normal pointing from shape1 to // shape2. Vec3f p1, p2, normal; - const FCL_REAL distance = ShapeShapeDistancer::run( - o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2, - normal); + const CoalScalar distance = + ShapeShapeDistancer::run( + o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2, + normal); result.update(distance, o1, o2, DistanceResult::NONE, DistanceResult::NONE, p1, p2, normal); @@ -68,11 +70,11 @@ struct ShapeShapeDistancer { return distance; } - static FCL_REAL run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const bool compute_signed_distance, Vec3f& p1, Vec3f& p2, - Vec3f& normal) { + static CoalScalar run(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, + const bool compute_signed_distance, Vec3f& p1, + Vec3f& p2, Vec3f& normal) { const ShapeType1* obj1 = static_cast(o1); const ShapeType2* obj2 = static_cast(o2); return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2, @@ -88,11 +90,12 @@ struct ShapeShapeDistancer { /// primitive shapes. /// @note This function might be specialized for some pairs of shapes. template -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { return ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, request, result); } @@ -109,11 +112,12 @@ namespace internal { /// these structures. /// @note This function might be specialized for some pairs of shapes. template -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const bool compute_signed_distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const bool compute_signed_distance, Vec3f& p1, + Vec3f& p2, Vec3f& normal) { return ::coal::ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal); } @@ -140,11 +144,11 @@ struct ShapeShapeCollider { const bool compute_penetration = request.enable_contact || (request.security_margin < 0); Vec3f p1, p2, normal; - FCL_REAL distance = internal::ShapeShapeDistance( + CoalScalar distance = internal::ShapeShapeDistance( o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal); size_t num_contacts = 0; - const FCL_REAL distToCollision = distance - request.security_margin; + const CoalScalar distToCollision = distance - request.security_margin; internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, p1, p2, normal); @@ -212,19 +216,19 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ template <> \ - COAL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ + COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ Vec3f& p2, Vec3f& normal); \ template <> \ - COAL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ + COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ Vec3f& p2, Vec3f& normal); \ template <> \ - inline COAL_DLLAPI FCL_REAL ShapeShapeDistance( \ + inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ @@ -239,7 +243,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, return result.min_distance; \ } \ template <> \ - inline COAL_DLLAPI FCL_REAL ShapeShapeDistance( \ + inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ @@ -256,13 +260,13 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, #define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \ template <> \ - COAL_DLLAPI FCL_REAL internal::ShapeShapeDistance( \ + COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ Vec3f& p2, Vec3f& normal); \ template <> \ - inline COAL_DLLAPI FCL_REAL ShapeShapeDistance( \ + inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ diff --git a/include/coal/internal/tools.h b/include/coal/internal/tools.h index 02cf695ee..ab26633d1 100644 --- a/include/coal/internal/tools.h +++ b/include/coal/internal/tools.h @@ -203,8 +203,8 @@ void eigen(const Eigen::MatrixBase& m, template bool isEqual(const Eigen::MatrixBase& lhs, const Eigen::MatrixBase& rhs, - const FCL_REAL tol = std::numeric_limits::epsilon() * - 100) { + const CoalScalar tol = std::numeric_limits::epsilon() * + 100) { return ((lhs - rhs).array().abs() < tol).all(); } diff --git a/include/coal/internal/traversal_node_base.h b/include/coal/internal/traversal_node_base.h index 3dc33f321..238dfc1a3 100644 --- a/include/coal/internal/traversal_node_base.h +++ b/include/coal/internal/traversal_node_base.h @@ -112,11 +112,11 @@ class CollisionTraversalNodeBase : public TraversalNodeBase { /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. virtual bool BVDisjoints(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const = 0; + CoalScalar& sqrDistLowerBound) const = 0; /// @brief Leaf test between node b1 and b2, if they are both leafs virtual void leafCollides(unsigned int /*b1*/, unsigned int /*b2*/, - FCL_REAL& /*sqrDistLowerBound*/) const = 0; + CoalScalar& /*sqrDistLowerBound*/) const = 0; /// @brief Check whether the traversal can stop bool canStop() const { return this->request.isSatisfied(*(this->result)); } @@ -145,16 +145,16 @@ class DistanceTraversalNodeBase : public TraversalNodeBase { /// @brief BV test between b1 and b2 /// @return a lower bound of the distance between the two BV. /// @note except for OBB, this method returns the distance. - virtual FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, - unsigned int /*b2*/) const { - return (std::numeric_limits::max)(); + virtual CoalScalar BVDistanceLowerBound(unsigned int /*b1*/, + unsigned int /*b2*/) const { + return (std::numeric_limits::max)(); } /// @brief Leaf test between node b1 and b2, if they are both leafs virtual void leafComputeDistance(unsigned int b1, unsigned int b2) const = 0; /// @brief Check whether the traversal can stop - virtual bool canStop(FCL_REAL /*c*/) const { return false; } + virtual bool canStop(CoalScalar /*c*/) const { return false; } /// @brief request setting for distance DistanceRequest request; diff --git a/include/coal/internal/traversal_node_bvh_hfield.h b/include/coal/internal/traversal_node_bvh_hfield.h index c9c1c659a..ad2d203dd 100644 --- a/include/coal/internal/traversal_node_bvh_hfield.h +++ b/include/coal/internal/traversal_node_bvh_hfield.h @@ -99,8 +99,8 @@ class MeshHeightFieldCollisionTraversalNode /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); @@ -144,7 +144,7 @@ class MeshHeightFieldCollisionTraversalNode /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. bool BVDisjoints(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; if (RTIsIdentity) return !this->model1->getBV(b1).overlap(this->model2->getBV(b2), @@ -173,7 +173,7 @@ class MeshHeightFieldCollisionTraversalNode /// and the object are not colliding, the penetration depth is /// negative. void leafCollides(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); @@ -196,14 +196,14 @@ class MeshHeightFieldCollisionTraversalNode Vec3f p1, p2; // closest points if no collision contact points if collision. Vec3f normal; - FCL_REAL distance; + CoalScalar distance; solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2, normal); - FCL_REAL distToCollision = distance - this->request.security_margin; + CoalScalar distToCollision = distance - this->request.security_margin; sqrDistLowerBound = distance * distance; if (distToCollision <= 0) { // collision Vec3f p(p1); // contact point - FCL_REAL penetrationDepth(0); + CoalScalar penetrationDepth(0); if (this->result->numContacts() < this->request.num_max_contacts) { // How much (Q1, Q2, Q3) should be moved so that all vertices are // above (P1, P2, P3). @@ -227,7 +227,7 @@ class MeshHeightFieldCollisionTraversalNode /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; Vec3f* vertices1 Triangle* tri_indices1; @@ -250,19 +250,19 @@ typedef MeshHeightFieldCollisionTraversalNode namespace details { template struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + static CoalScalar run(const BVNode& b1, const BVNode& b2) { return b1.distance(b2); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { + static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { return distance(R, T, b1.bv, b2.bv); } }; template <> struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { @@ -271,9 +271,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const Matrix3f& R, const Vec3f& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { @@ -286,8 +286,8 @@ struct DistanceTraversalBVDistanceLowerBound_impl { template <> struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { @@ -296,9 +296,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const Matrix3f& R, const Vec3f& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { @@ -338,8 +338,8 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); @@ -376,7 +376,7 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @brief Traversal node for distance computation between two meshes @@ -416,7 +416,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { if (enable_statistics) num_bv_tests++; if (RTIsIdentity) return details::DistanceTraversalBVDistanceLowerBound_impl::run( @@ -450,21 +450,21 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { // nearest point pair Vec3f P1, P2, normal; - FCL_REAL d2; + CoalScalar d2; if (RTIsIdentity) d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, P2); else d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, RT._R(), RT._T(), P1, P2); - FCL_REAL d = sqrt(d2); + CoalScalar d = sqrt(d2); this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2, normal); } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { + bool canStop(CoalScalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; @@ -478,8 +478,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { Triangle* tri_indices2; /// @brief relative and absolute error, default value is 0.01 for both terms - FCL_REAL rel_err; - FCL_REAL abs_err; + CoalScalar rel_err; + CoalScalar abs_err; details::RelativeTransformation RT; @@ -501,7 +501,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { init_tri2_points[2] = vertices2[init_tri2[2]]; Vec3f p1, p2, normal; - FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance( + CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance( init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), RT._T(), p1, p2)); diff --git a/include/coal/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h index 3b5d09ce6..ec68e528f 100644 --- a/include/coal/internal/traversal_node_bvh_shape.h +++ b/include/coal/internal/traversal_node_bvh_shape.h @@ -89,7 +89,7 @@ class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @brief Traversal node for collision between mesh and shape @@ -117,7 +117,7 @@ class MeshShapeCollisionTraversalNode /// distance between bounding volumes. /// @brief BV culling test in one BVTT node bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; bool disjoint; if (RTIsIdentity) @@ -136,7 +136,7 @@ class MeshShapeCollisionTraversalNode /// @brief Intersection testing between leaves (one triangle and one shape) void leafCollides(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); @@ -154,7 +154,7 @@ class MeshShapeCollisionTraversalNode const bool compute_penetration = this->request.enable_contact || (this->request.security_margin < 0); Vec3f c1, c2, normal; - FCL_REAL distance; + CoalScalar distance; if (RTIsIdentity) { static const Transform3f Id; @@ -166,7 +166,7 @@ class MeshShapeCollisionTraversalNode &tri, this->tf1, this->model2, this->tf2, this->nsolver, compute_penetration, c1, c2, normal); } - const FCL_REAL distToCollision = distance - this->request.security_margin; + const CoalScalar distToCollision = distance - this->request.security_margin; internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), distToCollision, c1, c2, normal); @@ -226,7 +226,7 @@ class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { return model1->getBV(b1).bv.distance(model2_bv); } @@ -236,7 +236,7 @@ class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @brief Traversal node for distance computation between shape and BVH @@ -268,7 +268,7 @@ class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { + CoalScalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const { return model1_bv.distance(model2->getBV(b2).bv); } @@ -278,7 +278,7 @@ class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @brief Traversal node for distance between mesh and shape @@ -309,7 +309,7 @@ class MeshShapeDistanceTraversalNode this->vertices[tri_id[2]]); Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( + const CoalScalar distance = internal::ShapeShapeDistance( &tri, this->tf1, this->model2, this->tf2, this->nsolver, this->request.enable_signed_distance, p1, p2, normal); @@ -318,7 +318,7 @@ class MeshShapeDistanceTraversalNode } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { + bool canStop(CoalScalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; @@ -328,8 +328,8 @@ class MeshShapeDistanceTraversalNode Vec3f* vertices; Triangle* tri_indices; - FCL_REAL rel_err; - FCL_REAL abs_err; + CoalScalar rel_err; + CoalScalar abs_err; const GJKSolver* nsolver; }; @@ -354,7 +354,7 @@ void meshShapeDistanceOrientedNodeleafComputeDistance( vertices[tri_id[2]]); Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( + const CoalScalar distance = internal::ShapeShapeDistance( &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); @@ -373,7 +373,7 @@ static inline void distancePreprocessOrientedNode( vertices[tri_id[2]]); Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( + const CoalScalar distance = internal::ShapeShapeDistance( &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, @@ -401,7 +401,7 @@ class MeshShapeDistanceTraversalNodeRSS void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); @@ -431,7 +431,7 @@ class MeshShapeDistanceTraversalNodekIOS void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); @@ -461,7 +461,7 @@ class MeshShapeDistanceTraversalNodeOBBRSS void postprocess() {} - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { if (this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); diff --git a/include/coal/internal/traversal_node_bvhs.h b/include/coal/internal/traversal_node_bvhs.h index 2f6fd1c91..99a442f39 100644 --- a/include/coal/internal/traversal_node_bvhs.h +++ b/include/coal/internal/traversal_node_bvhs.h @@ -86,8 +86,8 @@ class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); @@ -124,7 +124,7 @@ class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @brief Traversal node for collision between two meshes @@ -149,7 +149,7 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { /// @retval sqrDistLowerBound square of a lower bound of the minimal /// distance between bounding volumes. bool BVDisjoints(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; bool disjoint; if (RTIsIdentity) @@ -181,7 +181,7 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { /// and the object are not colliding, the penetration depth is /// negative. void leafCollides(unsigned int b1, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); @@ -210,11 +210,11 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { this->request.enable_contact || (this->request.security_margin < 0); Vec3f p1, p2, normal; DistanceResult distanceResult; - FCL_REAL distance = internal::ShapeShapeDistance( + CoalScalar distance = internal::ShapeShapeDistance( &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1, p2, normal); - const FCL_REAL distToCollision = distance - this->request.security_margin; + const CoalScalar distToCollision = distance - this->request.security_margin; internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result), distToCollision, p1, p2, normal); @@ -252,19 +252,19 @@ typedef MeshCollisionTraversalNode MeshCollisionTraversalNodeOBBRSS; namespace details { template struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { + static CoalScalar run(const BVNode& b1, const BVNode& b2) { return b1.distance(b2); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { + static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + const BVNode& b2) { return distance(R, T, b1.bv, b2.bv); } }; template <> struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { @@ -273,9 +273,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const Matrix3f& R, const Vec3f& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { @@ -288,8 +288,8 @@ struct DistanceTraversalBVDistanceLowerBound_impl { template <> struct DistanceTraversalBVDistanceLowerBound_impl { - static FCL_REAL run(const BVNode& b1, const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (b1.overlap(b2, request, sqrDistLowerBound)) { @@ -298,9 +298,9 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, - const BVNode& b2) { - FCL_REAL sqrDistLowerBound; + static CoalScalar run(const Matrix3f& R, const Vec3f& T, + const BVNode& b1, const BVNode& b2) { + CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); // request.break_distance = ? if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) { @@ -340,8 +340,8 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(unsigned int b1, unsigned int b2) const { - FCL_REAL sz1 = model1->getBV(b1).bv.size(); - FCL_REAL sz2 = model2->getBV(b2).bv.size(); + CoalScalar sz1 = model1->getBV(b1).bv.size(); + CoalScalar sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); @@ -378,7 +378,7 @@ class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @brief Traversal node for distance computation between two meshes @@ -418,7 +418,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const { if (enable_statistics) num_bv_tests++; if (RTIsIdentity) return details::DistanceTraversalBVDistanceLowerBound_impl::run( @@ -452,21 +452,21 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { // nearest point pair Vec3f P1, P2, normal; - FCL_REAL d2; + CoalScalar d2; if (RTIsIdentity) d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1, P2); else d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, RT._R(), RT._T(), P1, P2); - FCL_REAL d = sqrt(d2); + CoalScalar d = sqrt(d2); this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2, normal); } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { + bool canStop(CoalScalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; @@ -480,8 +480,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { Triangle* tri_indices2; /// @brief relative and absolute error, default value is 0.01 for both terms - FCL_REAL rel_err; - FCL_REAL abs_err; + CoalScalar rel_err; + CoalScalar abs_err; details::RelativeTransformation RT; @@ -503,7 +503,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { init_tri2_points[2] = vertices2[init_tri2[2]]; Vec3f p1, p2, normal; - FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance( + CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance( init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), RT._T(), p1, p2)); diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h index e15c00e18..1b5f6b1e7 100644 --- a/include/coal/internal/traversal_node_hfield_shape.h +++ b/include/coal/internal/traversal_node_hfield_shape.h @@ -63,10 +63,10 @@ Convex buildConvexQuadrilateral(const HFNode& node, const VecXf& x_grid = model.getXGrid(); const VecXf& y_grid = model.getYGrid(); - const FCL_REAL min_height = model.getMinHeight(); + const CoalScalar min_height = model.getMinHeight(); - const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], - y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], + y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); @@ -126,11 +126,11 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, const VecXf& x_grid = model.getXGrid(); const VecXf& y_grid = model.getYGrid(); - const FCL_REAL min_height = model.getMinHeight(); + const CoalScalar min_height = model.getMinHeight(); - const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], - y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; - const FCL_REAL max_height = node.max_height; + const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], + y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const CoalScalar max_height = node.max_height; const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); @@ -279,7 +279,7 @@ inline Vec3f projectPointOnTriangle(const Vec3f& contact_point, return contact_point_projected; } -inline FCL_REAL distanceContactPointToTriangle( +inline CoalScalar distanceContactPointToTriangle( const Vec3f& contact_point, const Triangle& triangle, const std::vector& points) { const Vec3f contact_point_projected = @@ -287,10 +287,10 @@ inline FCL_REAL distanceContactPointToTriangle( return (contact_point_projected - contact_point).norm(); } -inline FCL_REAL distanceContactPointToFace(const size_t face_id, - const Vec3f& contact_point, - const Convex& convex, - size_t& closest_face_id) { +inline CoalScalar distanceContactPointToFace(const size_t face_id, + const Vec3f& contact_point, + const Convex& convex, + size_t& closest_face_id) { assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]"); const std::vector& points = *(convex.points); @@ -300,11 +300,11 @@ inline FCL_REAL distanceContactPointToFace(const size_t face_id, return distanceContactPointToTriangle(contact_point, triangle, points); } else { const Triangle& triangle1 = (*(convex.polygons))[face_id]; - const FCL_REAL distance_to_triangle1 = + const CoalScalar distance_to_triangle1 = distanceContactPointToTriangle(contact_point, triangle1, points); const Triangle& triangle2 = (*(convex.polygons))[face_id + 1]; - const FCL_REAL distance_to_triangle2 = + const CoalScalar distance_to_triangle2 = distanceContactPointToTriangle(contact_point, triangle2, points); if (distance_to_triangle1 > distance_to_triangle2) { @@ -320,10 +320,10 @@ inline FCL_REAL distanceContactPointToFace(const size_t face_id, template bool binCorrection(const Convex& convex, const int convex_active_faces, const Shape& shape, - const Transform3f& shape_pose, FCL_REAL& distance, + const Transform3f& shape_pose, CoalScalar& distance, Vec3f& contact_1, Vec3f& contact_2, Vec3f& normal, Vec3f& face_normal, const bool is_collision) { - const FCL_REAL prec = 1e-12; + const CoalScalar prec = 1e-12; const std::vector& points = *(convex.points); bool hfield_witness_is_on_bin_side = true; @@ -341,11 +341,12 @@ bool binCorrection(const Convex& convex, if (convex_active_faces & 8) active_faces.push_back(6); Triangle face_triangle; - FCL_REAL shortest_distance_to_face = (std::numeric_limits::max)(); + CoalScalar shortest_distance_to_face = + (std::numeric_limits::max)(); face_normal = normal; for (const size_t active_face : active_faces) { size_t closest_face_id; - const FCL_REAL distance_to_face = distanceContactPointToFace( + const CoalScalar distance_to_face = distanceContactPointToFace( active_face, contact_1, convex, closest_face_id); const bool contact_point_is_on_face = distance_to_face <= prec; @@ -378,9 +379,9 @@ bool binCorrection(const Convex& convex, shape_pose.rotation() * _support + shape_pose.translation(); // Project support into the inclined bin having triangle - const FCL_REAL offset_plane = face_normal.dot(face_pointA); + const CoalScalar offset_plane = face_normal.dot(face_pointA); const Plane projection_plane(face_normal, offset_plane); - const FCL_REAL distance_support_projection_plane = + const CoalScalar distance_support_projection_plane = projection_plane.signedDistance(support); const Vec3f projected_support = @@ -404,7 +405,7 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, const Convex& convex2, const int convex2_active_faces, const Transform3f& tf1, const Shape& shape, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& c1, Vec3f& c2, Vec3f& normal, + CoalScalar& distance, Vec3f& c1, Vec3f& c2, Vec3f& normal, Vec3f& normal_top, bool& hfield_witness_is_on_bin_side) { enum { RTIsIdentity = Options & RelativeTransformationIsIdentity }; @@ -417,7 +418,7 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, const bool compute_penetration = true; Vec3f contact1_1, contact1_2, contact2_1, contact2_2; Vec3f normal1, normal1_top, normal2, normal2_top; - FCL_REAL distance1, distance2; + CoalScalar distance1, distance2; if (RTIsIdentity) { distance1 = internal::ShapeShapeDistance, Shape>( @@ -515,7 +516,7 @@ class HeightFieldShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: typedef CollisionTraversalNodeBase Base; - typedef Eigen::Array Array2d; + typedef Eigen::Array Array2d; enum { Options = _Options, @@ -556,7 +557,7 @@ class HeightFieldShapeCollisionTraversalNode /// distance between bounding volumes. /// @brief BV culling test in one BVTT node bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; bool disjoint; @@ -580,7 +581,7 @@ class HeightFieldShapeCollisionTraversalNode /// @brief Intersection testing between leaves (one Convex and one shape) void leafCollides(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { count++; if (this->enable_statistics) this->num_leaf_tests++; const HFNode& node = this->model1->getBV(b1); @@ -606,7 +607,7 @@ class HeightFieldShapeCollisionTraversalNode convex2.computeLocalAABB(); } - FCL_REAL distance; + CoalScalar distance; // Vec3f contact_point, normal; Vec3f c1, c2, normal, normal_face; bool hfield_witness_is_on_bin_side; @@ -616,7 +617,7 @@ class HeightFieldShapeCollisionTraversalNode convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance, c1, c2, normal, normal_face, hfield_witness_is_on_bin_side); - FCL_REAL distToCollision = distance - this->request.security_margin; + CoalScalar distToCollision = distance - this->request.security_margin; if (distToCollision <= this->request.collision_distance_threshold) { sqrDistLowerBound = 0; if (this->result->numContacts() < this->request.num_max_contacts) { @@ -647,7 +648,7 @@ class HeightFieldShapeCollisionTraversalNode mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; mutable int count; }; @@ -696,7 +697,7 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { + CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { return model1->getBV(b1).bv.distance( model2_bv); // TODO(jcarpent): tf1 is not taken into account here. } @@ -715,7 +716,7 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { details::buildConvexQuadrilateral(node, *this->model1); Vec3f p1, p2, normal; - const FCL_REAL distance = + const CoalScalar distance = internal::ShapeShapeDistance( &convex, this->tf1, this->model2, this->tf2, this->nsolver, this->request.enable_signed_distance, p1, p2, normal); @@ -725,15 +726,15 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { } /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { + bool canStop(CoalScalar c) const { if ((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } - FCL_REAL rel_err; - FCL_REAL abs_err; + CoalScalar rel_err; + CoalScalar abs_err; const GJKSolver* nsolver; @@ -743,7 +744,7 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { mutable int num_bv_tests; mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; + mutable CoalScalar query_time_seconds; }; /// @} diff --git a/include/coal/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h index 33bd2e1f0..29cefe594 100644 --- a/include/coal/internal/traversal_node_octree.h +++ b/include/coal/internal/traversal_node_octree.h @@ -154,7 +154,7 @@ class COAL_DLLAPI OcTreeSolver { void OcTreeHeightFieldIntersect( const OcTree* tree1, const HeightField* tree2, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, - CollisionResult& result_, FCL_REAL& sqrDistLowerBound) const { + CollisionResult& result_, CoalScalar& sqrDistLowerBound) const { crequest = &request_; cresult = &result_; @@ -169,7 +169,7 @@ class COAL_DLLAPI OcTreeSolver { const Transform3f& tf2, const CollisionRequest& request_, CollisionResult& result_, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { crequest = &request_; cresult = &result_; @@ -260,7 +260,7 @@ class COAL_DLLAPI OcTreeSolver { } Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( + const CoalScalar distance = internal::ShapeShapeDistance( &box, box_tf, &s, tf2, this->solver, this->drequest->enable_signed_distance, p1, p2, normal); @@ -283,7 +283,7 @@ class COAL_DLLAPI OcTreeSolver { AABB aabb1; convertBV(child_bv, tf1, aabb1); - FCL_REAL d = aabb1.distance(aabb2); + CoalScalar d = aabb1.distance(aabb2); if (d < dresult->min_distance) { if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2)) @@ -315,7 +315,7 @@ class COAL_DLLAPI OcTreeSolver { else { OBB obb1; convertBV(bv1, tf1, obb1); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, sqrDistLowerBound); @@ -391,9 +391,10 @@ class COAL_DLLAPI OcTreeSolver { (*(tree2->vertices))[tri_id[2]]); Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( - &box, box_tf, &tri, tf2, this->solver, - this->drequest->enable_signed_distance, p1, p2, normal); + const CoalScalar distance = + internal::ShapeShapeDistance( + &box, box_tf, &tri, tf2, this->solver, + this->drequest->enable_signed_distance, p1, p2, normal); this->dresult->update(distance, tree1, tree2, (int)(root1 - tree1->getRoot()), @@ -415,7 +416,7 @@ class COAL_DLLAPI OcTreeSolver { AABB child_bv; computeChildBV(bv1, i, child_bv); - FCL_REAL d; + CoalScalar d; AABB aabb1, aabb2; convertBV(child_bv, tf1, aabb1); convertBV(tree2->getBV(root2).bv, tf2, aabb2); @@ -429,7 +430,7 @@ class COAL_DLLAPI OcTreeSolver { } } } else { - FCL_REAL d; + CoalScalar d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); unsigned int child = (unsigned int)tree2->getBV(root2).leftChild(); @@ -483,7 +484,7 @@ class COAL_DLLAPI OcTreeSolver { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bvn2.bv, tf2, obb2); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { internal::updateDistanceLowerBoundFromBV(*crequest, *cresult, sqrDistLowerBound); @@ -515,10 +516,10 @@ class COAL_DLLAPI OcTreeSolver { const bool compute_penetration = this->crequest->enable_contact || (this->crequest->security_margin < 0); Vec3f c1, c2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( + const CoalScalar distance = internal::ShapeShapeDistance( &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2, normal); - const FCL_REAL distToCollision = + const CoalScalar distToCollision = distance - this->crequest->security_margin; internal::updateDistanceLowerBoundFromLeaf( @@ -566,7 +567,7 @@ class COAL_DLLAPI OcTreeSolver { bool OcTreeHeightFieldIntersectRecurse( const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const HeightField* tree2, unsigned int root2, const Transform3f& tf1, - const Transform3f& tf2, FCL_REAL& sqrDistLowerBound) const { + const Transform3f& tf2, CoalScalar& sqrDistLowerBound) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? @@ -587,7 +588,7 @@ class COAL_DLLAPI OcTreeSolver { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bvn2.bv, tf2, obb2); - FCL_REAL sqrDistLowerBound_; + CoalScalar sqrDistLowerBound_; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) { if (sqrDistLowerBound_ < sqrDistLowerBound) sqrDistLowerBound = sqrDistLowerBound_; @@ -618,7 +619,7 @@ class COAL_DLLAPI OcTreeSolver { } Vec3f c1, c2, normal, normal_top; - FCL_REAL distance; + CoalScalar distance; bool hfield_witness_is_on_bin_side; bool collision = details::shapeDistance( @@ -626,7 +627,7 @@ class COAL_DLLAPI OcTreeSolver { convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal, normal_top, hfield_witness_is_on_bin_side); - FCL_REAL distToCollision = + CoalScalar distToCollision = distance - crequest->security_margin * (normal_top.dot(normal)); if (distToCollision <= crequest->collision_distance_threshold) { @@ -686,7 +687,7 @@ class COAL_DLLAPI OcTreeSolver { bool HeightFieldOcTreeIntersectRecurse( const HeightField* tree1, unsigned int root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1, - const Transform3f& tf2, FCL_REAL& sqrDistLowerBound) const { + const Transform3f& tf2, CoalScalar& sqrDistLowerBound) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? @@ -707,7 +708,7 @@ class COAL_DLLAPI OcTreeSolver { OBB obb1, obb2; convertBV(bvn1.bv, tf1, obb1); convertBV(bv2, tf2, obb2); - FCL_REAL sqrDistLowerBound_; + CoalScalar sqrDistLowerBound_; if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) { if (sqrDistLowerBound_ < sqrDistLowerBound) sqrDistLowerBound = sqrDistLowerBound_; @@ -738,7 +739,7 @@ class COAL_DLLAPI OcTreeSolver { } Vec3f c1, c2, normal, normal_top; - FCL_REAL distance; + CoalScalar distance; bool hfield_witness_is_on_bin_side; bool collision = details::shapeDistance( @@ -746,7 +747,7 @@ class COAL_DLLAPI OcTreeSolver { convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal, normal_top, hfield_witness_is_on_bin_side); - FCL_REAL distToCollision = + CoalScalar distToCollision = distance - crequest->security_margin * (normal_top.dot(normal)); if (distToCollision <= crequest->collision_distance_threshold) { @@ -820,7 +821,7 @@ class COAL_DLLAPI OcTreeSolver { } Vec3f p1, p2, normal; - const FCL_REAL distance = internal::ShapeShapeDistance( + const CoalScalar distance = internal::ShapeShapeDistance( &box1, box1_tf, &box2, box2_tf, this->solver, this->drequest->enable_signed_distance, p1, p2, normal); @@ -844,7 +845,7 @@ class COAL_DLLAPI OcTreeSolver { AABB child_bv; computeChildBV(bv1, i, child_bv); - FCL_REAL d; + CoalScalar d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); convertBV(bv2, tf2, aabb2); @@ -864,7 +865,7 @@ class COAL_DLLAPI OcTreeSolver { AABB child_bv; computeChildBV(bv2, i, child_bv); - FCL_REAL d; + CoalScalar d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); convertBV(bv2, tf2, aabb2); @@ -907,7 +908,7 @@ class COAL_DLLAPI OcTreeSolver { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) { if (cresult->distance_lower_bound > 0 && sqrDistLowerBound < @@ -948,11 +949,11 @@ class COAL_DLLAPI OcTreeSolver { const bool compute_penetration = (this->crequest->enable_contact || (this->crequest->security_margin < 0)); Vec3f c1, c2, normal; - FCL_REAL distance = internal::ShapeShapeDistance( + CoalScalar distance = internal::ShapeShapeDistance( &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1, c2, normal); - const FCL_REAL distToCollision = + const CoalScalar distToCollision = distance - this->crequest->security_margin; internal::updateDistanceLowerBoundFromLeaf( @@ -1016,11 +1017,11 @@ class COAL_DLLAPI OcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; } + bool BVDisjoints(unsigned, unsigned, CoalScalar&) const { return false; } - void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const { + void leafCollides(unsigned, unsigned, CoalScalar& sqrDistLowerBound) const { otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } @@ -1045,14 +1046,14 @@ class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { return false; } void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } @@ -1078,14 +1079,14 @@ class COAL_DLLAPI OcTreeShapeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int, coal::FCL_REAL&) const { + bool BVDisjoints(unsigned int, unsigned int, coal::CoalScalar&) const { return false; } void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } @@ -1110,14 +1111,14 @@ class COAL_DLLAPI MeshOcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { return false; } void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } @@ -1142,14 +1143,14 @@ class COAL_DLLAPI OcTreeMeshCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { return false; } void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); - sqrDistLowerBound = std::max((FCL_REAL)0, result->distance_lower_bound); + sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound); sqrDistLowerBound *= sqrDistLowerBound; } @@ -1174,12 +1175,12 @@ class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { return false; } void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request, *result, sqrDistLowerBound); } @@ -1205,12 +1206,12 @@ class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { + bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const { return false; } void leafCollides(unsigned int, unsigned int, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request, *result, sqrDistLowerBound); } @@ -1239,9 +1240,9 @@ class COAL_DLLAPI OcTreeDistanceTraversalNode otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned, unsigned) const { return -1; } + CoalScalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; } - bool BVDistanceLowerBound(unsigned, unsigned, FCL_REAL&) const { + bool BVDistanceLowerBound(unsigned, unsigned, CoalScalar&) const { return false; } @@ -1267,7 +1268,9 @@ class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); @@ -1291,7 +1294,9 @@ class COAL_DLLAPI OcTreeShapeDistanceTraversalNode otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); @@ -1315,7 +1320,9 @@ class COAL_DLLAPI MeshOcTreeDistanceTraversalNode otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); @@ -1339,7 +1346,9 @@ class COAL_DLLAPI OcTreeMeshDistanceTraversalNode otsolver = NULL; } - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; } + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { + return -1; + } void leafComputeDistance(unsigned int, unsigned int) const { otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); diff --git a/include/coal/internal/traversal_node_shapes.h b/include/coal/internal/traversal_node_shapes.h index b34eaad59..a10d8b715 100644 --- a/include/coal/internal/traversal_node_shapes.h +++ b/include/coal/internal/traversal_node_shapes.h @@ -65,12 +65,12 @@ class COAL_DLLAPI ShapeCollisionTraversalNode } /// @brief BV culling test in one BVTT node - bool BVDisjoints(int, int, FCL_REAL&) const { + bool BVDisjoints(int, int, CoalScalar&) const { COAL_THROW_PRETTY("Not implemented", std::runtime_error); } /// @brief Intersection testing between leaves (two shapes) - void leafCollides(int, int, FCL_REAL&) const { + void leafCollides(int, int, CoalScalar&) const { ShapeShapeCollide(this->model1, this->tf1, this->model2, this->tf2, this->nsolver, this->request, *(this->result)); } @@ -99,7 +99,7 @@ class COAL_DLLAPI ShapeDistanceTraversalNode } /// @brief BV culling test in one BVTT node - FCL_REAL BVDistanceLowerBound(unsigned int, unsigned int) const { + CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; // should not be used } diff --git a/include/coal/internal/traversal_recurse.h b/include/coal/internal/traversal_recurse.h index ad28bcd24..c0626b514 100644 --- a/include/coal/internal/traversal_recurse.h +++ b/include/coal/internal/traversal_recurse.h @@ -53,10 +53,11 @@ namespace coal { /// @retval sqrDistLowerBound squared lower bound on distance between objects. void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, - FCL_REAL& sqrDistLowerBound); + CoalScalar& sqrDistLowerBound); void collisionNonRecurse(CollisionTraversalNodeBase* node, - BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound); + BVHFrontList* front_list, + CoalScalar& sqrDistLowerBound); /// @brief Recurse function for distance void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h index 0795da1f6..3bfcd0c37 100644 --- a/include/coal/math/transform.h +++ b/include/coal/math/transform.h @@ -43,8 +43,8 @@ namespace coal { -COAL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; -typedef Eigen::Quaternion Quatf; +COAL_DEPRECATED typedef Eigen::Quaternion Quaternion3f; +typedef Eigen::Quaternion Quatf; static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) { o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")"; @@ -190,8 +190,8 @@ class COAL_DLLAPI Transform3f { /// @brief check whether the transform is identity inline bool isIdentity( - const FCL_REAL& prec = - Eigen::NumTraits::dummy_precision()) const { + const CoalScalar& prec = + Eigen::NumTraits::dummy_precision()) const { return R.isIdentity(prec) && T.isZero(prec); } @@ -218,26 +218,26 @@ class COAL_DLLAPI Transform3f { template inline Quatf fromAxisAngle(const Eigen::MatrixBase& axis, - FCL_REAL angle) { - return Quatf(Eigen::AngleAxis(angle, axis)); + CoalScalar angle) { + return Quatf(Eigen::AngleAxis(angle, axis)); } /// @brief Uniformly random quaternion sphere. /// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio). inline Quatf uniformRandomQuaternion() { // Rotational part - const FCL_REAL u1 = (FCL_REAL)rand() / RAND_MAX; - const FCL_REAL u2 = (FCL_REAL)rand() / RAND_MAX; - const FCL_REAL u3 = (FCL_REAL)rand() / RAND_MAX; - - const FCL_REAL mult1 = std::sqrt(FCL_REAL(1.0) - u1); - const FCL_REAL mult2 = std::sqrt(u1); - - static const FCL_REAL PI_value = static_cast(EIGEN_PI); - FCL_REAL s2 = std::sin(2 * PI_value * u2); - FCL_REAL c2 = std::cos(2 * PI_value * u2); - FCL_REAL s3 = std::sin(2 * PI_value * u3); - FCL_REAL c3 = std::cos(2 * PI_value * u3); + const CoalScalar u1 = (CoalScalar)rand() / RAND_MAX; + const CoalScalar u2 = (CoalScalar)rand() / RAND_MAX; + const CoalScalar u3 = (CoalScalar)rand() / RAND_MAX; + + const CoalScalar mult1 = std::sqrt(CoalScalar(1.0) - u1); + const CoalScalar mult2 = std::sqrt(u1); + + static const CoalScalar PI_value = static_cast(EIGEN_PI); + CoalScalar s2 = std::sin(2 * PI_value * u2); + CoalScalar c2 = std::cos(2 * PI_value * u2); + CoalScalar s3 = std::sin(2 * PI_value * u3); + CoalScalar c3 = std::cos(2 * PI_value * u3); Quatf q; q.w() = mult1 * s2; diff --git a/include/coal/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h index f72292b2c..ae92c05e7 100644 --- a/include/coal/narrowphase/gjk.h +++ b/include/coal/narrowphase/gjk.h @@ -101,7 +101,7 @@ struct COAL_DLLAPI GJK { }; public: - FCL_REAL distance_upper_bound; + CoalScalar distance_upper_bound; Status status; GJKVariant gjk_variant; GJKConvergenceCriterion convergence_criterion; @@ -115,14 +115,14 @@ struct COAL_DLLAPI GJK { /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than /// infinity, GJK will early stop as soon as it finds `distance` to be greater /// than `distance_upper_bound`. - FCL_REAL distance; + CoalScalar distance; Simplex* simplex; // Pointer to the result of the last run of GJK. private: // max_iteration and tolerance are made private // because they are meant to be set by the `reset` function. size_t max_iterations; - FCL_REAL tolerance; + CoalScalar tolerance; SimplexV store_v[4]; SimplexV* free_v[4]; @@ -140,7 +140,7 @@ struct COAL_DLLAPI GJK { /// with some vertices closer than this threshold. /// /// Suggested values are 100 iterations and a tolerance of 1e-6. - GJK(size_t max_iterations_, FCL_REAL tolerance_) + GJK(size_t max_iterations_, CoalScalar tolerance_) : max_iterations(max_iterations_), tolerance(tolerance_) { COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", std::invalid_argument); @@ -150,7 +150,7 @@ struct COAL_DLLAPI GJK { /// @brief resets the GJK algorithm, preparing it for a new run. /// Other than the maximum number of iterations and the tolerance, /// this function does **not** modify the parameters of the GJK algorithm. - void reset(size_t max_iterations_, FCL_REAL tolerance_); + void reset(size_t max_iterations_, CoalScalar tolerance_); /// @brief GJK algorithm, given the initial value guess Status evaluate( @@ -191,20 +191,20 @@ struct COAL_DLLAPI GJK { /// GJK stops when it proved the distance is more than this threshold. /// @note The closest points will be erroneous in this case. /// If you want the closest points, set this to infinity (the default). - void setDistanceEarlyBreak(const FCL_REAL& dup) { + void setDistanceEarlyBreak(const CoalScalar& dup) { distance_upper_bound = dup; } /// @brief Convergence check used to stop GJK when shapes are not in /// collision. - bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega) const; + bool checkConvergence(const Vec3f& w, const CoalScalar& rl, CoalScalar& alpha, + const CoalScalar& omega) const; /// @brief Get the max number of iterations of GJK. size_t getNumMaxIterations() const { return max_iterations; } /// @brief Get the tolerance of GJK. - FCL_REAL getTolerance() const { return tolerance; } + CoalScalar getTolerance() const { return tolerance; } /// @brief Get the number of iterations of the last run of GJK. size_t getNumIterations() const { return iterations; } @@ -259,7 +259,7 @@ struct COAL_DLLAPI EPA { typedef GJK::SimplexV SimplexVertex; struct COAL_DLLAPI SimplexFace { Vec3f n; - FCL_REAL d; + CoalScalar d; bool ignore; // If the origin does not project inside the face, we // ignore this face. size_t vertex_id[3]; // Index of vertex in sv_store. @@ -344,14 +344,14 @@ struct COAL_DLLAPI EPA { GJK::Simplex result; Vec3f normal; support_func_guess_t support_hint; - FCL_REAL depth; + CoalScalar depth; SimplexFace* closest_face; private: // max_iteration and tolerance are made private // because they are meant to be set by the `reset` function. size_t max_iterations; - FCL_REAL tolerance; + CoalScalar tolerance; std::vector sv_store; std::vector fc_store; @@ -360,7 +360,7 @@ struct COAL_DLLAPI EPA { size_t iterations; public: - EPA(size_t max_iterations_, FCL_REAL tolerance_) + EPA(size_t max_iterations_, CoalScalar tolerance_) : max_iterations(max_iterations_), tolerance(tolerance_) { initialize(); } @@ -385,7 +385,7 @@ struct COAL_DLLAPI EPA { size_t getNumMaxFaces() const { return fc_store.size(); } /// @brief Get the tolerance of EPA. - FCL_REAL getTolerance() const { return tolerance; } + CoalScalar getTolerance() const { return tolerance; } /// @brief Get the number of iterations of the last run of EPA. size_t getNumIterations() const { return iterations; } @@ -404,7 +404,7 @@ struct COAL_DLLAPI EPA { /// @note calling this function destroys the previous state of EPA. /// In the future, we may want to copy it instead, i.e. when EPA will /// be (properly) warm-startable. - void reset(size_t max_iterations, FCL_REAL tolerance); + void reset(size_t max_iterations, CoalScalar tolerance); /// \return a Status which can be demangled using (status & Valid) or /// (status & Failed). The other values provide a more detailled @@ -428,7 +428,7 @@ struct COAL_DLLAPI EPA { void initialize(); bool getEdgeDist(SimplexFace* face, const SimplexVertex& a, - const SimplexVertex& b, FCL_REAL& dist); + const SimplexVertex& b, CoalScalar& dist); /// @brief Add a new face to the polytope. /// This function sets the `ignore` flag to `true` if the origin does not diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h index 677373e56..c0af94243 100644 --- a/include/coal/narrowphase/minkowski_difference.h +++ b/include/coal/narrowphase/minkowski_difference.h @@ -51,7 +51,7 @@ namespace details { /// /// @note The Minkowski difference is expressed in the frame of the first shape. struct COAL_DLLAPI MinkowskiDiff { - typedef Eigen::Array Array2d; + typedef Eigen::Array Array2d; /// @brief points to two shapes const ShapeBase* shapes[2]; diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h index 90bc04b11..261917ccf 100644 --- a/include/coal/narrowphase/narrowphase.h +++ b/include/coal/narrowphase/narrowphase.h @@ -65,7 +65,7 @@ struct COAL_DLLAPI GJKSolver { size_t gjk_max_iterations; /// @brief tolerance of GJK - FCL_REAL gjk_tolerance; + CoalScalar gjk_tolerance; /// @brief which warm start to use for GJK GJKInitialGuess gjk_initial_guess; @@ -83,7 +83,7 @@ struct COAL_DLLAPI GJKSolver { /// @brief If GJK can guarantee that the distance between the shapes is /// greater than this value, it will early stop. - FCL_REAL distance_upper_bound; + CoalScalar distance_upper_bound; /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak). GJKVariant gjk_variant; @@ -101,14 +101,14 @@ struct COAL_DLLAPI GJKSolver { size_t epa_max_iterations; /// @brief tolerance of EPA - FCL_REAL epa_tolerance; + CoalScalar epa_tolerance; /// @brief Minkowski difference used by GJK and EPA algorithms mutable details::MinkowskiDiff minkowski_difference; private: // Used internally for assertion checks. - static constexpr FCL_REAL m_dummy_precision = 1e-6; + static constexpr CoalScalar m_dummy_precision = 1e-6; public: COAL_COMPILER_DIAGNOSTIC_PUSH @@ -128,7 +128,7 @@ struct COAL_DLLAPI GJKSolver { enable_cached_guess(false), // Use gjk_initial_guess instead cached_guess(Vec3f(1, 0, 0)), support_func_cached_guess(support_func_guess_t::Zero()), - distance_upper_bound((std::numeric_limits::max)()), + distance_upper_bound((std::numeric_limits::max)()), gjk_variant(GJKVariant::DefaultGJK), gjk_convergence_criterion(GJKConvergenceCriterion::Default), gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute), @@ -171,7 +171,7 @@ struct COAL_DLLAPI GJKSolver { this->gjk_max_iterations = request.gjk_max_iterations; this->gjk_tolerance = request.gjk_tolerance; // For distance computation, we don't want GJK to early stop - this->distance_upper_bound = (std::numeric_limits::max)(); + this->distance_upper_bound = (std::numeric_limits::max)(); this->gjk_variant = request.gjk_variant; this->gjk_convergence_criterion = request.gjk_convergence_criterion; this->gjk_convergence_criterion_type = @@ -269,7 +269,7 @@ struct COAL_DLLAPI GJKSolver { /// @brief Helper to return the precision of the solver on the distance /// estimate, depending on whether or not `compute_penetration` is true. - FCL_REAL getDistancePrecision(const bool compute_penetration) const { + CoalScalar getDistancePrecision(const bool compute_penetration) const { return compute_penetration ? (std::max)(this->gjk_tolerance, this->epa_tolerance) : this->gjk_tolerance; @@ -305,11 +305,12 @@ struct COAL_DLLAPI GJKSolver { /// It's up to the user to decide whether the shapes are in collision or not, /// based on that estimate. template - FCL_REAL shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, const bool compute_penetration, - Vec3f& p1, Vec3f& p2, Vec3f& normal) const { + CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, + const Transform3f& tf2, + const bool compute_penetration, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { constexpr bool relative_transformation_already_computed = false; - FCL_REAL distance; + CoalScalar distance; this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal, relative_transformation_already_computed); return distance; @@ -319,16 +320,16 @@ struct COAL_DLLAPI GJKSolver { /// second shape is a triangle. It is more efficient to pre-compute the /// relative transformation between the two shapes before calling GJK/EPA. template - FCL_REAL shapeDistance(const S1& s1, const Transform3f& tf1, - const TriangleP& s2, const Transform3f& tf2, - const bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { + CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1, + const TriangleP& s2, const Transform3f& tf2, + const bool compute_penetration, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { const Transform3f tf_1M2(tf1.inverseTimes(tf2)); TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b), tf_1M2.transform(s2.c)); constexpr bool relative_transformation_already_computed = true; - FCL_REAL distance; + CoalScalar distance; this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1, p2, normal, relative_transformation_already_computed); return distance; @@ -336,11 +337,11 @@ struct COAL_DLLAPI GJKSolver { /// @brief See other partial template specialization of shapeDistance above. template - FCL_REAL shapeDistance(const TriangleP& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, - const bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { - FCL_REAL distance = this->shapeDistance( + CoalScalar shapeDistance(const TriangleP& s1, const Transform3f& tf1, + const S2& s2, const Transform3f& tf2, + const bool compute_penetration, Vec3f& p1, Vec3f& p2, + Vec3f& normal) const { + CoalScalar distance = this->shapeDistance( s2, tf2, s1, tf1, compute_penetration, p2, p1, normal); normal = -normal; return distance; @@ -421,7 +422,7 @@ struct COAL_DLLAPI GJKSolver { void runGJKAndEPA( const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, const bool compute_penetration, - FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal, + CoalScalar& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal, const bool relative_transformation_already_computed = false) const { // Reset internal state of GJK algorithm if (relative_transformation_already_computed) @@ -450,9 +451,9 @@ struct COAL_DLLAPI GJKSolver { std::logic_error); this->cached_guess = Vec3f(1, 0, 0); this->support_func_cached_guess.setZero(); - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); break; case details::GJK::Failed: // @@ -586,8 +587,8 @@ struct COAL_DLLAPI GJKSolver { } void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, - Vec3f& p2, + CoalScalar& distance, + Vec3f& p1, Vec3f& p2, Vec3f& normal) const { COAL_UNUSED_VARIABLE(tf1); // Cache gjk result for potential future call to this GJKSolver. @@ -596,7 +597,7 @@ struct COAL_DLLAPI GJKSolver { distance = this->gjk.distance; p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); // If we absolutely want to return some witness points, we could use // the following code (or simply merge the early stopped case with the // valid case below): @@ -607,7 +608,7 @@ struct COAL_DLLAPI GJKSolver { } void GJKExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, + CoalScalar& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { // Apart from early stopping, there are two cases where GJK says there is no // collision: @@ -634,8 +635,8 @@ struct COAL_DLLAPI GJKSolver { } void GJKCollisionExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, - Vec3f& p2, + CoalScalar& distance, + Vec3f& p1, Vec3f& p2, Vec3f& normal) const { COAL_UNUSED_VARIABLE(tf1); COAL_ASSERT(this->gjk.distance <= @@ -650,11 +651,11 @@ struct COAL_DLLAPI GJKSolver { distance = this->gjk.distance; p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } void EPAExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, + CoalScalar& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { // Cache EPA result for potential future call to this GJKSolver. // This caching allows to warm-start the next GJK call. @@ -709,15 +710,15 @@ struct COAL_DLLAPI GJKSolver { } void EPAFailedExtractWitnessPointsAndNormal(const Transform3f& tf1, - FCL_REAL& distance, Vec3f& p1, + CoalScalar& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { this->cached_guess = Vec3f(1, 0, 0); this->support_func_cached_guess.setZero(); COAL_UNUSED_VARIABLE(tf1); - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } }; diff --git a/include/coal/narrowphase/narrowphase_defaults.h b/include/coal/narrowphase/narrowphase_defaults.h index 54ce75328..1fd2775a8 100644 --- a/include/coal/narrowphase/narrowphase_defaults.h +++ b/include/coal/narrowphase/narrowphase_defaults.h @@ -44,21 +44,21 @@ namespace coal { /// GJK constexpr size_t GJK_DEFAULT_MAX_ITERATIONS = 128; -constexpr FCL_REAL GJK_DEFAULT_TOLERANCE = 1e-6; +constexpr CoalScalar GJK_DEFAULT_TOLERANCE = 1e-6; /// Note: if the considered shapes are on the order of the meter, and the /// convergence criterion of GJK is the default VDB criterion, /// setting a tolerance of 1e-6 on the GJK algorithm makes it precise up to /// the micro-meter. /// The same is true for EPA. -constexpr FCL_REAL GJK_MINIMUM_TOLERANCE = 1e-6; +constexpr CoalScalar GJK_MINIMUM_TOLERANCE = 1e-6; /// EPA /// EPA build a polytope which maximum size is: /// - `#iterations + 4` vertices /// - `2 x #iterations + 4` faces constexpr size_t EPA_DEFAULT_MAX_ITERATIONS = 64; -constexpr FCL_REAL EPA_DEFAULT_TOLERANCE = 1e-6; -constexpr FCL_REAL EPA_MINIMUM_TOLERANCE = 1e-6; +constexpr CoalScalar EPA_DEFAULT_TOLERANCE = 1e-6; +constexpr CoalScalar EPA_MINIMUM_TOLERANCE = 1e-6; } // namespace coal diff --git a/include/coal/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h index 5bc3facba..8dac64d41 100644 --- a/include/coal/narrowphase/support_functions.h +++ b/include/coal/narrowphase/support_functions.h @@ -193,9 +193,9 @@ void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, /// sphere radii. template void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3); -/// @brief Same as @ref getSupportSet(const ShapeBase*, const FCL_REAL, +/// @brief Same as @ref getSupportSet(const ShapeBase*, const CoalScalar, /// SupportSet&, const int) but also constructs the support set frame from /// `dir`. /// @note The support direction `dir` is expressed in the local frame of the @@ -205,7 +205,7 @@ void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, template void getSupportSet(const ShapeBase* shape, const Vec3f& dir, SupportSet& support_set, int& hint, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3) { + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3) { support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir); const Vec3f& support_dir = support_set.getNormal(); const Vec3f support = getSupport<_SupportOptions>(shape, support_dir, hint); @@ -219,7 +219,7 @@ template void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); + CoalScalar tol = 1e-3); /// @brief Box support set function. /// Assumes the support set frame has already been computed. @@ -227,7 +227,7 @@ template void getShapeSupportSet(const Box* box, SupportSet& support_set, int& /*unused*/, ShapeSupportData& support_data, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); + CoalScalar tol = 1e-3); /// @brief Sphere support set function. /// Assumes the support set frame has already been computed. @@ -235,7 +235,7 @@ template void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL /*unused*/ tol = 1e-3); + CoalScalar /*unused*/ tol = 1e-3); /// @brief Ellipsoid support set function. /// Assumes the support set frame has already been computed. @@ -243,7 +243,7 @@ template void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL /*unused*/ tol = 1e-3); + CoalScalar /*unused*/ tol = 1e-3); /// @brief Capsule support set function. /// Assumes the support set frame has already been computed. @@ -251,21 +251,21 @@ template void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); + CoalScalar tol = 1e-3); /// @brief Cone support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Cone* cone, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3); /// @brief Cylinder support set function. /// Assumes the support set frame has already been computed. template void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, - size_t num_sampled_supports = 6, FCL_REAL tol = 1e-3); + size_t num_sampled_supports = 6, CoalScalar tol = 1e-3); /// @brief ConvexBase support set function. /// Assumes the support set frame has already been computed. @@ -275,7 +275,7 @@ template void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); + CoalScalar tol = 1e-3); /// @brief Support set function for large ConvexBase (>32 vertices). /// Assumes the support set frame has already been computed. @@ -283,7 +283,7 @@ template void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, int& /*unused*/, ShapeSupportData& /*unused*/, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); + CoalScalar tol = 1e-3); /// @brief Support set function for small ConvexBase (<32 vertices). /// Assumes the support set frame has already been computed. @@ -291,7 +291,7 @@ template void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t /*unused*/ num_sampled_supports = 6, - FCL_REAL tol = 1e-3); + CoalScalar tol = 1e-3); /// @brief Computes the convex-hull of support_set. For now, this function is /// only needed for Box and ConvexBase. diff --git a/include/coal/octree.h b/include/coal/octree.h index c3635c643..7dee68df0 100644 --- a/include/coal/octree.h +++ b/include/coal/octree.h @@ -54,16 +54,16 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { protected: shared_ptr tree; - FCL_REAL default_occupancy; + CoalScalar default_occupancy; - FCL_REAL occupancy_threshold; - FCL_REAL free_threshold; + CoalScalar occupancy_threshold; + CoalScalar free_threshold; public: typedef octomap::OcTreeNode OcTreeNode; /// @brief construct octree with a given resolution - explicit OcTree(FCL_REAL resolution) + explicit OcTree(CoalScalar resolution) : tree(shared_ptr( new octomap::OcTree(resolution))) { default_occupancy = tree->getOccupancyThres(); @@ -125,18 +125,19 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { } // Account for the size of the boxes. - const FCL_REAL resolution = tree->getResolution(); + const CoalScalar resolution = tree->getResolution(); max_extent.array() += float(resolution / 2.); min_extent.array() -= float(resolution / 2.); - aabb_local = AABB(min_extent.cast(), max_extent.cast()); + aabb_local = + AABB(min_extent.cast(), max_extent.cast()); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); } /// @brief get the bounding volume for the root AABB getRootBV() const { - FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; + CoalScalar delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; // std::cout << "octree size " << delta << std::endl; return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)); @@ -149,7 +150,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { unsigned long size() const { return tree->size(); } /// @brief Returns the resolution of the octree - FCL_REAL getResolution() const { return tree->getResolution(); } + CoalScalar getResolution() const { return tree->getResolution(); } /// @brief get the root node of the octree OcTreeNode* getRoot() const { return tree->getRoot(); } @@ -183,12 +184,12 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { it != end; ++it) { // if(tree->isNodeOccupied(*it)) if (isNodeOccupied(&*it)) { - FCL_REAL x = it.getX(); - FCL_REAL y = it.getY(); - FCL_REAL z = it.getZ(); - FCL_REAL size = it.getSize(); - FCL_REAL c = (*it).getOccupancy(); - FCL_REAL t = tree->getOccupancyThres(); + CoalScalar x = it.getX(); + CoalScalar y = it.getY(); + CoalScalar z = it.getZ(); + CoalScalar size = it.getSize(); + CoalScalar c = (*it).getOccupancy(); + CoalScalar t = tree->getOccupancyThres(); Vec6f box; box << x, y, z, size, c, t; @@ -201,7 +202,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { /// \brief Returns a byte description of *this std::vector tobytes() const { typedef Eigen::Matrix Vec3float; - const size_t total_size = (tree->size() * sizeof(FCL_REAL) * 3) / 2; + const size_t total_size = (tree->size() * sizeof(CoalScalar) * 3) / 2; std::vector bytes; bytes.reserve(total_size); @@ -210,9 +211,9 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { end = tree->end(); it != end; ++it) { const Vec3f box_pos = - Eigen::Map(&it.getCoordinate().x()).cast(); + Eigen::Map(&it.getCoordinate().x()).cast(); if (isNodeOccupied(&*it)) - std::copy(box_pos.data(), box_pos.data() + sizeof(FCL_REAL) * 3, + std::copy(box_pos.data(), box_pos.data() + sizeof(CoalScalar) * 3, std::back_inserter(bytes)); } @@ -221,19 +222,19 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { /// @brief the threshold used to decide whether one node is occupied, this is /// NOT the octree occupied_thresold - FCL_REAL getOccupancyThres() const { return occupancy_threshold; } + CoalScalar getOccupancyThres() const { return occupancy_threshold; } /// @brief the threshold used to decide whether one node is free, this is NOT /// the octree free_threshold - FCL_REAL getFreeThres() const { return free_threshold; } + CoalScalar getFreeThres() const { return free_threshold; } - FCL_REAL getDefaultOccupancy() const { return default_occupancy; } + CoalScalar getDefaultOccupancy() const { return default_occupancy; } - void setCellDefaultOccupancy(FCL_REAL d) { default_occupancy = d; } + void setCellDefaultOccupancy(CoalScalar d) { default_occupancy = d; } - void setOccupancyThres(FCL_REAL d) { occupancy_threshold = d; } + void setOccupancyThres(CoalScalar d) { occupancy_threshold = d; } - void setFreeThres(FCL_REAL d) { free_threshold = d; } + void setFreeThres(CoalScalar d) { free_threshold = d; } /// @return ptr to child number childIdx of node OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { @@ -331,8 +332,8 @@ static inline void computeChildBV(const AABB& root_bv, unsigned int i, /// \returns An OcTree that can be used for collision checking and more. /// COAL_DLLAPI OcTreePtr_t -makeOctree(const Eigen::Matrix& point_cloud, - const FCL_REAL resolution); +makeOctree(const Eigen::Matrix& point_cloud, + const CoalScalar resolution); } // namespace coal diff --git a/include/coal/serialization/contact_patch.h b/include/coal/serialization/contact_patch.h index cdad60478..aae049daa 100644 --- a/include/coal/serialization/contact_patch.h +++ b/include/coal/serialization/contact_patch.h @@ -16,7 +16,7 @@ template void serialize(Archive& ar, coal::ContactPatch& contact_patch, const unsigned int /*version*/) { using namespace coal; - typedef Eigen::Matrix PolygonPoints; + typedef Eigen::Matrix PolygonPoints; size_t patch_size = contact_patch.size(); ar& make_nvp("patch_size", patch_size); @@ -25,7 +25,7 @@ void serialize(Archive& ar, coal::ContactPatch& contact_patch, contact_patch.points().resize(patch_size); } Eigen::Map points_map( - reinterpret_cast(contact_patch.points().data()), 2, + reinterpret_cast(contact_patch.points().data()), 2, static_cast(patch_size)); ar& make_nvp("points", points_map); } @@ -43,7 +43,7 @@ void serialize(Archive& ar, coal::ContactPatchRequest& request, ar& make_nvp("max_num_patch", request.max_num_patch); size_t num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); - FCL_REAL patch_tolerance = request.getPatchTolerance(); + CoalScalar patch_tolerance = request.getPatchTolerance(); ar& make_nvp("num_samples_curved_shapes", num_samples_curved_shapes); ar& make_nvp("patch_tolerance", num_samples_curved_shapes); diff --git a/include/coal/serialization/convex.h b/include/coal/serialization/convex.h index 2aefe25fc..52999cee0 100644 --- a/include/coal/serialization/convex.h +++ b/include/coal/serialization/convex.h @@ -61,7 +61,7 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base, convex_base.normals.reset( new std::vector(convex_base.num_normals_and_offsets)); convex_base.offsets.reset( - new std::vector(convex_base.num_normals_and_offsets)); + new std::vector(convex_base.num_normals_and_offsets)); } } @@ -73,23 +73,23 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base, } } - typedef Eigen::Matrix MatrixPoints; + typedef Eigen::Matrix MatrixPoints; if (convex_base.num_points > 0) { Eigen::Map points_map( - reinterpret_cast(convex_base.points->data()), 3, + reinterpret_cast(convex_base.points->data()), 3, convex_base.num_points); ar& make_nvp("points", points_map); } - typedef Eigen::Matrix VecOfReals; + typedef Eigen::Matrix VecOfReals; if (convex_base.num_normals_and_offsets > 0) { Eigen::Map normals_map( - reinterpret_cast(convex_base.normals->data()), 3, + reinterpret_cast(convex_base.normals->data()), 3, convex_base.num_normals_and_offsets); ar& make_nvp("normals", normals_map); Eigen::Map offsets_map( - reinterpret_cast(convex_base.offsets->data()), 1, + reinterpret_cast(convex_base.offsets->data()), 1, convex_base.num_normals_and_offsets); ar& make_nvp("offsets", offsets_map); } @@ -97,7 +97,7 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base, typedef Eigen::Matrix VecOfInts; if (num_warm_start_supports > 0) { Eigen::Map warm_start_support_points_map( - reinterpret_cast( + reinterpret_cast( convex_base.support_warm_starts.points.data()), 3, num_warm_start_supports); ar& make_nvp("warm_start_support_points", warm_start_support_points_map); diff --git a/include/coal/serialization/geometric_shapes.h b/include/coal/serialization/geometric_shapes.h index 661bfa534..2a14614c2 100644 --- a/include/coal/serialization/geometric_shapes.h +++ b/include/coal/serialization/geometric_shapes.h @@ -18,7 +18,7 @@ void serialize(Archive& ar, coal::ShapeBase& shape_base, ar& make_nvp( "base", boost::serialization::base_object(shape_base)); - ::coal::FCL_REAL radius = shape_base.getSweptSphereRadius(); + ::coal::CoalScalar radius = shape_base.getSweptSphereRadius(); ar& make_nvp("swept_sphere_radius", radius); if (Archive::is_loading::value) { diff --git a/include/coal/serialization/kIOS.h b/include/coal/serialization/kIOS.h index 2a024e056..14c6457fc 100644 --- a/include/coal/serialization/kIOS.h +++ b/include/coal/serialization/kIOS.h @@ -24,7 +24,7 @@ void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) { ar& make_nvp("num_spheres", bv.num_spheres); std::array centers{}; - std::array radii; + std::array radii; for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { centers[i] = bv.spheres[i].o; radii[i] = bv.spheres[i].r; @@ -40,7 +40,7 @@ void load(Archive& ar, coal::kIOS& bv, const unsigned int /*version*/) { ar >> make_nvp("num_spheres", bv.num_spheres); std::array centers; - std::array radii; + std::array radii; ar >> make_nvp("centers", make_array(centers.data(), centers.size())); ar >> make_nvp("radii", make_array(radii.data(), radii.size())); for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { diff --git a/include/coal/shape/convex.h b/include/coal/shape/convex.h index 067160ad6..916fdfda1 100644 --- a/include/coal/shape/convex.h +++ b/include/coal/shape/convex.h @@ -75,7 +75,7 @@ class Convex : public ConvexBase { Vec3f computeCOM() const; - FCL_REAL computeVolume() const; + CoalScalar computeVolume() const; /// /// @brief Set the current Convex from a list of points and polygons. diff --git a/include/coal/shape/details/convex.hxx b/include/coal/shape/details/convex.hxx index e77a6a094..566af0230 100644 --- a/include/coal/shape/details/convex.hxx +++ b/include/coal/shape/details/convex.hxx @@ -144,7 +144,7 @@ Vec3f Convex::computeCOM() const { typedef typename PolygonT::index_type index_type; Vec3f com(0, 0, 0); - FCL_REAL vol = 0; + CoalScalar vol = 0; if (!(points.get())) { std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices." << std::endl; @@ -174,7 +174,7 @@ Vec3f Convex::computeCOM() const { polygon[static_cast((j + 1) % polygon.size())]; const Vec3f& v1 = points_[e_first]; const Vec3f& v2 = points_[e_second]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); + CoalScalar d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol; } @@ -184,11 +184,11 @@ Vec3f Convex::computeCOM() const { } template -FCL_REAL Convex::computeVolume() const { +CoalScalar Convex::computeVolume() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::index_type index_type; - FCL_REAL vol = 0; + CoalScalar vol = 0; if (!(points.get())) { std::cerr << "Error in `Convex::computeVolume`! Convex has no vertices." << std::endl; @@ -219,7 +219,7 @@ FCL_REAL Convex::computeVolume() const { polygon[static_cast((j + 1) % polygon.size())]; const Vec3f& v1 = points_[e_first]; const Vec3f& v2 = points_[e_second]; - FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); + CoalScalar d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; } } diff --git a/include/coal/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h index 0dd176265..48516cd79 100644 --- a/include/coal/shape/geometric_shape_to_BVH_model.h +++ b/include/coal/shape/geometric_shape_to_BVH_model.h @@ -48,9 +48,9 @@ namespace coal { template void generateBVHModel(BVHModel& model, const Box& shape, const Transform3f& pose) { - FCL_REAL a = shape.halfSide[0]; - FCL_REAL b = shape.halfSide[1]; - FCL_REAL c = shape.halfSide[2]; + CoalScalar a = shape.halfSide[0]; + CoalScalar b = shape.halfSide[1]; + CoalScalar c = shape.halfSide[2]; std::vector points(8); std::vector tri_indices(12); points[0] = Vec3f(a, -b, c); @@ -94,18 +94,18 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, std::vector points; std::vector tri_indices; - FCL_REAL r = shape.radius; - FCL_REAL phi, phid; - const FCL_REAL pi = boost::math::constants::pi(); + CoalScalar r = shape.radius; + CoalScalar phi, phid; + const CoalScalar pi = boost::math::constants::pi(); phid = pi * 2 / seg; phi = 0; - FCL_REAL theta, thetad; + CoalScalar theta, thetad; thetad = pi / (ring + 1); theta = 0; for (unsigned int i = 0; i < ring; ++i) { - FCL_REAL theta_ = theta + thetad * (i + 1); + CoalScalar theta_ = theta + thetad * (i + 1); for (unsigned int j = 0; j < seg; ++j) { points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), @@ -157,9 +157,9 @@ template void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere) { - FCL_REAL r = shape.radius; - FCL_REAL n_low_bound = - std::sqrt((FCL_REAL)n_faces_for_unit_sphere / FCL_REAL(2.)) * r * r; + CoalScalar r = shape.radius; + CoalScalar n_low_bound = + std::sqrt((CoalScalar)n_faces_for_unit_sphere / CoalScalar(2.)) * r * r; unsigned int ring = (unsigned int)ceil(n_low_bound); unsigned int seg = (unsigned int)ceil(n_low_bound); @@ -175,14 +175,14 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, std::vector points; std::vector tri_indices; - FCL_REAL r = shape.radius; - FCL_REAL h = shape.halfLength; - FCL_REAL phi, phid; - const FCL_REAL pi = boost::math::constants::pi(); + CoalScalar r = shape.radius; + CoalScalar h = shape.halfLength; + CoalScalar phi, phid; + const CoalScalar pi = boost::math::constants::pi(); phid = pi * 2 / tot; phi = 0; - FCL_REAL hd = 2 * h / h_num; + CoalScalar hd = 2 * h / h_num; for (unsigned int i = 0; i < tot; ++i) points.push_back( @@ -245,14 +245,14 @@ template void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder) { - FCL_REAL r = shape.radius; - FCL_REAL h = 2 * shape.halfLength; + CoalScalar r = shape.radius; + CoalScalar h = 2 * shape.halfLength; - const FCL_REAL pi = boost::math::constants::pi(); + const CoalScalar pi = boost::math::constants::pi(); unsigned int tot = (unsigned int)(tot_for_unit_cylinder * r); - FCL_REAL phid = pi * 2 / tot; + CoalScalar phid = pi * 2 / tot; - FCL_REAL circle_edge = phid * r; + CoalScalar circle_edge = phid * r; unsigned int h_num = (unsigned int)ceil(h / circle_edge); generateBVHModel(model, shape, pose, tot, h_num); @@ -267,19 +267,19 @@ void generateBVHModel(BVHModel& model, const Cone& shape, std::vector points; std::vector tri_indices; - FCL_REAL r = shape.radius; - FCL_REAL h = shape.halfLength; + CoalScalar r = shape.radius; + CoalScalar h = shape.halfLength; - FCL_REAL phi, phid; - const FCL_REAL pi = boost::math::constants::pi(); + CoalScalar phi, phid; + const CoalScalar pi = boost::math::constants::pi(); phid = pi * 2 / tot; phi = 0; - FCL_REAL hd = 2 * h / h_num; + CoalScalar hd = 2 * h / h_num; for (unsigned int i = 0; i < h_num - 1; ++i) { - FCL_REAL h_i = h - (i + 1) * hd; - FCL_REAL rh = r * (0.5 - h_i / h / 2); + CoalScalar h_i = h - (i + 1) * hd; + CoalScalar rh = r * (0.5 - h_i / h / 2); for (unsigned int j = 0; j < tot; ++j) { points.push_back( Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); @@ -336,14 +336,14 @@ void generateBVHModel(BVHModel& model, const Cone& shape, template void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone) { - FCL_REAL r = shape.radius; - FCL_REAL h = 2 * shape.halfLength; + CoalScalar r = shape.radius; + CoalScalar h = 2 * shape.halfLength; - const FCL_REAL pi = boost::math::constants::pi(); + const CoalScalar pi = boost::math::constants::pi(); unsigned int tot = (unsigned int)(tot_for_unit_cone * r); - FCL_REAL phid = pi * 2 / tot; + CoalScalar phid = pi * 2 / tot; - FCL_REAL circle_edge = phid * r; + CoalScalar circle_edge = phid * r; unsigned int h_num = (unsigned int)ceil(h / circle_edge); generateBVHModel(model, shape, pose, tot, h_num); diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h index 9d6a62f2e..d21d5c214 100644 --- a/include/coal/shape/geometric_shapes.h +++ b/include/coal/shape/geometric_shapes.h @@ -73,7 +73,7 @@ class COAL_DLLAPI ShapeBase : public CollisionGeometry { /// @brief Set radius of sphere swept around the shape. /// Must be >= 0. - void setSweptSphereRadius(FCL_REAL radius) { + void setSweptSphereRadius(CoalScalar radius) { if (radius < 0) { COAL_THROW_PRETTY("Swept-sphere radius must be positive.", std::invalid_argument); @@ -83,7 +83,9 @@ class COAL_DLLAPI ShapeBase : public CollisionGeometry { /// @brief Get radius of sphere swept around the shape. /// This radius is always >= 0. - FCL_REAL getSweptSphereRadius() const { return this->m_swept_sphere_radius; } + CoalScalar getSweptSphereRadius() const { + return this->m_swept_sphere_radius; + } protected: /// \brief Radius of the sphere swept around the shape. @@ -97,7 +99,7 @@ class COAL_DLLAPI ShapeBase : public CollisionGeometry { /// which rounds the sharp corners of a shape. /// The swept sphere radius is a property of the shape itself and can be /// manually updated between collision checks. - FCL_REAL m_swept_sphere_radius{0}; + CoalScalar m_swept_sphere_radius{0}; }; /// @defgroup Geometric_Shapes Geometric shapes @@ -123,7 +125,8 @@ class COAL_DLLAPI TriangleP : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } - // std::pair inflated(const FCL_REAL value) const { + // std::pair inflated(const CoalScalar value) const + // { // if (value == 0) return std::make_pair(new TriangleP(*this), // Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize(); // BC.normalize(); @@ -137,9 +140,9 @@ class COAL_DLLAPI TriangleP : public ShapeBase { // Transform3f()); // } // - // FCL_REAL minInflationValue() const + // CoalScalar minInflationValue() const // { - // return (std::numeric_limits::max)(); // TODO(jcarpent): + // return (std::numeric_limits::max)(); // TODO(jcarpent): // implement // } @@ -162,7 +165,7 @@ class COAL_DLLAPI TriangleP : public ShapeBase { /// @brief Center at zero point, axis aligned box class COAL_DLLAPI Box : public ShapeBase { public: - Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) + Box(CoalScalar x, CoalScalar y, CoalScalar z) : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {} Box(const Vec3f& side_) : ShapeBase(), halfSide(side_ / 2) {} @@ -191,15 +194,15 @@ class COAL_DLLAPI Box : public ShapeBase { /// @brief Get node type: a box NODE_TYPE getNodeType() const { return GEOM_BOX; } - FCL_REAL computeVolume() const { return 8 * halfSide.prod(); } + CoalScalar computeVolume() const { return 8 * halfSide.prod(); } Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); + CoalScalar V = computeVolume(); Vec3f s(halfSide.cwiseAbs2() * V); return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); } - FCL_REAL minInflationValue() const { return -halfSide.minCoeff(); } + CoalScalar minInflationValue() const { return -halfSide.minCoeff(); } /// \brief Inflate the box by an amount given by `value`. /// This value can be positive or negative but must always >= @@ -209,7 +212,7 @@ class COAL_DLLAPI Box : public ShapeBase { /// /// \returns a new inflated box and the related transform to account for the /// change of shape frame - std::pair inflated(const FCL_REAL value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") " << "is two small. It should be at least: " @@ -239,7 +242,7 @@ class COAL_DLLAPI Sphere : public ShapeBase { /// @brief Default constructor Sphere() {} - explicit Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {} + explicit Sphere(CoalScalar radius_) : ShapeBase(), radius(radius_) {} Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {} @@ -247,7 +250,7 @@ class COAL_DLLAPI Sphere : public ShapeBase { virtual Sphere* clone() const { return new Sphere(*this); }; /// @brief Radius of the sphere - FCL_REAL radius; + CoalScalar radius; /// @brief Compute AABB void computeLocalAABB(); @@ -256,16 +259,16 @@ class COAL_DLLAPI Sphere : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_SPHERE; } Matrix3f computeMomentofInertia() const { - FCL_REAL I = 0.4 * radius * radius * computeVolume(); + CoalScalar I = 0.4 * radius * radius * computeVolume(); return I * Matrix3f::Identity(); } - FCL_REAL computeVolume() const { - return 4 * boost::math::constants::pi() * radius * radius * + CoalScalar computeVolume() const { + return 4 * boost::math::constants::pi() * radius * radius * radius / 3; } - FCL_REAL minInflationValue() const { return -radius; } + CoalScalar minInflationValue() const { return -radius; } /// \brief Inflate the sphere by an amount given by `value`. /// This value can be positive or negative but must always >= @@ -275,7 +278,7 @@ class COAL_DLLAPI Sphere : public ShapeBase { /// /// \returns a new inflated sphere and the related transform to account for /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " @@ -304,7 +307,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { /// @brief Default constructor Ellipsoid() {} - Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) + Ellipsoid(CoalScalar rx, CoalScalar ry, CoalScalar rz) : ShapeBase(), radii(rx, ry, rz) {} explicit Ellipsoid(const Vec3f& radii) : radii(radii) {} @@ -325,21 +328,21 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; } Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); - FCL_REAL a2 = V * radii[0] * radii[0]; - FCL_REAL b2 = V * radii[1] * radii[1]; - FCL_REAL c2 = V * radii[2] * radii[2]; + CoalScalar V = computeVolume(); + CoalScalar a2 = V * radii[0] * radii[0]; + CoalScalar b2 = V * radii[1] * radii[1]; + CoalScalar c2 = V * radii[2] * radii[2]; return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0, 0.2 * (a2 + b2)) .finished(); } - FCL_REAL computeVolume() const { - return 4 * boost::math::constants::pi() * radii[0] * radii[1] * + CoalScalar computeVolume() const { + return 4 * boost::math::constants::pi() * radii[0] * radii[1] * radii[2] / 3; } - FCL_REAL minInflationValue() const { return -radii.minCoeff(); } + CoalScalar minInflationValue() const { return -radii.minCoeff(); } /// \brief Inflate the ellipsoid by an amount given by `value`. /// This value can be positive or negative but must always >= @@ -349,7 +352,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { /// /// \returns a new inflated ellipsoid and the related transform to account for /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " @@ -382,7 +385,7 @@ class COAL_DLLAPI Capsule : public ShapeBase { /// @brief Default constructor Capsule() {} - Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { + Capsule(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } @@ -393,10 +396,10 @@ class COAL_DLLAPI Capsule : public ShapeBase { virtual Capsule* clone() const { return new Capsule(*this); }; /// @brief Radius of capsule - FCL_REAL radius; + CoalScalar radius; /// @brief Half Length along z axis - FCL_REAL halfLength; + CoalScalar halfLength; /// @brief Compute AABB void computeLocalAABB(); @@ -404,27 +407,27 @@ class COAL_DLLAPI Capsule : public ShapeBase { /// @brief Get node type: a capsule NODE_TYPE getNodeType() const { return GEOM_CAPSULE; } - FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * + CoalScalar computeVolume() const { + return boost::math::constants::pi() * radius * radius * ((halfLength * 2) + radius * 4 / 3.0); } Matrix3f computeMomentofInertia() const { - FCL_REAL v_cyl = radius * radius * (halfLength * 2) * - boost::math::constants::pi(); - FCL_REAL v_sph = radius * radius * radius * - boost::math::constants::pi() * 4 / 3.0; + CoalScalar v_cyl = radius * radius * (halfLength * 2) * + boost::math::constants::pi(); + CoalScalar v_sph = radius * radius * radius * + boost::math::constants::pi() * 4 / 3.0; - FCL_REAL h2 = halfLength * halfLength; - FCL_REAL r2 = radius * radius; - FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + - v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength); - FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; + CoalScalar h2 = halfLength * halfLength; + CoalScalar r2 = radius * radius; + CoalScalar ix = v_cyl * (h2 / 3. + r2 / 4.) + + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength); + CoalScalar iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } - FCL_REAL minInflationValue() const { return -radius; } + CoalScalar minInflationValue() const { return -radius; } /// \brief Inflate the capsule by an amount given by `value`. /// This value can be positive or negative but must always >= @@ -434,7 +437,7 @@ class COAL_DLLAPI Capsule : public ShapeBase { /// /// \returns a new inflated capsule and the related transform to account for /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " @@ -466,7 +469,7 @@ class COAL_DLLAPI Cone : public ShapeBase { /// @brief Default constructor Cone() {} - Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { + Cone(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } @@ -477,10 +480,10 @@ class COAL_DLLAPI Cone : public ShapeBase { virtual Cone* clone() const { return new Cone(*this); }; /// @brief Radius of the cone - FCL_REAL radius; + CoalScalar radius; /// @brief Half Length along z axis - FCL_REAL halfLength; + CoalScalar halfLength; /// @brief Compute AABB void computeLocalAABB(); @@ -488,23 +491,25 @@ class COAL_DLLAPI Cone : public ShapeBase { /// @brief Get node type: a cone NODE_TYPE getNodeType() const { return GEOM_CONE; } - FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * + CoalScalar computeVolume() const { + return boost::math::constants::pi() * radius * radius * (halfLength * 2) / 3; } Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); - FCL_REAL ix = + CoalScalar V = computeVolume(); + CoalScalar ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20); - FCL_REAL iz = 0.3 * V * radius * radius; + CoalScalar iz = 0.3 * V * radius * radius; return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); } - FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } + CoalScalar minInflationValue() const { + return -(std::min)(radius, halfLength); + } /// \brief Inflate the cone by an amount given by `value`. /// This value can be positive or negative but must always >= @@ -514,7 +519,7 @@ class COAL_DLLAPI Cone : public ShapeBase { /// /// \returns a new inflated cone and the related transform to account for the /// change of shape frame - std::pair inflated(const FCL_REAL value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " @@ -522,14 +527,15 @@ class COAL_DLLAPI Cone : public ShapeBase { std::invalid_argument); // tan(alpha) = 2*halfLength/radius; - const FCL_REAL tan_alpha = 2 * halfLength / radius; - const FCL_REAL sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha); - const FCL_REAL top_inflation = value / sin_alpha; - const FCL_REAL bottom_inflation = value; + const CoalScalar tan_alpha = 2 * halfLength / radius; + const CoalScalar sin_alpha = + tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha); + const CoalScalar top_inflation = value / sin_alpha; + const CoalScalar bottom_inflation = value; - const FCL_REAL new_lz = 2 * halfLength + top_inflation + bottom_inflation; - const FCL_REAL new_cz = (top_inflation + bottom_inflation) / 2.; - const FCL_REAL new_radius = new_lz / tan_alpha; + const CoalScalar new_lz = 2 * halfLength + top_inflation + bottom_inflation; + const CoalScalar new_cz = (top_inflation + bottom_inflation) / 2.; + const CoalScalar new_radius = new_lz / tan_alpha; return std::make_pair(Cone(new_radius, new_lz), Transform3f(Vec3f(0., 0., new_cz))); @@ -556,7 +562,7 @@ class COAL_DLLAPI Cylinder : public ShapeBase { /// @brief Default constructor Cylinder() {} - Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) { + Cylinder(CoalScalar radius_, CoalScalar lz_) : ShapeBase(), radius(radius_) { halfLength = lz_ / 2; } @@ -575,10 +581,10 @@ class COAL_DLLAPI Cylinder : public ShapeBase { virtual Cylinder* clone() const { return new Cylinder(*this); }; /// @brief Radius of the cylinder - FCL_REAL radius; + CoalScalar radius; /// @brief Half Length along z axis - FCL_REAL halfLength; + CoalScalar halfLength; /// @brief Compute AABB void computeLocalAABB(); @@ -586,19 +592,21 @@ class COAL_DLLAPI Cylinder : public ShapeBase { /// @brief Get node type: a cylinder NODE_TYPE getNodeType() const { return GEOM_CYLINDER; } - FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * + CoalScalar computeVolume() const { + return boost::math::constants::pi() * radius * radius * (halfLength * 2); } Matrix3f computeMomentofInertia() const { - FCL_REAL V = computeVolume(); - FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3); - FCL_REAL iz = V * radius * radius / 2; + CoalScalar V = computeVolume(); + CoalScalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3); + CoalScalar iz = V * radius * radius / 2; return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } - FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } + CoalScalar minInflationValue() const { + return -(std::min)(radius, halfLength); + } /// \brief Inflate the cylinder by an amount given by `value`. /// This value can be positive or negative but must always >= @@ -608,7 +616,7 @@ class COAL_DLLAPI Cylinder : public ShapeBase { /// /// \returns a new inflated cylinder and the related transform to account for /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " @@ -884,12 +892,12 @@ class Convex; class COAL_DLLAPI Halfspace : public ShapeBase { public: /// @brief Construct a half space with normal direction and offset - Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { + Halfspace(const Vec3f& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } /// @brief Construct a plane with normal direction and offset - Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) + Halfspace(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar d_) : ShapeBase(), n(a, b, c), d(d_) { unitNormalTest(); } @@ -909,11 +917,11 @@ class COAL_DLLAPI Halfspace : public ShapeBase { /// @brief Clone *this into a new Halfspace virtual Halfspace* clone() const { return new Halfspace(*this); }; - FCL_REAL signedDistance(const Vec3f& p) const { + CoalScalar signedDistance(const Vec3f& p) const { return n.dot(p) - (d + this->getSweptSphereRadius()); } - FCL_REAL distance(const Vec3f& p) const { + CoalScalar distance(const Vec3f& p) const { return std::abs(this->signedDistance(p)); } @@ -923,8 +931,8 @@ class COAL_DLLAPI Halfspace : public ShapeBase { /// @brief Get node type: a half space NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } - FCL_REAL minInflationValue() const { - return std::numeric_limits::lowest(); + CoalScalar minInflationValue() const { + return std::numeric_limits::lowest(); } /// \brief Inflate the halfspace by an amount given by `value`. @@ -935,7 +943,7 @@ class COAL_DLLAPI Halfspace : public ShapeBase { /// /// \returns a new inflated halfspace and the related transform to account for /// the change of shape frame - std::pair inflated(const FCL_REAL value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " @@ -948,7 +956,7 @@ class COAL_DLLAPI Halfspace : public ShapeBase { Vec3f n; /// @brief Plane offset - FCL_REAL d; + CoalScalar d; protected: /// @brief Turn non-unit normal into unit @@ -975,12 +983,12 @@ class COAL_DLLAPI Halfspace : public ShapeBase { class COAL_DLLAPI Plane : public ShapeBase { public: /// @brief Construct a plane with normal direction and offset - Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { + Plane(const Vec3f& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } /// @brief Construct a plane with normal direction and offset - Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) + Plane(CoalScalar a, CoalScalar b, CoalScalar c, CoalScalar d_) : ShapeBase(), n(a, b, c), d(d_) { unitNormalTest(); } @@ -999,9 +1007,9 @@ class COAL_DLLAPI Plane : public ShapeBase { /// @brief Clone *this into a new Plane virtual Plane* clone() const { return new Plane(*this); }; - FCL_REAL signedDistance(const Vec3f& p) const { - const FCL_REAL dist = n.dot(p) - d; - FCL_REAL signed_dist = + CoalScalar signedDistance(const Vec3f& p) const { + const CoalScalar dist = n.dot(p) - d; + CoalScalar signed_dist = std::abs(n.dot(p) - d) - this->getSweptSphereRadius(); if (dist >= 0) { return signed_dist; @@ -1012,7 +1020,7 @@ class COAL_DLLAPI Plane : public ShapeBase { return signed_dist; } - FCL_REAL distance(const Vec3f& p) const { + CoalScalar distance(const Vec3f& p) const { return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius()); } @@ -1026,7 +1034,7 @@ class COAL_DLLAPI Plane : public ShapeBase { Vec3f n; /// @brief Plane offset - FCL_REAL d; + CoalScalar d; protected: /// @brief Turn non-unit normal into unit diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index 788fb73ed..583e611c6 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -135,7 +135,7 @@ void exposeBroadPhase() { typedef SpatialHashingCollisionManager Derived; bp::class_>( "SpatialHashingCollisionManager", bp::no_init) - .def(dv::init>()); } } diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh index 03790438f..b0ae02d30 100644 --- a/python/broadphase/broadphase_callbacks.hh +++ b/python/broadphase/broadphase_callbacks.hh @@ -84,7 +84,7 @@ struct DistanceCallBackBaseWrapper : DistanceCallBackBase, return distance(o1, o2, dist.coeffRef(0, 0)); } - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) { + bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wconversion" return this->get_override("distance")(o1, o2, dist); diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 32aa74d51..2adacda28 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -130,8 +130,8 @@ void exposeHeightField(const std::string& bvname) { type_name.c_str(), doxygen::class_doc(), no_init) .def(dv::init>()) .def(dv::init, const HeightField&>()) - .def(dv::init, FCL_REAL, FCL_REAL, const MatrixXf&, - bp::optional>()) + .def(dv::init, CoalScalar, CoalScalar, const MatrixXf&, + bp::optional>()) .DEF_CLASS_FUNC(Geometry, getXDim) .DEF_CLASS_FUNC(Geometry, getYDim) @@ -278,7 +278,7 @@ void exposeShapes() { "Box", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Box, halfSide) .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone), @@ -289,7 +289,7 @@ void exposeShapes() { class_, shared_ptr>( "Capsule", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Capsule, radius) .DEF_RW_CLASS_ATTRIB(Capsule, halfLength) @@ -301,7 +301,7 @@ void exposeShapes() { class_, shared_ptr>( "Cone", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cone, radius) .DEF_RW_CLASS_ATTRIB(Cone, halfLength) @@ -360,7 +360,7 @@ void exposeShapes() { class_, shared_ptr>( "Cylinder", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Cylinder, radius) .DEF_RW_CLASS_ATTRIB(Cylinder, halfLength) @@ -372,9 +372,10 @@ void exposeShapes() { class_, shared_ptr>( "Halfspace", doxygen::class_doc(), no_init) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def( + dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Halfspace, n) .DEF_RW_CLASS_ATTRIB(Halfspace, d) @@ -386,9 +387,9 @@ void exposeShapes() { class_, shared_ptr>( "Plane", doxygen::class_doc(), no_init) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Plane, n) .DEF_RW_CLASS_ATTRIB(Plane, d) @@ -401,7 +402,7 @@ void exposeShapes() { "Sphere", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Sphere, radius) .def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone), return_value_policy()) @@ -411,7 +412,7 @@ void exposeShapes() { class_, shared_ptr>( "Ellipsoid", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) @@ -438,7 +439,7 @@ void exposeShapes() { boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) { Vec3f P, Q; - FCL_REAL distance = self.distance(other, &P, &Q); + CoalScalar distance = self.distance(other, &P, &Q); return boost::python::make_tuple(distance, P, Q); } @@ -524,7 +525,8 @@ void exposeCollisionGeometries() { "Check whether two AABB are overlaping and return the overloaping " "part if true.") - .def("distance", (FCL_REAL(AABB::*)(const AABB&) const) & AABB::distance, + .def("distance", + (CoalScalar(AABB::*)(const AABB&) const) & AABB::distance, bp::args("self", "other"), "Distance between two AABBs.") // .def("distance", // AABB_distance_proxy, @@ -563,18 +565,18 @@ void exposeCollisionGeometries() { .def("volume", &AABB::volume, bp::arg("self"), "Volume of the AABB.") .def("expand", - static_cast(&AABB::expand), + static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), + // AABB &, CoalScalar)>(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), + // (AABB::*)(const AABB &, CoalScalar)>(&AABB::expand)), bp::return_internal_reference<>()) .def("expand", - static_cast(&AABB::expand), + static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), + // CoalScalar)>(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), + // (AABB::*)(const CoalScalar)>(&AABB::expand)), bp::return_internal_reference<>()) .def("expand", static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast()) .def(dv::init()) + const Vec3f&, CoalScalar>()) .add_property( "o1", make_function(&geto<1>, diff --git a/python/contact_patch.cc b/python/contact_patch.cc index e79721125..4721e8754 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -93,12 +93,12 @@ void exposeContactPatchAPI() { ContactPatchRequest>()) { class_( "ContactPatchRequest", doxygen::class_doc(), - init>( + init>( (arg("self"), arg("max_num_patch"), arg("num_samples_curved_shapes"), arg("patch_tolerance")), "ContactPatchRequest constructor.")) .def(dv::init>()) + bp::optional>()) .DEF_RW_CLASS_ATTRIB(ContactPatchRequest, max_num_patch) .DEF_CLASS_FUNC(ContactPatchRequest, getNumSamplesCurvedShapes) .DEF_CLASS_FUNC(ContactPatchRequest, setNumSamplesCurvedShapes) diff --git a/python/distance.cc b/python/distance.cc index e6080a8b7..0e0032d9e 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -73,7 +73,7 @@ void exposeDistanceAPI() { if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "DistanceRequest", doxygen::class_doc(), - init >( + init >( (arg("self"), arg("enable_nearest_points"), arg("rel_err"), arg("abs_err")), "Constructor")) @@ -149,14 +149,14 @@ void exposeDistanceAPI() { doxygen::def( "distance", - static_cast( + static_cast( &distance)); doxygen::def( "distance", - static_cast( + static_cast( &distance)); class_("ComputeDistance", @@ -164,7 +164,7 @@ void exposeDistanceAPI() { .def(dv::init()) .def("__call__", - static_cast(&ComputeDistance::operator())); } diff --git a/python/gjk.cc b/python/gjk.cc index c5dbb0d10..9f705fb91 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -178,7 +178,7 @@ void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("GJK", doxygen::class_doc(), no_init) - .def(doxygen::visitor::init()) + .def(doxygen::visitor::init()) .DEF_RW_CLASS_ATTRIB(GJK, distance) .DEF_RW_CLASS_ATTRIB(GJK, ray) .DEF_RW_CLASS_ATTRIB(GJK, support_hint) diff --git a/python/math.cc b/python/math.cc index 1622eb340..85c365d46 100644 --- a/python/math.cc +++ b/python/math.cc @@ -95,7 +95,7 @@ void exposeMaths() { return_value_policy()) .def("isIdentity", &Transform3f::isIdentity, (bp::arg("self"), - bp::arg("prec") = Eigen::NumTraits::dummy_precision()), + bp::arg("prec") = Eigen::NumTraits::dummy_precision()), doxygen::member_func_doc(&Transform3f::getTranslation)) .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) diff --git a/python/octree.cc b/python/octree.cc index c18c7d64b..a569bba51 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -35,7 +35,7 @@ void exposeOctree() { bp::class_, shared_ptr >( "OcTree", doxygen::class_doc(), bp::no_init) - .def(dv::init()) + .def(dv::init()) .def("clone", &OcTree::clone, doxygen::member_func_doc(&OcTree::clone), bp::return_value_policy()) .def(dv::member_func("getTreeDepth", &OcTree::getTreeDepth)) diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index c7b427a75..de3d582d0 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -44,18 +44,18 @@ namespace coal { AABB::AABB() - : min_(Vec3f::Constant((std::numeric_limits::max)())), - max_(Vec3f::Constant(-(std::numeric_limits::max)())) {} + : min_(Vec3f::Constant((std::numeric_limits::max)())), + max_(Vec3f::Constant(-(std::numeric_limits::max)())) {} bool AABB::overlap(const AABB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { - const FCL_REAL break_distance_squared = + CoalScalar& sqrDistLowerBound) const { + const CoalScalar break_distance_squared = request.break_distance * request.break_distance; sqrDistLowerBound = (min_ - other.max_ - Vec3f::Constant(request.security_margin)) .array() - .max(FCL_REAL(0)) + .max(CoalScalar(0)) .matrix() .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; @@ -63,7 +63,7 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request, sqrDistLowerBound = (other.min_ - max_ - Vec3f::Constant(request.security_margin)) .array() - .max(FCL_REAL(0)) + .max(CoalScalar(0)) .matrix() .squaredNorm(); if (sqrDistLowerBound > break_distance_squared) return false; @@ -71,23 +71,23 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request, return true; } -FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { - FCL_REAL result = 0; +CoalScalar AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { + CoalScalar result = 0; for (Eigen::DenseIndex i = 0; i < 3; ++i) { - const FCL_REAL& amin = min_[i]; - const FCL_REAL& amax = max_[i]; - const FCL_REAL& bmin = other.min_[i]; - const FCL_REAL& bmax = other.max_[i]; + const CoalScalar& amin = min_[i]; + const CoalScalar& amax = max_[i]; + const CoalScalar& bmin = other.min_[i]; + const CoalScalar& bmax = other.max_[i]; if (amin > bmax) { - FCL_REAL delta = bmax - amin; + CoalScalar delta = bmax - amin; result += delta * delta; if (P && Q) { (*P)[i] = amin; (*Q)[i] = bmax; } } else if (bmin > amax) { - FCL_REAL delta = amax - bmin; + CoalScalar delta = amax - bmin; result += delta * delta; if (P && Q) { (*P)[i] = amax; @@ -96,11 +96,11 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { } else { if (P && Q) { if (bmin >= amin) { - FCL_REAL t = 0.5 * (amax + bmin); + CoalScalar t = 0.5 * (amax + bmin); (*P)[i] = t; (*Q)[i] = t; } else { - FCL_REAL t = 0.5 * (amin + bmax); + CoalScalar t = 0.5 * (amin + bmax); (*P)[i] = t; (*Q)[i] = t; } @@ -111,19 +111,19 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { return std::sqrt(result); } -FCL_REAL AABB::distance(const AABB& other) const { - FCL_REAL result = 0; +CoalScalar AABB::distance(const AABB& other) const { + CoalScalar result = 0; for (Eigen::DenseIndex i = 0; i < 3; ++i) { - const FCL_REAL& amin = min_[i]; - const FCL_REAL& amax = max_[i]; - const FCL_REAL& bmin = other.min_[i]; - const FCL_REAL& bmax = other.max_[i]; + const CoalScalar& amin = min_[i]; + const CoalScalar& amax = max_[i]; + const CoalScalar& bmin = other.min_[i]; + const CoalScalar& bmax = other.max_[i]; if (amin > bmax) { - FCL_REAL delta = bmax - amin; + CoalScalar delta = bmax - amin; result += delta * delta; } else if (bmin > amax) { - FCL_REAL delta = amax - bmin; + CoalScalar delta = amax - bmin; result += delta * delta; } } @@ -139,7 +139,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) { + CoalScalar& sqrDistLowerBound) { AABB bb1(translate(rotate(b1, R0), T0)); return bb1.overlap(b2, request, sqrDistLowerBound); } @@ -156,8 +156,8 @@ bool AABB::overlap(const Plane& p) const { const Vec3f support2 = ((-p.n).array() > 0).select(halfside, -halfside) + center; - const FCL_REAL dist1 = p.n.dot(support1) - p.d; - const FCL_REAL dist2 = p.n.dot(support2) - p.d; + const CoalScalar dist1 = p.n.dot(support1) - p.d; + const CoalScalar dist2 = p.n.dot(support2) - p.d; const int sign1 = (dist1 > 0) ? 1 : -1; const int sign2 = (dist2 > 0) ? 1 : -1; @@ -169,8 +169,8 @@ bool AABB::overlap(const Plane& p) const { // Both supports are on the same side of the plane. // We now need to check if they are on the same side of the plane inflated // by the swept-sphere radius. - const FCL_REAL ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius(); - const FCL_REAL ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius(); + const CoalScalar ssr_dist1 = std::abs(dist1) - p.getSweptSphereRadius(); + const CoalScalar ssr_dist2 = std::abs(dist2) - p.getSweptSphereRadius(); const int ssr_sign1 = (ssr_dist1 > 0) ? 1 : -1; const int ssr_sign2 = (ssr_dist2 > 0) ? 1 : -1; return ssr_sign1 != ssr_sign2; diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index e69b49ede..621b059c2 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -67,7 +67,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) { computeVertices(b2, vertex + 8); Matrix3f M; Vec3f E[3]; - FCL_REAL s[3] = {0, 0, 0}; + CoalScalar s[3] = {0, 0, 0}; // obb axes b.axes.col(0).noalias() = (b1.To - b2.To).normalized(); @@ -122,7 +122,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { b.axes = q.toRotationMatrix(); Vec3f vertex[8], diff; - FCL_REAL real_max = (std::numeric_limits::max)(); + CoalScalar real_max = (std::numeric_limits::max)(); Vec3f pmin(real_max, real_max, real_max); Vec3f pmax(-real_max, -real_max, -real_max); @@ -130,7 +130,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for (int j = 0; j < 3; ++j) { - FCL_REAL dot = diff.dot(b.axes.col(j)); + CoalScalar dot = diff.dot(b.axes.col(j)); if (dot > pmax[j]) pmax[j] = dot; else if (dot < pmin[j]) @@ -142,7 +142,7 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { for (int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for (int j = 0; j < 3; ++j) { - FCL_REAL dot = diff.dot(b.axes.col(j)); + CoalScalar dot = diff.dot(b.axes.col(j)); if (dot > pmax[j]) pmax[j] = dot; else if (dot < pmin[j]) @@ -160,8 +160,8 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b) { - FCL_REAL t, s; - const FCL_REAL reps = 1e-6; + CoalScalar t, s; + const CoalScalar reps = 1e-6; Matrix3f Bf(B.array().abs() + reps); // Bf += reps; @@ -286,20 +286,20 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } namespace internal { -inline FCL_REAL obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf) { +inline CoalScalar obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a, + const Vec3f& b, const Matrix3f& Bf) { // |T| - |B| * b - a Vec3f AABB_corner(T.cwiseAbs() - a); AABB_corner.noalias() -= Bf * b; - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm(); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); } -inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf) { +inline CoalScalar obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const Matrix3f& Bf) { // Bf = |B| // | B^T T| - Bf^T * a - b - FCL_REAL s, t = 0; + CoalScalar s, t = 0; s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; if (s > 0) t += s * s; s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; @@ -313,15 +313,15 @@ template struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { static inline bool run(int ia, int ja, int ka, const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + const Matrix3f& Bf, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 < 1e-6) return false; - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { @@ -344,23 +344,23 @@ struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, const Vec3f& a_, const Vec3f& b_, const CollisionRequest& request, - FCL_REAL& squaredLowerBoundDistance) { + CoalScalar& squaredLowerBoundDistance) { assert(request.security_margin > -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) - - 10 * Eigen::NumTraits::epsilon() && + 10 * Eigen::NumTraits::epsilon() && "A negative security margin could not be lower than the OBB extent."); - // const FCL_REAL breakDistance(request.break_distance + + // const CoalScalar breakDistance(request.break_distance + // request.security_margin); - const FCL_REAL breakDistance2 = + const CoalScalar breakDistance2 = request.break_distance * request.break_distance; Matrix3f Bf(B.cwiseAbs()); const Vec3f a((a_ + Vec3f::Constant(request.security_margin / 2)) .array() - .max(FCL_REAL(0))); + .max(CoalScalar(0))); const Vec3f b((b_ + Vec3f::Constant(request.security_margin / 2)) .array() - .max(FCL_REAL(0))); + .max(CoalScalar(0))); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); @@ -403,7 +403,7 @@ bool OBB::overlap(const OBB& other) const { } bool OBB::overlap(const OBB& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part @@ -424,7 +424,7 @@ bool OBB::overlap(const OBB& other, const CollisionRequest& request, bool OBB::contain(const Vec3f& p) const { Vec3f local_p(p - To); - FCL_REAL proj = local_p.dot(axes.col(0)); + CoalScalar proj = local_p.dot(axes.col(0)); if ((proj > extent[0]) || (proj < -extent[0])) return false; proj = local_p.dot(axes.col(1)); @@ -448,8 +448,8 @@ OBB& OBB::operator+=(const Vec3f& p) { OBB OBB::operator+(const OBB& other) const { Vec3f center_diff = To - other.To; - FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); - FCL_REAL max_extent2 = + CoalScalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); + CoalScalar max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); if (center_diff.norm() > 2 * (max_extent + max_extent2)) { return merge_largedist(*this, other); @@ -458,7 +458,8 @@ OBB OBB::operator+(const OBB& other) const { } } -FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const { +CoalScalar OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, + Vec3f* /*Q*/) const { std::cerr << "OBB distance not implemented!" << std::endl; return 0.0; } @@ -473,7 +474,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, } bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2, - const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { + const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To); Vec3f T(b1.axes.transpose() * Ttemp); Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); diff --git a/src/BV/OBB.h b/src/BV/OBB.h index 051a9af71..907272a50 100644 --- a/src/BV/OBB.h +++ b/src/BV/OBB.h @@ -41,7 +41,7 @@ namespace coal { bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const CollisionRequest& request, - FCL_REAL& squaredLowerBoundDistance); + CoalScalar& squaredLowerBoundDistance); bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b); diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index 1738f8abd..15c3043ac 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -45,7 +45,7 @@ namespace coal { /// @brief Clip value between a and b -void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { +void clipToRange(CoalScalar& val, CoalScalar a, CoalScalar b) { if (val < a) val = a; else if (val > b) @@ -63,9 +63,9 @@ void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { /// of each segment. "T" in the dot products is the vector betweeen Pa and Pb. /// Reference: "On fast computation of distance between line segments." Vladimir /// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. -void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, - FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) { - FCL_REAL denom = 1 - A_dot_B * A_dot_B; +void segCoords(CoalScalar& t, CoalScalar& u, CoalScalar a, CoalScalar b, + CoalScalar A_dot_B, CoalScalar A_dot_T, CoalScalar B_dot_T) { + CoalScalar denom = 1 - A_dot_B * A_dot_B; if (denom == 0) t = 0; @@ -91,12 +91,12 @@ void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, /// Pa + A*t, 0 <= t <= a, is within the half space /// determined by the point Pa and the direction Anorm. /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. -bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, - FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, - FCL_REAL B_dot_T) { +bool inVoronoi(CoalScalar a, CoalScalar b, CoalScalar Anorm_dot_B, + CoalScalar Anorm_dot_T, CoalScalar A_dot_B, CoalScalar A_dot_T, + CoalScalar B_dot_T) { if (fabs(Anorm_dot_B) < 1e-7) return false; - FCL_REAL t, u, v; + CoalScalar t, u, v; u = -Anorm_dot_T / Anorm_dot_B; clipToRange(u, 0, b); @@ -117,18 +117,18 @@ bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, /// @brief Distance between two oriented rectangles; P and Q (optional return /// values) are the closest points in the rectangles, both are in the local /// frame of the first rectangle. -FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, - const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, - Vec3f* Q = NULL) { - FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; +CoalScalar rectDistance(const Matrix3f& Rab, Vec3f const& Tab, + const CoalScalar a[2], const CoalScalar b[2], + Vec3f* P = NULL, Vec3f* Q = NULL) { + CoalScalar A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; A0_dot_B0 = Rab(0, 0); A0_dot_B1 = Rab(0, 1); A1_dot_B0 = Rab(1, 0); A1_dot_B1 = Rab(1, 1); - FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; - FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; + CoalScalar aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; + CoalScalar bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; aA0_dot_B0 = a[0] * A0_dot_B0; aA0_dot_B1 = a[0] * A0_dot_B1; @@ -142,13 +142,13 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, Vec3f Tba(Rab.transpose() * Tab); Vec3f S; - FCL_REAL t, u; + CoalScalar t, u; // determine if any edge pair contains the closest points - FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x; - FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x; - FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; + CoalScalar ALL_x, ALU_x, AUL_x, AUU_x; + CoalScalar BLL_x, BLU_x, BUL_x, BUU_x; + CoalScalar LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; ALL_x = -Tba[0]; ALU_x = ALL_x + aA1_dot_B0; @@ -277,14 +277,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y; + CoalScalar ALL_y, ALU_y, AUL_y, AUU_y; ALL_y = -Tba[1]; ALU_y = ALL_y + aA1_dot_B1; AUL_y = ALL_y + aA0_dot_B1; AUU_y = ALU_y + aA0_dot_B1; - FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; + CoalScalar LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; if (ALL_y < ALU_y) { LA1_ly = ALL_y; @@ -404,14 +404,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y; + CoalScalar BLL_y, BLU_y, BUL_y, BUU_y; BLL_y = Tab[1]; BLU_y = BLL_y + bA1_dot_B1; BUL_y = BLL_y + bA1_dot_B0; BUU_y = BLU_y + bA1_dot_B0; - FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; + CoalScalar LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; if (ALL_x < AUL_x) { LA0_lx = ALL_x; @@ -530,7 +530,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; + CoalScalar LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; if (ALL_y < AUL_y) { LA0_ly = ALL_y; @@ -652,7 +652,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, // no edges passed, take max separation along face normals - FCL_REAL sep1, sep2; + CoalScalar sep1, sep2; if (Tab[2] > 0.0) { sep1 = Tab[2]; @@ -707,7 +707,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } } - FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2); + CoalScalar sep = (sep1 > sep2 ? sep1 : sep2); return (sep > 0 ? sep : 0); } @@ -722,7 +722,7 @@ bool RSS::overlap(const RSS& other) const { /// Now compute R1'R2 Matrix3f R(axes.transpose() * other.axes); - FCL_REAL dist = rectDistance(R, T, length, other.length); + CoalScalar dist = rectDistance(R, T, length, other.length); return (dist <= (radius + other.radius)); } @@ -737,12 +737,12 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, Vec3f T(b1.axes.transpose() * Ttemp); Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); - FCL_REAL dist = rectDistance(R, T, b1.length, b2.length); + CoalScalar dist = rectDistance(R, T, b1.length, b2.length); return (dist <= (b1.radius + b2.radius)); } bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, - const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { + const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] @@ -752,8 +752,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f T(b1.axes.transpose() * Ttemp); Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); - FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - - b2.radius - request.security_margin; + CoalScalar dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - + b2.radius - request.security_margin; if (dist <= 0) return true; sqrDistLowerBound = dist * dist; return false; @@ -762,10 +762,10 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, bool RSS::contain(const Vec3f& p) const { Vec3f local_p = p - Tr; // FIXME: Vec3f proj (axes.transpose() * local_p); - FCL_REAL proj0 = local_p.dot(axes.col(0)); - FCL_REAL proj1 = local_p.dot(axes.col(1)); - FCL_REAL proj2 = local_p.dot(axes.col(2)); - FCL_REAL abs_proj2 = fabs(proj2); + CoalScalar proj0 = local_p.dot(axes.col(0)); + CoalScalar proj1 = local_p.dot(axes.col(1)); + CoalScalar proj2 = local_p.dot(axes.col(2)); + CoalScalar abs_proj2 = fabs(proj2); Vec3f proj(proj0, proj1, proj2); /// projection is within the rectangle @@ -774,17 +774,17 @@ bool RSS::contain(const Vec3f& p) const { return (abs_proj2 < radius); } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { - FCL_REAL y = (proj1 > 0) ? length[1] : 0; + CoalScalar y = (proj1 > 0) ? length[1] : 0; Vec3f v(proj0, y, 0); return ((proj - v).squaredNorm() < radius * radius); } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; + CoalScalar x = (proj0 > 0) ? length[0] : 0; Vec3f v(x, proj1, 0); return ((proj - v).squaredNorm() < radius * radius); } else { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; - FCL_REAL y = (proj1 > 0) ? length[1] : 0; + CoalScalar x = (proj0 > 0) ? length[0] : 0; + CoalScalar y = (proj1 > 0) ? length[1] : 0; Vec3f v(x, y, 0); return ((proj - v).squaredNorm() < radius * radius); } @@ -792,10 +792,10 @@ bool RSS::contain(const Vec3f& p) const { RSS& RSS::operator+=(const Vec3f& p) { Vec3f local_p = p - Tr; - FCL_REAL proj0 = local_p.dot(axes.col(0)); - FCL_REAL proj1 = local_p.dot(axes.col(1)); - FCL_REAL proj2 = local_p.dot(axes.col(2)); - FCL_REAL abs_proj2 = fabs(proj2); + CoalScalar proj0 = local_p.dot(axes.col(0)); + CoalScalar proj1 = local_p.dot(axes.col(1)); + CoalScalar proj2 = local_p.dot(axes.col(2)); + CoalScalar abs_proj2 = fabs(proj2); Vec3f proj(proj0, proj1, proj2); // projection is within the rectangle @@ -813,19 +813,19 @@ RSS& RSS::operator+=(const Vec3f& p) { } } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { - FCL_REAL y = (proj1 > 0) ? length[1] : 0; + CoalScalar y = (proj1 > 0) ? length[1] : 0; Vec3f v(proj0, y, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); + CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { - FCL_REAL delta_y = + CoalScalar delta_y = -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y); length[1] += delta_y; if (proj1 < 0) Tr[1] -= delta_y; } else { - FCL_REAL delta_y = fabs(proj1 - y); + CoalScalar delta_y = fabs(proj1 - y); length[1] += delta_y; if (proj1 < 0) Tr[1] -= delta_y; @@ -837,19 +837,19 @@ RSS& RSS::operator+=(const Vec3f& p) { } } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; + CoalScalar x = (proj0 > 0) ? length[0] : 0; Vec3f v(x, proj1, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); + CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { - FCL_REAL delta_x = + CoalScalar delta_x = -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x); length[0] += delta_x; if (proj0 < 0) Tr[0] -= delta_x; } else { - FCL_REAL delta_x = fabs(proj0 - x); + CoalScalar delta_x = fabs(proj0 - x); length[0] += delta_x; if (proj0 < 0) Tr[0] -= delta_x; @@ -860,20 +860,20 @@ RSS& RSS::operator+=(const Vec3f& p) { } } } else { - FCL_REAL x = (proj0 > 0) ? length[0] : 0; - FCL_REAL y = (proj1 > 0) ? length[1] : 0; + CoalScalar x = (proj0 > 0) ? length[0] : 0; + CoalScalar y = (proj1 > 0) ? length[1] : 0; Vec3f v(x, y, 0); - FCL_REAL new_r_sqr = (proj - v).squaredNorm(); + CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing else { if (abs_proj2 < radius) { - FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2); - FCL_REAL delta_diag = + CoalScalar diag = std::sqrt(new_r_sqr - proj2 * proj2); + CoalScalar delta_diag = -std::sqrt(radius * radius - proj2 * proj2) + diag; - FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x); - FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y); + CoalScalar delta_x = delta_diag / diag * fabs(proj0 - x); + CoalScalar delta_y = delta_diag / diag * fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; @@ -882,8 +882,8 @@ RSS& RSS::operator+=(const Vec3f& p) { Tr[1] -= delta_y; } } else { - FCL_REAL delta_x = fabs(proj0 - x); - FCL_REAL delta_y = fabs(proj1 - y); + CoalScalar delta_x = fabs(proj0 - x); + CoalScalar delta_y = fabs(proj1 - y); length[0] += delta_x; length[1] += delta_y; @@ -942,7 +942,7 @@ RSS RSS::operator+(const RSS& other) const { Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors - FCL_REAL s[3] = {0, 0, 0}; + CoalScalar s[3] = {0, 0, 0}; getCovariance(v, NULL, NULL, NULL, 16, M); eigen(M, s, E); @@ -979,28 +979,28 @@ RSS RSS::operator+(const RSS& other) const { return bv; } -FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const { +CoalScalar RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const { // compute what transform [R,T] that takes us from cs1 to cs2. // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] // First compute the rotation part, then translation part Matrix3f R(axes.transpose() * other.axes); Vec3f T(axes.transpose() * (other.Tr - Tr)); - FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q); + CoalScalar dist = rectDistance(R, T, length, other.length, P, Q); dist -= (radius + other.radius); - return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; + return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist; } -FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, - const RSS& b2, Vec3f* P, Vec3f* Q) { +CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, + const RSS& b2, Vec3f* P, Vec3f* Q) { Matrix3f R(b1.axes.transpose() * R0 * b2.axes); Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr); Vec3f T(b1.axes.transpose() * Ttemp); - FCL_REAL dist = rectDistance(R, T, b1.length, b2.length, P, Q); + CoalScalar dist = rectDistance(R, T, b1.length, b2.length, P, Q); dist -= (b1.radius + b2.radius); - return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; + return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist; } RSS translate(const RSS& bv, const Vec3f& t) { diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp index 80e94990e..cc581a84d 100644 --- a/src/BV/kDOP.cpp +++ b/src/BV/kDOP.cpp @@ -44,7 +44,8 @@ namespace coal { /// @brief Find the smaller and larger one of two values -inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) { +inline void minmax(CoalScalar a, CoalScalar b, CoalScalar& minv, + CoalScalar& maxv) { if (a > b) { minv = b; maxv = a; @@ -54,7 +55,7 @@ inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) { } } /// @brief Merge the interval [minv, maxv] and value p/ -inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) { +inline void minmax(CoalScalar p, CoalScalar& minv, CoalScalar& maxv) { if (p > maxv) maxv = p; if (p < minv) minv = p; } @@ -62,11 +63,11 @@ inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) { /// @brief Compute the distances to planes with normals from KDOP vectors except /// those of AABB face planes template -void getDistances(const Vec3f& /*p*/, FCL_REAL* /*d*/) {} +void getDistances(const Vec3f& /*p*/, CoalScalar* /*d*/) {} /// @brief Specification of getDistances template <> -inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) { +inline void getDistances<5>(const Vec3f& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -75,7 +76,7 @@ inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) { } template <> -inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) { +inline void getDistances<6>(const Vec3f& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -85,7 +86,7 @@ inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) { } template <> -inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) { +inline void getDistances<9>(const Vec3f& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -99,7 +100,7 @@ inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) { template KDOP::KDOP() { - FCL_REAL real_max = (std::numeric_limits::max)(); + CoalScalar real_max = (std::numeric_limits::max)(); dist_.template head().setConstant(real_max); dist_.template tail().setConstant(-real_max); } @@ -110,7 +111,7 @@ KDOP::KDOP(const Vec3f& v) { dist_[i] = dist_[N / 2 + i] = v[i]; } - FCL_REAL d[(N - 6) / 2]; + CoalScalar d[(N - 6) / 2]; getDistances<(N - 6) / 2>(v, d); for (short i = 0; i < (N - 6) / 2; ++i) { dist_[3 + i] = dist_[3 + i + N / 2] = d[i]; @@ -123,7 +124,7 @@ KDOP::KDOP(const Vec3f& a, const Vec3f& b) { minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); } - FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2]; + CoalScalar ad[(N - 6) / 2], bd[(N - 6) / 2]; getDistances<(N - 6) / 2>(a, ad); getDistances<(N - 6) / 2>(b, bd); for (short i = 0; i < (N - 6) / 2; ++i) { @@ -142,11 +143,11 @@ bool KDOP::overlap(const KDOP& other) const { template bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); + CoalScalar& sqrDistLowerBound) const { + const CoalScalar breakDistance(request.break_distance + + request.security_margin); - FCL_REAL a = + CoalScalar a = (dist_.template head() - other.dist_.template tail()) .minCoeff(); if (a > breakDistance) { @@ -154,7 +155,7 @@ bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, return false; } - FCL_REAL b = + CoalScalar b = (other.dist_.template head() - dist_.template tail()) .minCoeff(); if (b > breakDistance) { @@ -172,7 +173,7 @@ bool KDOP::inside(const Vec3f& p) const { if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false; enum { P = ((N - 6) / 2) }; - Eigen::Array d; + Eigen::Array d; getDistances

(p, d.data()); if ((d < dist_.template segment

(3)).any()) return false; @@ -187,7 +188,7 @@ KDOP& KDOP::operator+=(const Vec3f& p) { minmax(p[i], dist_[i], dist_[N / 2 + i]); } - FCL_REAL pd[(N - 6) / 2]; + CoalScalar pd[(N - 6) / 2]; getDistances<(N - 6) / 2>(p, pd); for (short i = 0; i < (N - 6) / 2; ++i) { minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]); @@ -212,8 +213,8 @@ KDOP KDOP::operator+(const KDOP& other) const { } template -FCL_REAL KDOP::distance(const KDOP& /*other*/, Vec3f* /*P*/, - Vec3f* /*Q*/) const { +CoalScalar KDOP::distance(const KDOP& /*other*/, Vec3f* /*P*/, + Vec3f* /*Q*/) const { std::cerr << "KDOP distance not implemented!" << std::endl; return 0.0; } @@ -226,7 +227,7 @@ KDOP translate(const KDOP& bv, const Vec3f& t) { res.dist(short(N / 2 + i)) += t[i]; } - FCL_REAL d[(N - 6) / 2]; + CoalScalar d[(N - 6) / 2]; getDistances<(N - 6) / 2>(t, d); for (short i = 0; i < (N - 6) / 2; ++i) { res.dist(short(3 + i)) += d[i]; diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp index 2b50f4b65..10e9d7fd1 100644 --- a/src/BV/kIOS.cpp +++ b/src/BV/kIOS.cpp @@ -47,8 +47,8 @@ namespace coal { bool kIOS::overlap(const kIOS& other) const { for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { - FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); - FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; + CoalScalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); + CoalScalar sum_r = spheres[i].r + other.spheres[j].r; if (o_dist > sum_r * sum_r) return false; } } @@ -57,11 +57,11 @@ bool kIOS::overlap(const kIOS& other) const { } bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) const { + CoalScalar& sqrDistLowerBound) const { for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { - FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); - FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; + CoalScalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); + CoalScalar sum_r = spheres[i].r + other.spheres[j].r; if (o_dist > sum_r * sum_r) { o_dist = sqrt(o_dist) - sum_r; sqrDistLowerBound = o_dist * o_dist; @@ -75,7 +75,7 @@ bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, bool kIOS::contain(const Vec3f& p) const { for (unsigned int i = 0; i < num_spheres; ++i) { - FCL_REAL r = spheres[i].r; + CoalScalar r = spheres[i].r; if ((spheres[i].o - p).squaredNorm() > r * r) return false; } @@ -84,8 +84,8 @@ bool kIOS::contain(const Vec3f& p) const { kIOS& kIOS::operator+=(const Vec3f& p) { for (unsigned int i = 0; i < num_spheres; ++i) { - FCL_REAL r = spheres[i].r; - FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm(); + CoalScalar r = spheres[i].r; + CoalScalar new_r_sqr = (p - spheres[i].o).squaredNorm(); if (new_r_sqr > r * r) { spheres[i].r = sqrt(new_r_sqr); } @@ -109,23 +109,23 @@ kIOS kIOS::operator+(const kIOS& other) const { return result; } -FCL_REAL kIOS::width() const { return obb.width(); } +CoalScalar kIOS::width() const { return obb.width(); } -FCL_REAL kIOS::height() const { return obb.height(); } +CoalScalar kIOS::height() const { return obb.height(); } -FCL_REAL kIOS::depth() const { return obb.depth(); } +CoalScalar kIOS::depth() const { return obb.depth(); } -FCL_REAL kIOS::volume() const { return obb.volume(); } +CoalScalar kIOS::volume() const { return obb.volume(); } -FCL_REAL kIOS::size() const { return volume(); } +CoalScalar kIOS::size() const { return volume(); } -FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { - FCL_REAL d_max = 0; +CoalScalar kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { + CoalScalar d_max = 0; long id_a = -1, id_b = -1; for (unsigned int i = 0; i < num_spheres; ++i) { for (unsigned int j = 0; j < other.num_spheres; ++j) { - FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() - - (spheres[i].r + other.spheres[j].r); + CoalScalar d = (spheres[i].o - other.spheres[j].o).norm() - + (spheres[i].r + other.spheres[j].r); if (d_max < d) { d_max = d; if (P && Q) { @@ -139,7 +139,7 @@ FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { if (P && Q) { if (id_a != -1 && id_b != -1) { const Vec3f v = spheres[id_a].o - spheres[id_b].o; - FCL_REAL len_v = v.norm(); + CoalScalar len_v = v.norm(); *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); } @@ -164,7 +164,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, const CollisionRequest& request, - FCL_REAL& sqrDistLowerBound) { + CoalScalar& sqrDistLowerBound) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o.noalias() = @@ -177,8 +177,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, return b1.overlap(b2_temp, request, sqrDistLowerBound); } -FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, - const kIOS& b2, Vec3f* P, Vec3f* Q) { +CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, + const kIOS& b2, Vec3f* P, Vec3f* Q) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp index cd45cabf6..807549e69 100644 --- a/src/BVH/BVH_model.cpp +++ b/src/BVH/BVH_model.cpp @@ -789,7 +789,7 @@ void BVHModelBase::computeLocalAABB() { aabb_radius = 0; for (unsigned int i = 0; i < num_vertices; ++i) { - FCL_REAL r = (aabb_center - vertices_[i]).squaredNorm(); + CoalScalar r = (aabb_center - vertices_[i]).squaredNorm(); if (r > aabb_radius) aabb_radius = r; } diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp index 2072921b2..9a9194310 100644 --- a/src/BVH/BVH_utility.cpp +++ b/src/BVH/BVH_utility.cpp @@ -88,7 +88,7 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const DistanceRequest distanceRequest(enable_nearest_points, compute_penetration); DistanceResult distanceResult; - const FCL_REAL distance = ShapeShapeDistance( + const CoalScalar distance = ShapeShapeDistance( &box, box_pose, &tri, Transform3f(), &gjk, distanceRequest, distanceResult); bool is_collision = @@ -263,13 +263,13 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, const Matrix3f& axes, Vec3f& origin, - FCL_REAL l[2], FCL_REAL& r) { + CoalScalar l[2], CoalScalar& r) { bool indirect_index = true; if (!indices) indirect_index = false; unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; - FCL_REAL(*P)[3] = new FCL_REAL[size_P][3]; + CoalScalar(*P)[3] = new CoalScalar[size_P][3]; int P_id = 0; @@ -322,34 +322,34 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - FCL_REAL minx, maxx, miny, maxy, minz, maxz; + CoalScalar minx, maxx, miny, maxy, minz, maxz; - FCL_REAL cz, radsqr; + CoalScalar cz, radsqr; minz = maxz = P[0][2]; for (unsigned int i = 1; i < size_P; ++i) { - FCL_REAL z_value = P[i][2]; + CoalScalar z_value = P[i][2]; if (z_value < minz) minz = z_value; else if (z_value > maxz) maxz = z_value; } - r = (FCL_REAL)0.5 * (maxz - minz); + r = (CoalScalar)0.5 * (maxz - minz); radsqr = r * r; - cz = (FCL_REAL)0.5 * (maxz + minz); + cz = (CoalScalar)0.5 * (maxz + minz); // compute an initial norm of rectangle along x direction // find minx and maxx as starting points unsigned int minindex = 0, maxindex = 0; - FCL_REAL mintmp, maxtmp; + CoalScalar mintmp, maxtmp; mintmp = maxtmp = P[0][0]; for (unsigned int i = 1; i < size_P; ++i) { - FCL_REAL x_value = P[i][0]; + CoalScalar x_value = P[i][0]; if (x_value < mintmp) { minindex = i; mintmp = x_value; @@ -359,22 +359,22 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - FCL_REAL x, dz; + CoalScalar x, dz; dz = P[minindex][2] - cz; - minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; - maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow minx/maxx for (unsigned int i = 0; i < size_P; ++i) { if (P[i][0] < minx) { dz = P[i][2] - cz; - x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); + x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); if (x < minx) minx = x; } else if (P[i][0] > maxx) { dz = P[i][2] - cz; - x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); + x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); if (x > maxx) maxx = x; } } @@ -386,7 +386,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, minindex = maxindex = 0; mintmp = maxtmp = P[0][1]; for (unsigned int i = 1; i < size_P; ++i) { - FCL_REAL y_value = P[i][1]; + CoalScalar y_value = P[i][1]; if (y_value < mintmp) { minindex = i; mintmp = y_value; @@ -396,30 +396,30 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - FCL_REAL y; + CoalScalar y; dz = P[minindex][2] - cz; - miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; - maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow miny/maxy for (unsigned int i = 0; i < size_P; ++i) { if (P[i][1] < miny) { dz = P[i][2] - cz; - y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); + y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); if (y < miny) miny = y; } else if (P[i][1] > maxy) { dz = P[i][2] - cz; - y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); + y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); if (y > maxy) maxy = y; } } // corners may have some points which are not covered - grow lengths if // necessary quite conservative (can be improved) - FCL_REAL dx, dy, u, t; - FCL_REAL a = std::sqrt((FCL_REAL)0.5); + CoalScalar dx, dy, u, t; + CoalScalar a = std::sqrt((CoalScalar)0.5); for (unsigned int i = 0; i < size_P; ++i) { if (P[i][0] > maxx) { if (P[i][1] > maxy) { @@ -428,7 +428,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = dx * a + dy * a; t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { maxx += u * a; maxy += u * a; @@ -439,7 +439,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = dx * a - dy * a; t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { maxx += u * a; miny -= u * a; @@ -452,7 +452,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = dy * a - dx * a; t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { minx -= u * a; maxy += u * a; @@ -463,7 +463,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, u = -dx * a - dy * a; t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) + (cz - P[i][2]) * (cz - P[i][2]); - u = u - std::sqrt(std::max(radsqr - t, 0)); + u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { minx -= u * a; miny -= u * a; @@ -474,8 +474,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, origin.noalias() = axes * Vec3f(minx, miny, cz); - l[0] = std::max(maxx - minx, 0); - l[1] = std::max(maxy - miny, 0); + l[0] = std::max(maxx - minx, 0); + l[1] = std::max(maxy - miny, 0); delete[] P; } @@ -490,7 +490,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL real_max = (std::numeric_limits::max)(); + CoalScalar real_max = (std::numeric_limits::max)(); Vec3f min_coord(real_max, real_max, real_max); Vec3f max_coord(-real_max, -real_max, -real_max); @@ -532,7 +532,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL real_max = (std::numeric_limits::max)(); + CoalScalar real_max = (std::numeric_limits::max)(); Vec3f min_coord(real_max, real_max, real_max); Vec3f max_coord(-real_max, -real_max, -real_max); @@ -583,27 +583,28 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, } void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, - Vec3f& center, FCL_REAL& radius) { + Vec3f& center, CoalScalar& radius) { Vec3f e1 = a - c; Vec3f e2 = b - c; - FCL_REAL e1_len2 = e1.squaredNorm(); - FCL_REAL e2_len2 = e2.squaredNorm(); + CoalScalar e1_len2 = e1.squaredNorm(); + CoalScalar e2_len2 = e2.squaredNorm(); Vec3f e3 = e1.cross(e2); - FCL_REAL e3_len2 = e3.squaredNorm(); + CoalScalar e3_len2 = e3.squaredNorm(); radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; radius = std::sqrt(radius) * 0.5; center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; } -static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, - unsigned int n, - const Vec3f& query) { +static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, + Triangle* ts, + unsigned int* indices, + unsigned int n, + const Vec3f& query) { bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL maxD = 0; + CoalScalar maxD = 0; for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle& t = ts[index]; @@ -612,7 +613,7 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, Triangle::index_type point_id = t[j]; const Vec3f& p = ps[point_id]; - FCL_REAL d = (p - query).squaredNorm(); + CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; } @@ -621,7 +622,7 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, Triangle::index_type point_id = t[j]; const Vec3f& p = ps2[point_id]; - FCL_REAL d = (p - query).squaredNorm(); + CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; } } @@ -630,24 +631,24 @@ static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, return std::sqrt(maxD); } -static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, - unsigned int* indices, - unsigned int n, - const Vec3f& query) { +static inline CoalScalar maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, + unsigned int* indices, + unsigned int n, + const Vec3f& query) { bool indirect_index = true; if (!indices) indirect_index = false; - FCL_REAL maxD = 0; + CoalScalar maxD = 0; for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Vec3f& p = ps[index]; - FCL_REAL d = (p - query).squaredNorm(); + CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; if (ps2) { const Vec3f& v = ps2[index]; - FCL_REAL d = (v - query).squaredNorm(); + CoalScalar d = (v - query).squaredNorm(); if (d > maxD) maxD = d; } } @@ -655,9 +656,9 @@ static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, return std::sqrt(maxD); } -FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, - const Vec3f& query) { +CoalScalar maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, + const Vec3f& query) { if (ts) return maximumDistance_mesh(ps, ps2, ts, indices, n, query); else diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp index 61006a385..deb74700a 100644 --- a/src/BVH/BV_fitter.cpp +++ b/src/BVH/BV_fitter.cpp @@ -47,7 +47,7 @@ static const double kIOS_RATIO = 1.5; static const double invSinA = 2; static const double cosA = sqrt(3.0) / 2.0; -static inline void axisFromEigen(Vec3f eigenV[3], FCL_REAL eigenS[3], +static inline void axisFromEigen(Vec3f eigenV[3], CoalScalar eigenS[3], Matrix3f& axes) { int min, mid, max; if (eigenS[0] > eigenS[1]) { @@ -87,7 +87,7 @@ void fit2(Vec3f* ps, OBB& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; Vec3f p1p2 = p1 - p2; - FCL_REAL len_p1p2 = p1p2.norm(); + CoalScalar len_p1p2 = p1p2.norm(); p1p2.normalize(); bv.axes.col(0).noalias() = p1p2; @@ -105,7 +105,7 @@ void fit3(Vec3f* ps, OBB& bv) { e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; - FCL_REAL len[3]; + CoalScalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); @@ -131,7 +131,7 @@ void fit6(Vec3f* ps, OBB& bv) { void fitn(Vec3f* ps, unsigned int n, OBB& bv) { Matrix3f M; Vec3f E[3]; - FCL_REAL s[3] = {0, 0, 0}; // three eigen values + CoalScalar s[3] = {0, 0, 0}; // three eigen values getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -156,7 +156,7 @@ void fit2(Vec3f* ps, RSS& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; bv.axes.col(0).noalias() = p1 - p2; - FCL_REAL len_p1p2 = bv.axes.col(0).norm(); + CoalScalar len_p1p2 = bv.axes.col(0).norm(); bv.axes.col(0) /= len_p1p2; generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2)); @@ -175,7 +175,7 @@ void fit3(Vec3f* ps, RSS& bv) { e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; - FCL_REAL len[3]; + CoalScalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); @@ -202,7 +202,7 @@ void fit6(Vec3f* ps, RSS& bv) { void fitn(Vec3f* ps, unsigned int n, RSS& bv) { Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors - FCL_REAL s[3] = {0, 0, 0}; + CoalScalar s[3] = {0, 0, 0}; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -233,22 +233,22 @@ void fit2(Vec3f* ps, kIOS& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; Vec3f p1p2 = p1 - p2; - FCL_REAL len_p1p2 = p1p2.norm(); + CoalScalar len_p1p2 = p1p2.norm(); p1p2.normalize(); Matrix3f& axes = bv.obb.axes; axes.col(0).noalias() = p1p2; generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2)); - FCL_REAL r0 = len_p1p2 * 0.5; + CoalScalar r0 = len_p1p2 * 0.5; bv.obb.extent << r0, 0, 0; bv.obb.To = (p1 + p2) * 0.5; bv.spheres[0].o = bv.obb.To; bv.spheres[0].r = r0; - FCL_REAL r1 = r0 * invSinA; - FCL_REAL r1cosA = r1 * cosA; + CoalScalar r1 = r0 * invSinA; + CoalScalar r1cosA = r1 * cosA; bv.spheres[1].r = r1; bv.spheres[2].r = r1; Vec3f delta = axes.col(1) * r1cosA; @@ -272,7 +272,7 @@ void fit3(Vec3f* ps, kIOS& bv) { e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; - FCL_REAL len[3]; + CoalScalar len[3]; len[0] = e[0].squaredNorm(); len[1] = e[1].squaredNorm(); len[2] = e[2].squaredNorm(); @@ -289,14 +289,14 @@ void fit3(Vec3f* ps, kIOS& bv) { bv.obb.extent); // compute radius and center - FCL_REAL r0; + CoalScalar r0; Vec3f center; circumCircleComputation(p1, p2, p3, center, r0); bv.spheres[0].o = center; bv.spheres[0].r = r0; - FCL_REAL r1 = r0 * invSinA; + CoalScalar r1 = r0 * invSinA; Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA); bv.spheres[1].r = r1; @@ -308,7 +308,7 @@ void fit3(Vec3f* ps, kIOS& bv) { void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { Matrix3f M; Vec3f E[3]; - FCL_REAL s[3] = {0, 0, 0}; // three eigen values; + CoalScalar s[3] = {0, 0, 0}; // three eigen values; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); @@ -321,7 +321,7 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { // get center and extension const Vec3f& center = bv.obb.To; const Vec3f& extent = bv.obb.extent; - FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); + CoalScalar r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); // decide the k in kIOS if (extent[0] > kIOS_RATIO * extent[2]) { @@ -336,12 +336,12 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { bv.spheres[0].r = r0; if (bv.num_spheres >= 3) { - FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; + CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; - FCL_REAL r11 = 0, r12 = 0; + CoalScalar r11 = 0, r12 = 0; r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o); r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o); bv.spheres[1].o += axes.col(2) * (-r10 + r11); @@ -352,7 +352,7 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { } if (bv.num_spheres >= 5) { - FCL_REAL r10 = bv.spheres[1].r; + CoalScalar r10 = bv.spheres[1].r; Vec3f delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - @@ -360,7 +360,7 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; - FCL_REAL r21 = 0, r22 = 0; + CoalScalar r21 = 0, r22 = 0; r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o); r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o); @@ -481,9 +481,9 @@ OBB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBB bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - FCL_REAL s[3]; // three eigen values + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors + CoalScalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -503,7 +503,7 @@ OBBRSS BVFitter::fit(unsigned int* primitive_indices, OBBRSS bv; Matrix3f M; Vec3f E[3]; - FCL_REAL s[3]; + CoalScalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -516,8 +516,8 @@ OBBRSS BVFitter::fit(unsigned int* primitive_indices, num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent); Vec3f origin; - FCL_REAL l[2]; - FCL_REAL r; + CoalScalar l[2]; + CoalScalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r); @@ -534,9 +534,9 @@ RSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { RSS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors - FCL_REAL s[3]; // three eigen values + Matrix3f M; // row first matrix + Vec3f E[3]; // row first eigen-vectors + CoalScalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); @@ -545,8 +545,8 @@ RSS BVFitter::fit(unsigned int* primitive_indices, // set rss origin, rectangle size and radius Vec3f origin; - FCL_REAL l[2]; - FCL_REAL r; + CoalScalar l[2]; + CoalScalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r); @@ -565,7 +565,7 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors - FCL_REAL s[3]; + CoalScalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -580,8 +580,8 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, const Vec3f& center = bv.obb.To; const Vec3f& extent = bv.obb.extent; - FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, - primitive_indices, num_primitives, center); + CoalScalar r0 = maximumDistance(vertices, prev_vertices, tri_indices, + primitive_indices, num_primitives, center); // decide k in kIOS if (extent[0] > kIOS_RATIO * extent[2]) { @@ -596,15 +596,15 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, bv.spheres[0].r = r0; if (bv.num_spheres >= 3) { - FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; + CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; - FCL_REAL r11 = + CoalScalar r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o); - FCL_REAL r12 = + CoalScalar r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o); @@ -616,7 +616,7 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, } if (bv.num_spheres >= 5) { - FCL_REAL r10 = bv.spheres[1].r; + CoalScalar r10 = bv.spheres[1].r; Vec3f delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - @@ -624,7 +624,7 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; - FCL_REAL r21 = 0, r22 = 0; + CoalScalar r21 = 0, r22 = 0; r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o); r22 = maximumDistance(vertices, prev_vertices, tri_indices, diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp index 1a78e332c..48c28433c 100644 --- a/src/BVH/BV_splitter.cpp +++ b/src/BVH/BV_splitter.cpp @@ -83,7 +83,7 @@ void computeSplitVector(const OBBRSS& bv, Vec3f& split_vector) { } template -void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value) { +void computeSplitValue_bvcenter(const BV& bv, CoalScalar& split_value) { Vec3f center = bv.center(); split_value = center[0]; } @@ -92,7 +92,8 @@ template void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, - const Vec3f& split_vector, FCL_REAL& split_value) { + const Vec3f& split_vector, + CoalScalar& split_value) { if (type == BVH_MODEL_TRIANGLES) { Vec3f c(Vec3f::Zero()); @@ -106,7 +107,7 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, } split_value = c.dot(split_vector) / (3 * num_primitives); } else if (type == BVH_MODEL_POINTCLOUD) { - FCL_REAL sum = 0; + CoalScalar sum = 0; for (unsigned int i = 0; i < num_primitives; ++i) { const Vec3f& p = vertices[primitive_indices[i]]; sum += p.dot(split_vector); @@ -121,8 +122,8 @@ void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, const Vec3f& split_vector, - FCL_REAL& split_value) { - std::vector proj(num_primitives); + CoalScalar& split_value) { + std::vector proj(num_primitives); if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index 124f8c671..ca1ba3249 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -183,7 +183,7 @@ bool SSaPCollisionManager::checkDis( typename std::vector::const_iterator pos_start, typename std::vector::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { while (pos_start < pos_end) { if (*pos_start != obj) // no distance between the same object { @@ -256,18 +256,18 @@ void SSaPCollisionManager::distance(CollisionObject* obj, callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== bool SSaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { static const unsigned int CUTOFF = 100; Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; Vec3f dummy_vector = obj->getAABB().max_; - if (min_dist < (std::numeric_limits::max)()) + if (min_dist < (std::numeric_limits::max)()) dummy_vector += Vec3f(min_dist, min_dist, min_dist); typename std::vector::const_iterator pos_start1 = @@ -284,7 +284,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, objs_z.end(); int status = 1; - FCL_REAL old_min_distance; + CoalScalar old_min_distance; while (1) { old_min_distance = min_dist; @@ -328,7 +328,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, if (dist_res) return true; if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) + if (old_min_distance < (std::numeric_limits::max)()) break; else { // from infinity to a finite one, only need one additional loop @@ -341,7 +341,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, { if (dummy_vector.isApprox( obj->getAABB().max_, - std::numeric_limits::epsilon() * 100)) + std::numeric_limits::epsilon() * 100)) dummy_vector = dummy_vector + delta; else dummy_vector = dummy_vector * 2 - obj->getAABB().max_; @@ -367,12 +367,12 @@ int SSaPCollisionManager::selectOptimalAxis( typename std::vector::const_iterator& it_beg, typename std::vector::const_iterator& it_end) { /// simple sweep and prune method - FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - - (objs_x[0])->getAABB().min_[0]; - FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - - (objs_y[0])->getAABB().min_[1]; - FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - - (objs_z[0])->getAABB().min_[2]; + CoalScalar delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - + (objs_x[0])->getAABB().min_[0]; + CoalScalar delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - + (objs_y[0])->getAABB().min_[1]; + CoalScalar delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - + (objs_z[0])->getAABB().min_[2]; int axis = 0; if (delta_y > delta_x && delta_y > delta_z) @@ -453,7 +453,7 @@ void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const { typename std::vector::const_iterator it, it_end; selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (; it != it_end; ++it) { if (distance_(*it, callback, min_dist)) return; } @@ -498,7 +498,7 @@ void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); typename std::vector::const_iterator it, end; if (this->size() < other_manager->size()) { for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it) diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 61f039d53..9e0d64c76 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -118,15 +118,15 @@ void SaPCollisionManager::registerObjects( obj_aabb_map[other_objs[i]] = sapaabb; } - FCL_REAL scale[3]; + CoalScalar scale[3]; for (int coord = 0; coord < 3; ++coord) { std::sort( endpoints.begin(), endpoints.end(), - std::bind(std::less(), - std::bind(static_cast( + std::bind(std::less(), + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, coord), - std::bind(static_cast( + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, coord))); @@ -203,7 +203,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) { } else // otherwise, find the correct location in the list and insert { EndPoint* curr_lo = new_sap->lo; - FCL_REAL curr_lo_val = curr_lo->getVal()[coord]; + CoalScalar curr_lo_val = curr_lo->getVal()[coord]; while ((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != nullptr)) current = current->next[coord]; @@ -231,7 +231,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) { current = new_sap->lo; EndPoint* curr_hi = new_sap->hi; - FCL_REAL curr_hi_val = curr_hi->getVal()[coord]; + CoalScalar curr_hi_val = curr_hi->getVal()[coord]; if (coord == 0) { while ((current->getVal()[coord] < curr_hi_val) && @@ -273,7 +273,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj) { void SaPCollisionManager::setup() { if (size() == 0) return; - FCL_REAL scale[3]; + CoalScalar scale[3]; scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1); scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); @@ -506,8 +506,8 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, int axis = optimal_axis; const AABB& obj_aabb = obj->getAABB(); - FCL_REAL min_val = obj_aabb.min_[axis]; - // FCL_REAL max_val = obj_aabb.max_[axis]; + CoalScalar min_val = obj_aabb.min_[axis]; + // CoalScalar max_val = obj_aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; @@ -519,11 +519,11 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, // iteration linearly const auto res_it = std::upper_bound( velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast( + std::bind(std::less(), + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast( + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, axis))); @@ -584,11 +584,11 @@ void SaPCollisionManager::collide(CollisionObject* obj, //============================================================================== bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); - if (min_dist < (std::numeric_limits::max)()) { + if (min_dist < (std::numeric_limits::max)()) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } @@ -596,14 +596,14 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, int axis = optimal_axis; int status = 1; - FCL_REAL old_min_distance; + CoalScalar old_min_distance; EndPoint* start_pos = elist[axis]; while (1) { old_min_distance = min_dist; - FCL_REAL min_val = aabb.min_[axis]; - // FCL_REAL max_val = aabb.max_[axis]; + CoalScalar min_val = aabb.min_[axis]; + // CoalScalar max_val = aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; @@ -613,11 +613,11 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, const auto res_it = std::upper_bound( velist[axis].begin(), velist[axis].end(), &dummy, - std::bind(std::less(), - std::bind(static_cast( + std::bind(std::less(), + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_1, axis), - std::bind(static_cast( + std::bind(static_cast( &EndPoint::getVal), std::placeholders::_2, axis))); @@ -652,7 +652,7 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, } if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) + if (old_min_distance < (std::numeric_limits::max)()) break; else { if (min_dist < old_min_distance) { @@ -679,7 +679,7 @@ void SaPCollisionManager::distance(CollisionObject* obj, callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } @@ -706,7 +706,7 @@ void SaPCollisionManager::distance(DistanceCallBackBase* callback) const { this->enable_tested_set_ = true; this->tested_set.clear(); - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { if (distance_((*it)->obj, callback, min_dist)) break; @@ -757,7 +757,7 @@ void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { @@ -795,7 +795,7 @@ Vec3f& SaPCollisionManager::EndPoint::getVal() { } //============================================================================== -FCL_REAL SaPCollisionManager::EndPoint::getVal(int i) const { +CoalScalar SaPCollisionManager::EndPoint::getVal(int i) const { if (minmax) return aabb->cached.max_[i]; else @@ -803,7 +803,7 @@ FCL_REAL SaPCollisionManager::EndPoint::getVal(int i) const { } //============================================================================== -FCL_REAL& SaPCollisionManager::EndPoint::getVal(int i) { +CoalScalar& SaPCollisionManager::EndPoint::getVal(int i) { if (minmax) return aabb->cached.max_[i]; else diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index f1a241d71..7d83629bf 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -100,7 +100,7 @@ void NaiveCollisionManager::distance(CollisionObject* obj, callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (auto* obj2 : objs) { if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { if ((*callback)(obj, obj2, min_dist)) return; @@ -131,7 +131,7 @@ void NaiveCollisionManager::distance(DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (typename std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) { @@ -182,7 +182,7 @@ void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (auto* obj1 : objs) { for (auto* obj2 : other_manager->objs) { if (obj1->getAABB().distance(obj2->getAABB()) < min_dist) { diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index afb765e14..05ac31513 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -149,7 +149,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); @@ -169,8 +169,8 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, AABB aabb2; convertBV(root2_bv, tf2, aabb2); - FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); - FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); + CoalScalar d1 = aabb2.distance(root1->children[0]->bv); + CoalScalar d2 = aabb2.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -206,7 +206,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, AABB aabb2; convertBV(child_bv, tf2, aabb2); - FCL_REAL d = root1->bv.distance(aabb2); + CoalScalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(root1, tree2, child, child_bv, tf2, callback, @@ -236,7 +236,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); @@ -407,7 +407,7 @@ bool selfCollisionRecurse( //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); @@ -416,8 +416,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { - FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv); - FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv); + CoalScalar d1 = root2->bv.distance(root1->children[0]->bv); + CoalScalar d2 = root2->bv.distance(root1->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -441,8 +441,8 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, } } } else { - FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv); - FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv); + CoalScalar d1 = root1->bv.distance(root2->children[0]->bv); + CoalScalar d2 = root1->bv.distance(root2->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -473,14 +473,14 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, DistanceCallBackBase* callback, - FCL_REAL& min_dist) { + CoalScalar& min_dist) { if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } - FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv); - FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv); + CoalScalar d1 = query->getAABB().distance(root->children[0]->bv); + CoalScalar d2 = query->getAABB().distance(root->children[1]->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -509,7 +509,7 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, //============================================================================== bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root->isLeaf()) return false; if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true; @@ -593,7 +593,7 @@ void DynamicAABBTreeCollisionManager::setup() { size_t height = dtree.getMaxHeight(); - if (((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0)) < + if (((CoalScalar)height - std::log((CoalScalar)num) / std::log(2.0)) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else @@ -690,7 +690,7 @@ void DynamicAABBTreeCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { #if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { @@ -724,7 +724,7 @@ void DynamicAABBTreeCollisionManager::distance( DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback, min_dist); } @@ -749,7 +749,7 @@ void DynamicAABBTreeCollisionManager::distance( DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree::distanceRecurse( dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist); } diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index d4f44f205..95f440c65 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -149,7 +149,7 @@ bool distanceRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { @@ -171,8 +171,8 @@ bool distanceRecurse_( AABB aabb2; convertBV(root2_bv, tf2, aabb2); - FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); + CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv); + CoalScalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -208,7 +208,7 @@ bool distanceRecurse_( AABB aabb2; convertBV(child_bv, tf2, aabb2); - FCL_REAL d = root1->bv.distance(aabb2); + CoalScalar d = root1->bv.distance(aabb2); if (d < min_dist) { if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, @@ -307,7 +307,7 @@ bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, - size_t root2_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) { + size_t root2_id, DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = @@ -320,8 +320,8 @@ bool distanceRecurse( if (root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { - FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); - FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); + CoalScalar d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); + CoalScalar d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -349,8 +349,8 @@ bool distanceRecurse( } } } else { - FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); - FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); + CoalScalar d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); + CoalScalar d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -386,15 +386,15 @@ bool distanceRecurse( bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, DistanceCallBackBase* callback, - FCL_REAL& min_dist) { + CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return (*callback)(root_obj, query, min_dist); } - FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv); - FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv); + CoalScalar d1 = query->getAABB().distance((nodes + root->children[0])->bv); + CoalScalar d2 = query->getAABB().distance((nodes + root->children[1])->bv); if (d2 < d1) { if (d2 < min_dist) { @@ -424,7 +424,7 @@ bool distanceRecurse( //============================================================================== bool selfDistanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, - size_t root_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) { + size_t root_id, DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; if (root->isLeaf()) return false; @@ -462,7 +462,7 @@ bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, - DistanceCallBackBase* callback, FCL_REAL& min_dist) { + DistanceCallBackBase* callback, CoalScalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), callback, min_dist); @@ -545,7 +545,7 @@ void DynamicAABBTreeArrayCollisionManager::setup() { int height = (int)dtree.getMaxHeight(); - if ((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0) < + if ((CoalScalar)height - std::log((CoalScalar)num) / std::log(2.0) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else @@ -641,7 +641,7 @@ void DynamicAABBTreeArrayCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); switch (obj->collisionGeometry()->getNodeType()) { #if COAL_HAVE_OCTOMAP case GEOM_OCTREE: { @@ -676,7 +676,7 @@ void DynamicAABBTreeArrayCollisionManager::distance( DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree_array::selfDistanceRecurse( dtree.getNodes(), dtree.getRoot(), callback, min_dist); } @@ -702,7 +702,7 @@ void DynamicAABBTreeArrayCollisionManager::distance( DynamicAABBTreeArrayCollisionManager* other_manager = static_cast(other_manager_); if ((size() == 0) || (other_manager->size() == 0)) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); detail::dynamic_AABB_tree_array::distanceRecurse( dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), callback, min_dist); diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 9107335dc..8a4313151 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -362,25 +362,25 @@ void IntervalTreeCollisionManager::distance( CollisionObject* obj, DistanceCallBackBase* callback) const { callback->init(); if (size() == 0) return; - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); distance_(obj, callback, min_dist); } //============================================================================== bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { static const unsigned int CUTOFF = 100; Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); - if (min_dist < (std::numeric_limits::max)()) { + if (min_dist < (std::numeric_limits::max)()) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } int status = 1; - FCL_REAL old_min_distance; + CoalScalar old_min_distance; while (1) { bool dist_res = false; @@ -425,7 +425,7 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, results2.clear(); if (status == 1) { - if (old_min_distance < (std::numeric_limits::max)()) + if (old_min_distance < (std::numeric_limits::max)()) break; else { if (min_dist < old_min_distance) { @@ -455,9 +455,9 @@ void IntervalTreeCollisionManager::collide( std::set active; std::set > overlap; size_t n = endpoints[0].size(); - FCL_REAL diff_x = endpoints[0][0].value - endpoints[0][n - 1].value; - FCL_REAL diff_y = endpoints[1][0].value - endpoints[1][n - 1].value; - FCL_REAL diff_z = endpoints[2][0].value - endpoints[2][n - 1].value; + CoalScalar diff_x = endpoints[0][0].value - endpoints[0][n - 1].value; + CoalScalar diff_y = endpoints[1][0].value - endpoints[1][n - 1].value; + CoalScalar diff_z = endpoints[2][0].value - endpoints[2][n - 1].value; int axis = 0; if (diff_y > diff_x && diff_y > diff_z) @@ -508,7 +508,7 @@ void IntervalTreeCollisionManager::distance( this->enable_tested_set_ = true; this->tested_set.clear(); - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); for (size_t i = 0; i < endpoints[0].size(); ++i) if (distance_(endpoints[0][i].obj, callback, min_dist)) break; @@ -556,7 +556,7 @@ void IntervalTreeCollisionManager::distance( return; } - FCL_REAL min_dist = (std::numeric_limits::max)(); + CoalScalar min_dist = (std::numeric_limits::max)(); if (this->size() < other_manager->size()) { for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) @@ -603,7 +603,7 @@ bool IntervalTreeCollisionManager::checkDist( typename std::deque::const_iterator pos_start, typename std::deque::const_iterator pos_end, CollisionObject* obj, DistanceCallBackBase* callback, - FCL_REAL& min_dist) const { + CoalScalar& min_dist) const { while (pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); if (ivl->obj != obj) { @@ -635,8 +635,8 @@ bool IntervalTreeCollisionManager::EndPoint::operator<( } //============================================================================== -IntervalTreeCollisionManager::SAPInterval::SAPInterval(FCL_REAL low_, - FCL_REAL high_, +IntervalTreeCollisionManager::SAPInterval::SAPInterval(CoalScalar low_, + CoalScalar high_, CollisionObject* obj_) : detail::SimpleInterval() { this->low = low_; diff --git a/src/broadphase/default_broadphase_callbacks.cpp b/src/broadphase/default_broadphase_callbacks.cpp index 222e77b85..8e54a47e2 100644 --- a/src/broadphase/default_broadphase_callbacks.cpp +++ b/src/broadphase/default_broadphase_callbacks.cpp @@ -64,7 +64,7 @@ bool CollisionCallBackDefault::collide(CollisionObject* o1, } bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* data, FCL_REAL& dist) { + void* data, CoalScalar& dist) { assert(data != nullptr); auto* cdata = static_cast(data); const DistanceRequest& request = cdata->request; @@ -85,7 +85,7 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, } bool DistanceCallBackDefault::distance(CollisionObject* o1, CollisionObject* o2, - FCL_REAL& dist) { + CoalScalar& dist) { return defaultDistanceFunction(o1, o2, &data, dist); } diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp index 63fdd8d76..34a2326c4 100644 --- a/src/broadphase/detail/interval_tree.cpp +++ b/src/broadphase/detail/interval_tree.cpp @@ -52,13 +52,13 @@ IntervalTree::IntervalTree() { invalid_node; invalid_node->red = false; invalid_node->key = invalid_node->high = invalid_node->max_high = - -(std::numeric_limits::max)(); + -(std::numeric_limits::max)(); invalid_node->stored_interval = nullptr; root = new IntervalTreeNode; root->parent = root->left = root->right = invalid_node; root->key = root->high = root->max_high = - (std::numeric_limits::max)(); + (std::numeric_limits::max)(); root->red = false; root->stored_interval = nullptr; @@ -381,7 +381,7 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) { /// @brief y should not be invalid_node in this case /// y is the node to splice out and x is its child if (y != z) { - y->max_high = -(std::numeric_limits::max)(); + y->max_high = -(std::numeric_limits::max)(); y->left = z->left; y->right = z->right; y->parent = z->parent; @@ -409,7 +409,7 @@ SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) { //============================================================================== /// @brief returns 1 if the intervals overlap, and 0 otherwise -bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) { +bool overlap(CoalScalar a1, CoalScalar a2, CoalScalar b1, CoalScalar b2) { if (a1 <= b1) { return (b1 <= a2); } else { @@ -418,7 +418,8 @@ bool overlap(FCL_REAL a1, FCL_REAL a2, FCL_REAL b1, FCL_REAL b2) { } //============================================================================== -std::deque IntervalTree::query(FCL_REAL low, FCL_REAL high) { +std::deque IntervalTree::query(CoalScalar low, + CoalScalar high) { std::deque result_stack; IntervalTreeNode* x = root->left; bool run = (x != invalid_node); diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index ba45f0e9b..dd92fb7c5 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -45,7 +45,7 @@ namespace coal { namespace detail { //============================================================================== -SpatialHash::SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) +SpatialHash::SpatialHash(const AABB& scene_limit_, CoalScalar cell_size_) : cell_size(cell_size_), scene_limit(scene_limit_) { width[0] = static_cast(std::ceil(scene_limit.width() / cell_size)); diff --git a/src/collision.cpp b/src/collision.cpp index 5b597f453..57b923045 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -69,7 +69,7 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { // If security margin is set to -infinity, return that there is no collision - if (request.security_margin == -std::numeric_limits::infinity()) { + if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); return false; } @@ -161,7 +161,7 @@ std::size_t ComputeCollision::run(const Transform3f& tf1, const CollisionRequest& request, CollisionResult& result) const { // If security margin is set to -infinity, return that there is no collision - if (request.security_margin == -std::numeric_limits::infinity()) { + if (request.security_margin == -std::numeric_limits::infinity()) { result.clear(); return false; } diff --git a/src/collision_node.cpp b/src/collision_node.cpp index 93caa018e..77c6adb04 100644 --- a/src/collision_node.cpp +++ b/src/collision_node.cpp @@ -41,10 +41,10 @@ namespace coal { void checkResultLowerBound(const CollisionResult& result, - FCL_REAL sqrDistLowerBound) { + CoalScalar sqrDistLowerBound) { COAL_UNUSED_VARIABLE(result); - const FCL_REAL dummy_precision = - std::sqrt(Eigen::NumTraits::epsilon()); + const CoalScalar dummy_precision = + std::sqrt(Eigen::NumTraits::epsilon()); COAL_UNUSED_VARIABLE(dummy_precision); if (sqrDistLowerBound == 0) { COAL_ASSERT(result.distance_lower_bound <= dummy_precision, @@ -65,7 +65,7 @@ void collide(CollisionTraversalNodeBase* node, const CollisionRequest& request, if (front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, request, result, front_list); } else { - FCL_REAL sqrDistLowerBound = 0; + CoalScalar sqrDistLowerBound = 0; if (recursive) collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound); else diff --git a/src/contact_patch/contact_patch_solver.cpp b/src/contact_patch/contact_patch_solver.cpp index 541915bf9..bf6418d51 100644 --- a/src/contact_patch/contact_patch_solver.cpp +++ b/src/contact_patch/contact_patch_solver.cpp @@ -46,7 +46,7 @@ template (shape); getShapeSupportSet<_SupportOptions>(shape_, support_set, hint, support_data, num_sampled_supports, tol); diff --git a/src/distance.cpp b/src/distance.cpp index a54efa3c6..97dc1f5e5 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -49,16 +49,16 @@ DistanceFunctionMatrix& getDistanceFunctionLookTable() { return table; } -FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar distance(const CollisionObject* o1, const CollisionObject* o2, + const DistanceRequest& request, DistanceResult& result) { return distance(o1->collisionGeometryPtr(), o1->getTransform(), o2->collisionGeometryPtr(), o2->getTransform(), request, result); } -FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar distance(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const DistanceRequest& request, DistanceResult& result) { GJKSolver solver(request); const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); @@ -68,7 +68,7 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); - FCL_REAL res = (std::numeric_limits::max)(); + CoalScalar res = (std::numeric_limits::max)(); if (object_type1 == OT_GEOM && (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) { @@ -135,10 +135,10 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1, func = looktable.distance_matrix[node_type1][node_type2]; } -FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const { - FCL_REAL res; +CoalScalar ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) const { + CoalScalar res; if (swap_geoms) { res = func(o2, tf2, o1, tf1, &solver, request, result); @@ -156,13 +156,13 @@ FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, return res; } -FCL_REAL ComputeDistance::operator()(const Transform3f& tf1, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) const { +CoalScalar ComputeDistance::operator()(const Transform3f& tf1, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) const { solver.set(request); - FCL_REAL res; + CoalScalar res; if (request.enable_timings) { Timer timer; res = run(tf1, tf2, request, result); diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp index 39f8a792a..f4dbc8061 100644 --- a/src/distance/box_halfspace.cpp +++ b/src/distance/box_halfspace.cpp @@ -48,20 +48,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Box& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp index f0c45e3fd..abce82fb7 100644 --- a/src/distance/box_plane.cpp +++ b/src/distance/box_plane.cpp @@ -47,27 +47,27 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver*, const bool, + Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Box& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver*, const bool, + Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Plane& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp index 220c20eb9..aafda9c17 100644 --- a/src/distance/box_sphere.cpp +++ b/src/distance/box_sphere.cpp @@ -47,27 +47,23 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Box& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::boxSphereDistance(s1, tf1, s2, tf2, p1, p2, normal); } template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Sphere& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::boxSphereDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp index c229f80e7..b09a35222 100644 --- a/src/distance/capsule_capsule.cpp +++ b/src/distance/capsule_capsule.cpp @@ -48,7 +48,7 @@ struct GJKSolver; namespace internal { /// Clamp num / denom in [0, 1] -FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) { +CoalScalar clamp(const CoalScalar& num, const CoalScalar& denom) { assert(denom >= 0.); if (num <= 0.) return 0.; @@ -59,8 +59,8 @@ FCL_REAL clamp(const FCL_REAL& num, const FCL_REAL& denom) { } /// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd -void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n, - const FCL_REAL& s_d, const Vec3f& d) { +void clamped_linear(Vec3f& a_sd, const Vec3f& a, const CoalScalar& s_n, + const CoalScalar& s_d, const Vec3f& d) { assert(s_d >= 0.); if (s_n <= 0.) a_sd = a; @@ -77,23 +77,23 @@ void clamped_linear(Vec3f& a_sd, const Vec3f& a, const FCL_REAL& s_n, /// @param wp1, wp2: witness points on the capsules /// @param normal: normal pointing from capsule1 to capsule2 template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& wp1, Vec3f& wp2, Vec3f& normal) { const Capsule* capsule1 = static_cast(o1); const Capsule* capsule2 = static_cast(o2); - FCL_REAL EPSILON = std::numeric_limits::epsilon() * 100; + CoalScalar EPSILON = std::numeric_limits::epsilon() * 100; // We assume that capsules are centered at the origin. const coal::Vec3f& c1 = tf1.getTranslation(); const coal::Vec3f& c2 = tf2.getTranslation(); // We assume that capsules are oriented along z-axis. - FCL_REAL halfLength1 = capsule1->halfLength; - FCL_REAL halfLength2 = capsule2->halfLength; - FCL_REAL radius1 = (capsule1->radius + capsule1->getSweptSphereRadius()); - FCL_REAL radius2 = (capsule2->radius + capsule2->getSweptSphereRadius()); + CoalScalar halfLength1 = capsule1->halfLength; + CoalScalar halfLength2 = capsule2->halfLength; + CoalScalar radius1 = (capsule1->radius + capsule1->getSweptSphereRadius()); + CoalScalar radius2 = (capsule2->radius + capsule2->getSweptSphereRadius()); // direction of capsules // ||d1|| = 2 * halfLength1 const coal::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); @@ -104,11 +104,11 @@ FCL_REAL ShapeShapeDistance( const coal::Vec3f p1 = c1 - d1 / 2; const coal::Vec3f p2 = c2 - d2 / 2; const coal::Vec3f r = p1 - p2; - FCL_REAL a = d1.dot(d1); - FCL_REAL b = d1.dot(d2); - FCL_REAL c = d1.dot(r); - FCL_REAL e = d2.dot(d2); - FCL_REAL f = d2.dot(r); + CoalScalar a = d1.dot(d1); + CoalScalar b = d1.dot(d2); + CoalScalar c = d1.dot(r); + CoalScalar e = d2.dot(d2); + CoalScalar f = d2.dot(r); // S1 is parametrized by the equation p1 + s * d1 // S2 is parametrized by the equation p2 + t * d2 @@ -127,10 +127,10 @@ FCL_REAL ShapeShapeDistance( w2 = p2; } else { // Always non-negative, equal 0 if the segments are colinear - FCL_REAL denom = fmax(a * e - b * b, 0); + CoalScalar denom = fmax(a * e - b * b, 0); - FCL_REAL s; - FCL_REAL t; + CoalScalar s; + CoalScalar t; if (denom > EPSILON) { s = clamp((b * f - c * e), denom); t = b * s + f; @@ -152,7 +152,7 @@ FCL_REAL ShapeShapeDistance( } // witness points achieving the distance between the two segments - FCL_REAL distance = (w1 - w2).norm(); + CoalScalar distance = (w1 - w2).norm(); // capsule spcecific distance computation distance = distance - (radius1 + radius2); diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp index 15daa03d5..3d794ba23 100644 --- a/src/distance/capsule_halfspace.cpp +++ b/src/distance/capsule_halfspace.cpp @@ -47,20 +47,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Capsule& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp index 9644e43e7..35c1b8ee3 100644 --- a/src/distance/capsule_plane.cpp +++ b/src/distance/capsule_plane.cpp @@ -47,20 +47,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Capsule& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp index 39649095a..6a3430d11 100644 --- a/src/distance/cone_halfspace.cpp +++ b/src/distance/cone_halfspace.cpp @@ -47,20 +47,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Cone& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp index ddd863ff5..350552fe0 100644 --- a/src/distance/cone_plane.cpp +++ b/src/distance/cone_plane.cpp @@ -47,27 +47,23 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Cone& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Plane& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 22fc88ad4..55acbfa73 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -43,20 +43,20 @@ namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const ConvexBase& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp index e2ece56ed..b0514489c 100644 --- a/src/distance/convex_plane.cpp +++ b/src/distance/convex_plane.cpp @@ -43,20 +43,20 @@ namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const ConvexBase& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp index 17b599908..3c06d8c8b 100644 --- a/src/distance/cylinder_halfspace.cpp +++ b/src/distance/cylinder_halfspace.cpp @@ -47,20 +47,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Cylinder& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp index fd1e31b79..125d3d4e0 100644 --- a/src/distance/cylinder_plane.cpp +++ b/src/distance/cylinder_plane.cpp @@ -47,20 +47,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Cylinder& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp index 836eb493f..5dc3e8b73 100644 --- a/src/distance/ellipsoid_halfspace.cpp +++ b/src/distance/ellipsoid_halfspace.cpp @@ -45,20 +45,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Ellipsoid& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp index 77d7cd2e9..169479041 100644 --- a/src/distance/ellipsoid_plane.cpp +++ b/src/distance/ellipsoid_plane.cpp @@ -44,20 +44,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Ellipsoid& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp index 260b12ec0..07648d704 100644 --- a/src/distance/halfspace_halfspace.cpp +++ b/src/distance/halfspace_halfspace.cpp @@ -44,7 +44,7 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp index 096b8ea40..abea962c3 100644 --- a/src/distance/halfspace_plane.cpp +++ b/src/distance/halfspace_plane.cpp @@ -44,7 +44,7 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { @@ -54,13 +54,13 @@ FCL_REAL ShapeShapeDistance( } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Plane& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - FCL_REAL distance = + CoalScalar distance = details::halfspacePlaneDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp index a716999e8..98b23b76e 100644 --- a/src/distance/plane_plane.cpp +++ b/src/distance/plane_plane.cpp @@ -44,12 +44,10 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +CoalScalar ShapeShapeDistance( + const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Plane& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); return details::planePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp index d02101177..2724700f9 100644 --- a/src/distance/sphere_capsule.cpp +++ b/src/distance/sphere_capsule.cpp @@ -44,7 +44,7 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { @@ -54,13 +54,13 @@ FCL_REAL ShapeShapeDistance( } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Capsule& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::sphereCapsuleDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp index c97bec1d5..932810809 100644 --- a/src/distance/sphere_cylinder.cpp +++ b/src/distance/sphere_cylinder.cpp @@ -47,7 +47,7 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { @@ -57,13 +57,13 @@ FCL_REAL ShapeShapeDistance( } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Cylinder& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::sphereCylinderDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp index 3f3d580cb..e0fdbc64a 100644 --- a/src/distance/sphere_halfspace.cpp +++ b/src/distance/sphere_halfspace.cpp @@ -47,20 +47,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Sphere& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp index a3d5a262b..c28df4d35 100644 --- a/src/distance/sphere_plane.cpp +++ b/src/distance/sphere_plane.cpp @@ -47,20 +47,20 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const Sphere& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index ea60685ad..365f8af5e 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -53,7 +53,7 @@ struct GJKSolver; namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index a74cdc8ba..f4220b25e 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -43,20 +43,20 @@ namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::halfspaceDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp index 89b60f7a9..8e3ba7e29 100644 --- a/src/distance/triangle_plane.cpp +++ b/src/distance/triangle_plane.cpp @@ -43,20 +43,20 @@ namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const TriangleP& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::planeDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp index aed689961..8e7e1ce72 100644 --- a/src/distance/triangle_sphere.cpp +++ b/src/distance/triangle_sphere.cpp @@ -43,20 +43,20 @@ namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { const TriangleP& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); - const FCL_REAL distance = + const CoalScalar distance = details::sphereTriangleDistance(s2, tf2, s1, tf1, p2, p1, normal); normal = -normal; return distance; } template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp index 602e9efb8..ae737e6dd 100644 --- a/src/distance/triangle_triangle.cpp +++ b/src/distance/triangle_triangle.cpp @@ -43,7 +43,7 @@ namespace coal { namespace internal { template <> -FCL_REAL ShapeShapeDistance( +CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* solver, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { @@ -84,10 +84,10 @@ FCL_REAL ShapeShapeDistance( // Retrieve witness points and normal solver->gjk.getWitnessPointsAndNormal(solver->minkowski_difference, p1, p2, normal); - FCL_REAL distance = solver->gjk.distance; + CoalScalar distance = solver->gjk.distance; if (gjk_status == details::GJK::Collision) { - FCL_REAL penetrationDepth = + CoalScalar penetrationDepth = details::computePenetration(t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal); distance = -penetrationDepth; } else { diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index 4f37096b9..5b482f298 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -48,10 +48,10 @@ namespace coal { #ifdef COAL_HAS_OCTOMAP template -FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const DistanceRequest& request, - DistanceResult& result) { +CoalScalar Distance(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; typename TraversalTraitsDistance::CollisionTraversal_t node; const TypeA* obj1 = static_cast(o1); @@ -66,7 +66,7 @@ FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1, #endif -COAL_LOCAL FCL_REAL distance_function_not_implemented( +COAL_LOCAL CoalScalar distance_function_not_implemented( const CollisionGeometry* o1, const Transform3f& /*tf1*/, const CollisionGeometry* o2, const Transform3f& /*tf2*/, const GJKSolver* /*nsolver*/, const DistanceRequest& /*request*/, @@ -84,11 +84,12 @@ COAL_LOCAL FCL_REAL distance_function_not_implemented( template struct COAL_LOCAL BVHShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; MeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); @@ -108,13 +109,13 @@ namespace details { template -FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar orientedBVHShapeDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; OrientedMeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); @@ -130,11 +131,12 @@ FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, template struct COAL_LOCAL BVHShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodeRSS, RSS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); @@ -143,11 +145,12 @@ struct COAL_LOCAL BVHShapeDistancer { template struct COAL_LOCAL BVHShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodekIOS, kIOS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); @@ -156,11 +159,12 @@ struct COAL_LOCAL BVHShapeDistancer { template struct COAL_LOCAL BVHShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedBVHShapeDistance< MeshShapeDistanceTraversalNodeOBBRSS, OBBRSS, T_SH>( o1, tf1, o2, tf2, nsolver, request, result); @@ -169,11 +173,12 @@ struct COAL_LOCAL BVHShapeDistancer { template struct COAL_LOCAL HeightFieldShapeDistancer { - static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { + static CoalScalar distance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const DistanceRequest& request, + DistanceResult& result) { COAL_UNUSED_VARIABLE(o1); COAL_UNUSED_VARIABLE(tf1); COAL_UNUSED_VARIABLE(o2); @@ -198,9 +203,9 @@ struct COAL_LOCAL HeightFieldShapeDistancer { }; template -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; MeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); @@ -220,12 +225,12 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, namespace details { template -FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar orientedMeshDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; OrientedMeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); @@ -240,39 +245,41 @@ FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, } // namespace details template <> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template <> -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const DistanceRequest& request, - DistanceResult& result) { - return details::orientedMeshDistance( - o1, tf1, o2, tf2, request, result); -} - -template <> -FCL_REAL BVHDistance(const CollisionGeometry* o1, +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { + return details::orientedMeshDistance( + o1, tf1, o2, tf2, request, result); +} + +template <> +CoalScalar BVHDistance(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, + const DistanceRequest& request, + DistanceResult& result) { return details::orientedMeshDistance( o1, tf1, o2, tf2, request, result); } template -FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* /*nsolver*/, - const DistanceRequest& request, DistanceResult& result) { +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* /*nsolver*/, + const DistanceRequest& request, DistanceResult& result) { return BVHDistance(o1, tf1, o2, tf2, request, result); } diff --git a/src/intersect.cpp b/src/intersect.cpp index 6feb74e18..feca0843e 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -46,9 +46,9 @@ namespace coal { bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, - const Vec3f& v3, Vec3f* n, FCL_REAL* t) { + const Vec3f& v3, Vec3f* n, CoalScalar* t) { Vec3f n_ = (v2 - v1).cross(v3 - v1); - FCL_REAL norm2 = n_.squaredNorm(); + CoalScalar norm2 = n_.squaredNorm(); if (norm2 > 0) { *n = n_ / sqrt(norm2); *t = n->dot(v1); @@ -61,7 +61,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y) { Vec3f T; - FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; + CoalScalar A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; Vec3f TMP; T = Q - P; @@ -74,12 +74,12 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, // t parameterizes ray P,A // u parameterizes ray Q,B - FCL_REAL t, u; + CoalScalar t, u; // compute t for the closest point on ray P,A to // ray Q,B - FCL_REAL denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; + CoalScalar denom = A_dot_A * B_dot_B - A_dot_B * A_dot_B; t = (A_dot_T * B_dot_B - B_dot_T * A_dot_B) / denom; @@ -153,8 +153,8 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, } } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - Vec3f& P, Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + Vec3f& P, Vec3f& Q) { // Compute vectors along the 6 sides Vec3f Sv[3]; @@ -178,7 +178,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], // points found, and whether the triangles were shown disjoint Vec3f V, Z, minP, minQ; - FCL_REAL mindd; + CoalScalar mindd; int shown_disjoint = 0; mindd = (S[0] - T[0]).squaredNorm() + 1; // Set first minimum safely high @@ -190,7 +190,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); V = Q - P; - FCL_REAL dd = V.dot(V); + CoalScalar dd = V.dot(V); // Verify this closest point pair only if the distance // squared is less than the minimum found thus far. @@ -201,13 +201,13 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], mindd = dd; Z = S[(i + 2) % 3] - P; - FCL_REAL a = Z.dot(VEC); + CoalScalar a = Z.dot(VEC); Z = T[(j + 2) % 3] - Q; - FCL_REAL b = Z.dot(VEC); + CoalScalar b = Z.dot(VEC); if ((a <= 0) && (b >= 0)) return dd; - FCL_REAL p = V.dot(VEC); + CoalScalar p = V.dot(VEC); if (a < 0) a = 0; if (b > 0) b = 0; @@ -233,7 +233,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], // First check for case 1 Vec3f Sn; - FCL_REAL Snl; + CoalScalar Snl; Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle Snl = Sn.dot(Sn); // Compute square of length of normal @@ -301,7 +301,7 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], } Vec3f Tn; - FCL_REAL Tnl; + CoalScalar Tnl; Tn = Tv[0].cross(Tv[1]); Tnl = Tn.dot(Tn); @@ -367,10 +367,10 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return 0; } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - Vec3f& P, Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + Vec3f& P, Vec3f& Q) { Vec3f S[3]; Vec3f T[3]; S[0] = S1; @@ -383,9 +383,9 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, return sqrTriDistance(S, T, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Matrix3f& R, const Vec3f& Tl, + Vec3f& P, Vec3f& Q) { Vec3f T_transformed[3]; T_transformed[0] = R * T[0] + Tl; T_transformed[1] = R * T[1] + Tl; @@ -394,9 +394,9 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return sqrTriDistance(S, T_transformed, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, Vec3f& P, - Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], + const Transform3f& tf, Vec3f& P, + Vec3f& Q) { Vec3f T_transformed[3]; T_transformed[0] = tf.transform(T[0]); T_transformed[1] = tf.transform(T[1]); @@ -405,11 +405,11 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return sqrTriDistance(S, T_transformed, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Matrix3f& R, const Vec3f& Tl, + Vec3f& P, Vec3f& Q) { Vec3f T1_transformed = R * T1 + Tl; Vec3f T2_transformed = R * T2 + Tl; Vec3f T3_transformed = R * T3 + Tl; @@ -417,11 +417,11 @@ FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, T3_transformed, P, Q); } -FCL_REAL TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, Vec3f& P, - Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, + const Vec3f& S3, const Vec3f& T1, + const Vec3f& T2, const Vec3f& T3, + const Transform3f& tf, Vec3f& P, + Vec3f& Q) { Vec3f T1_transformed = tf.transform(T1); Vec3f T2_transformed = tf.transform(T2); Vec3f T3_transformed = tf.transform(T3); @@ -434,10 +434,10 @@ Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, ProjectResult res; const Vec3f d = b - a; - const FCL_REAL l = d.squaredNorm(); + const CoalScalar l = d.squaredNorm(); if (l > 0) { - const FCL_REAL t = (p - a).dot(d); + const CoalScalar t = (p - a).dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if (t >= l) { @@ -464,10 +464,10 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f* vt[] = {&a, &b, &c}; const Vec3f dl[] = {a - b, b - c, c - a}; const Vec3f& n = dl[0].cross(dl[1]); - const FCL_REAL l = n.squaredNorm(); + const CoalScalar l = n.squaredNorm(); if (l > 0) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { if ((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the @@ -490,8 +490,8 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, if (mindist < 0) // the origin project is within the triangle { - FCL_REAL d = (a - p).dot(n); - FCL_REAL s = sqrt(l); + CoalScalar d = (a - p).dot(n); + CoalScalar s = sqrt(l); Vec3f p_to_project = n * (d / l); mindist = p_to_project.squaredNorm(); res.encode = 7; // m = 0x111 @@ -517,7 +517,7 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, static const size_t nexti[] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c, &d}; const Vec3f dl[3] = {a - d, b - d, c - d}; - FCL_REAL vl = triple(dl[0], dl[1], dl[2]); + CoalScalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0; if (ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng @@ -525,11 +525,11 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, // does not grow toward the origin (in fact origin is // on the other side of the abc face) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; - FCL_REAL s = vl * (d - p).dot(dl[i].cross(dl[j])); + CoalScalar s = vl * (d - p).dot(dl[i].cross(dl[j])); if (s > 0) // the origin is to the outside part of a triangle face, then // the optimal can only be on the triangle face { @@ -572,10 +572,10 @@ Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, ProjectResult res; const Vec3f d = b - a; - const FCL_REAL l = d.squaredNorm(); + const CoalScalar l = d.squaredNorm(); if (l > 0) { - const FCL_REAL t = -a.dot(d); + const CoalScalar t = -a.dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if (t >= l) { @@ -602,10 +602,10 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, const Vec3f* vt[] = {&a, &b, &c}; const Vec3f dl[] = {a - b, b - c, c - a}; const Vec3f& n = dl[0].cross(dl[1]); - const FCL_REAL l = n.squaredNorm(); + const CoalScalar l = n.squaredNorm(); if (l > 0) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { if (vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the @@ -628,8 +628,8 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, if (mindist < 0) // the origin project is within the triangle { - FCL_REAL d = a.dot(n); - FCL_REAL s = sqrt(l); + CoalScalar d = a.dot(n); + CoalScalar s = sqrt(l); Vec3f o_to_project = n * (d / l); mindist = o_to_project.squaredNorm(); res.encode = 7; // m = 0x111 @@ -654,7 +654,7 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, static const size_t nexti[] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c, &d}; const Vec3f dl[3] = {a - d, b - d, c - d}; - FCL_REAL vl = triple(dl[0], dl[1], dl[2]); + CoalScalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0; if (ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng @@ -662,11 +662,11 @@ Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, // does not grow toward the origin (in fact origin is // on the other side of the abc face) { - FCL_REAL mindist = -1; + CoalScalar mindist = -1; for (size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; - FCL_REAL s = vl * d.dot(dl[i].cross(dl[j])); + CoalScalar s = vl * d.dot(dl[i].cross(dl[j])); if (s > 0) // the origin is to the outside part of a triangle face, then // the optimal can only be on the triangle face { diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index 804e6c89e..4cc1601e7 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -54,15 +54,15 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p, Vec3f v = s2 - s1; Vec3f w = p - s1; - FCL_REAL c1 = w.dot(v); - FCL_REAL c2 = v.dot(v); + CoalScalar c1 = w.dot(v); + CoalScalar c2 = v.dot(v); if (c1 <= 0) { sp = s1; } else if (c2 <= c1) { sp = s2; } else { - FCL_REAL b = c1 / c2; + CoalScalar b = c1 / c2; Vec3f Pb = s1 + v * b; sp = Pb; } @@ -72,9 +72,11 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p, /// @param p2 witness point on the Capsule. /// @param normal pointing from shape 1 to shape 2 (sphere to capsule). /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, - const Capsule& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar sphereCapsuleDistance(const Sphere& s1, + const Transform3f& tf1, + const Capsule& s2, + const Transform3f& tf2, Vec3f& p1, + Vec3f& p2, Vec3f& normal) { Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength))); Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength))); Vec3f s_c = tf1.getTranslation(); @@ -83,12 +85,12 @@ inline FCL_REAL sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); normal = segment_point - s_c; - FCL_REAL norm(normal.norm()); - FCL_REAL r1 = s1.radius + s1.getSweptSphereRadius(); - FCL_REAL r2 = s2.radius + s2.getSweptSphereRadius(); - FCL_REAL dist = norm - r1 - r2; + CoalScalar norm(normal.norm()); + CoalScalar r1 = s1.radius + s1.getSweptSphereRadius(); + CoalScalar r2 = s2.radius + s2.getSweptSphereRadius(); + CoalScalar dist = norm - r1 - r2; - static const FCL_REAL eps(std::numeric_limits::epsilon()); + static const CoalScalar eps(std::numeric_limits::epsilon()); if (norm > eps) { normal.normalize(); } else { @@ -103,14 +105,15 @@ inline FCL_REAL sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, /// @param p2 witness point on the Cylinder. /// @param normal pointing from shape 1 to shape 2 (sphere to cylinder). /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, - const Cylinder& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { - static const FCL_REAL eps(sqrt(std::numeric_limits::epsilon())); - FCL_REAL r1(s1.radius); - FCL_REAL r2(s2.radius); - FCL_REAL lz2(s2.halfLength); +inline CoalScalar sphereCylinderDistance(const Sphere& s1, + const Transform3f& tf1, + const Cylinder& s2, + const Transform3f& tf2, Vec3f& p1, + Vec3f& p2, Vec3f& normal) { + static const CoalScalar eps(sqrt(std::numeric_limits::epsilon())); + CoalScalar r1(s1.radius); + CoalScalar r2(s2.radius); + CoalScalar lz2(s2.halfLength); // boundaries of the cylinder axis Vec3f A(tf2.transform(Vec3f(0, 0, -lz2))); Vec3f B(tf2.transform(Vec3f(0, 0, lz2))); @@ -123,14 +126,14 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, assert((B - A - (s2.halfLength * 2) * u).norm() < eps); Vec3f AS(S - A); // abscissa of S on cylinder axis with A as the origin - FCL_REAL s(u.dot(AS)); + CoalScalar s(u.dot(AS)); Vec3f P(A + s * u); Vec3f PS(S - P); - FCL_REAL dPS = PS.norm(); + CoalScalar dPS = PS.norm(); // Normal to cylinder axis such that plane (A, u, v) contains sphere // center Vec3f v(0, 0, 0); - FCL_REAL dist; + CoalScalar dist; if (dPS > eps) { // S is not on cylinder axis v = (1 / dPS) * PS; @@ -146,7 +149,7 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, // closest point on cylinder is on cylinder circle basis p2 = A + r2 * v; Vec3f Sp2(p2 - S); - FCL_REAL dSp2 = Sp2.norm(); + CoalScalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; p1 = S + r1 * normal; @@ -179,7 +182,7 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, // closest point on cylinder is on cylinder circle basis p2 = B + r2 * v; Vec3f Sp2(p2 - S); - FCL_REAL dSp2 = Sp2.norm(); + CoalScalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; p1 = S + r1 * normal; @@ -196,8 +199,8 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -211,19 +214,19 @@ inline FCL_REAL sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, /// @param p2 witness point on the second Sphere. /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two spheres (negative if penetration). -inline FCL_REAL sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, + const Sphere& s2, const Transform3f& tf2, + Vec3f& p1, Vec3f& p2, Vec3f& normal) { const coal::Vec3f& center1 = tf1.getTranslation(); const coal::Vec3f& center2 = tf2.getTranslation(); - FCL_REAL r1 = (s1.radius + s1.getSweptSphereRadius()); - FCL_REAL r2 = (s2.radius + s2.getSweptSphereRadius()); + CoalScalar r1 = (s1.radius + s1.getSweptSphereRadius()); + CoalScalar r2 = (s2.radius + s2.getSweptSphereRadius()); Vec3f c1c2 = center2 - center1; - FCL_REAL cdist = c1c2.norm(); + CoalScalar cdist = c1c2.norm(); Vec3f unit(1, 0, 0); - if (cdist > Eigen::NumTraits::epsilon()) unit = c1c2 / cdist; - FCL_REAL dist = cdist - r1 - r2; + if (cdist > Eigen::NumTraits::epsilon()) unit = c1c2 / cdist; + CoalScalar dist = cdist - r1 - r2; normal = unit; p1.noalias() = center1 + r1 * unit; p2.noalias() = center2 - r2 * unit; @@ -231,14 +234,14 @@ inline FCL_REAL sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, } /** @brief the minimum distance from a point to a line */ -inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to, - const Vec3f& p, Vec3f& nearest) { +inline CoalScalar segmentSqrDistance(const Vec3f& from, const Vec3f& to, + const Vec3f& p, Vec3f& nearest) { Vec3f diff = p - from; Vec3f v = to - from; - FCL_REAL t = v.dot(diff); + CoalScalar t = v.dot(diff); if (t > 0) { - FCL_REAL dotVV = v.squaredNorm(); + CoalScalar dotVV = v.squaredNorm(); if (t < dotVV) { t /= dotVV; diff -= v * t; @@ -268,7 +271,7 @@ inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, Vec3f edge2_normal(edge2.cross(normal)); Vec3f edge3_normal(edge3.cross(normal)); - FCL_REAL r1, r2, r3; + CoalScalar r1, r2, r3; r1 = edge1_normal.dot(p1_to_p); r2 = edge2_normal.dot(p2_to_p); r3 = edge3_normal.dot(p3_to_p); @@ -282,10 +285,11 @@ inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, /// @param p2 witness point on the second Sphere. /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, - const TriangleP& tri, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +inline CoalScalar sphereTriangleDistance(const Sphere& s, + const Transform3f& tf1, + const TriangleP& tri, + const Transform3f& tf2, Vec3f& p1, + Vec3f& p2, Vec3f& normal) { const Vec3f& P1 = tf2.transform(tri.a); const Vec3f& P2 = tf2.transform(tri.b); const Vec3f& P3 = tf2.transform(tri.c); @@ -297,15 +301,15 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, // object with a swept-sphere radius of r2 is equivalent to comparing the // first object with a swept-sphere radius of r1 + r2 against the second // object with a swept-sphere radius of 0. - const FCL_REAL& radius = + const CoalScalar& radius = s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius(); assert(radius >= 0); assert(s.radius >= 0); Vec3f p1_to_center = center - P1; - FCL_REAL distance_from_plane = p1_to_center.dot(tri_normal); + CoalScalar distance_from_plane = p1_to_center.dot(tri_normal); Vec3f closest_point( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); - FCL_REAL min_distance_sqr, distance_sqr; + Vec3f::Constant(std::numeric_limits::quiet_NaN())); + CoalScalar min_distance_sqr, distance_sqr; if (distance_from_plane < 0) { distance_from_plane *= -1; @@ -335,7 +339,7 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, normal = (closest_point - center).normalized(); p1 = center + normal * (s.radius + s.getSweptSphereRadius()); p2 = closest_point - normal * tri.getSweptSphereRadius(); - const FCL_REAL distance = std::sqrt(min_distance_sqr) - radius; + const CoalScalar distance = std::sqrt(min_distance_sqr) - radius; return distance; } @@ -343,9 +347,9 @@ inline FCL_REAL sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, - const ShapeBase& s, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1, + const ShapeBase& s, const Transform3f& tf2, + Vec3f& p1, Vec3f& p2, Vec3f& normal) { // TODO(louis): handle multiple contact points when the halfspace normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). @@ -362,12 +366,12 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, getSupport(&s, -n_2, hint); p2 = tf2.transform(p2); - const FCL_REAL dist = new_h.signedDistance(p2); + const CoalScalar dist = new_h.signedDistance(p2); p1.noalias() = p2 - dist * new_h.n; normal.noalias() = new_h.n; - const FCL_REAL dummy_precision = - std::sqrt(Eigen::NumTraits::dummy_precision()); + const CoalScalar dummy_precision = + std::sqrt(Eigen::NumTraits::dummy_precision()); COAL_UNUSED_VARIABLE(dummy_precision); assert(new_h.distance(p1) <= dummy_precision); return dist; @@ -377,9 +381,9 @@ inline FCL_REAL halfspaceDistance(const Halfspace& h, const Transform3f& tf1, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1, - const ShapeBase& s, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1, + const ShapeBase& s, const Transform3f& tf2, + Vec3f& p1, Vec3f& p2, Vec3f& normal) { // TODO(louis): handle multiple contact points when the plane normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). @@ -402,14 +406,14 @@ inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1, getSupport(&s, -n_h2, hint); p2h2 = tf2.transform(p2h2); - FCL_REAL dist1 = new_h[0].signedDistance(p2h1); - FCL_REAL dist2 = new_h[1].signedDistance(p2h2); + CoalScalar dist1 = new_h[0].signedDistance(p2h1); + CoalScalar dist2 = new_h[1].signedDistance(p2h2); - const FCL_REAL dummy_precision = - std::sqrt(Eigen::NumTraits::dummy_precision()); + const CoalScalar dummy_precision = + std::sqrt(Eigen::NumTraits::dummy_precision()); COAL_UNUSED_VARIABLE(dummy_precision); - FCL_REAL dist; + CoalScalar dist; if (dist1 >= dist2) { dist = dist1; p2.noalias() = p2h1; @@ -431,9 +435,9 @@ inline FCL_REAL planeDistance(const Plane& plane, const Transform3f& tf1, /// @param ps the witness point on the sphere. /// @param normal pointing from box to sphere /// @return the distance between the two shapes (negative if penetration). -inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, - const Sphere& s, const Transform3f& tfs, - Vec3f& pb, Vec3f& ps, Vec3f& normal) { +inline CoalScalar boxSphereDistance(const Box& b, const Transform3f& tfb, + const Sphere& s, const Transform3f& tfs, + Vec3f& pb, Vec3f& ps, Vec3f& normal) { const Vec3f& os = tfs.getTranslation(); const Vec3f& ob = tfb.getTranslation(); const Matrix3f& Rb = tfb.getRotation(); @@ -443,9 +447,9 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, bool outside = false; const Vec3f os_in_b_frame(Rb.transpose() * (os - ob)); int axis = -1; - FCL_REAL min_d = (std::numeric_limits::max)(); + CoalScalar min_d = (std::numeric_limits::max)(); for (int i = 0; i < 3; ++i) { - FCL_REAL facedist; + CoalScalar facedist; if (os_in_b_frame(i) < -b.halfSide(i)) { // outside pb.noalias() -= b.halfSide(i) * Rb.col(i); outside = true; @@ -462,9 +466,9 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, } } normal = pb - os; - FCL_REAL pdist = normal.norm(); - FCL_REAL dist; // distance between sphere and box - if (outside) { // pb is on the box + CoalScalar pdist = normal.norm(); + CoalScalar dist; // distance between sphere and box + if (outside) { // pb is on the box dist = pdist - s.radius; normal /= -pdist; } else { // pb is inside the box @@ -482,8 +486,8 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, } // Take swept-sphere radius into account - const FCL_REAL ssrb = b.getSweptSphereRadius(); - const FCL_REAL ssrs = s.getSweptSphereRadius(); + const CoalScalar ssrb = b.getSweptSphereRadius(); + const CoalScalar ssrs = s.getSweptSphereRadius(); if (ssrb > 0 || ssrs > 0) { pb += ssrb * normal; ps -= ssrs * normal; @@ -505,36 +509,36 @@ inline FCL_REAL boxSphereDistance(const Box& b, const Transform3f& tfb, /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. -inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, - const Transform3f& tf1, - const Halfspace& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1, + const Transform3f& tf1, + const Halfspace& s2, + const Transform3f& tf2, Vec3f& p1, + Vec3f& p2, Vec3f& normal) { Halfspace new_s1 = transform(s1, tf1); Halfspace new_s2 = transform(s2, tf2); - FCL_REAL distance; + CoalScalar distance; Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_sq_norm = dir.squaredNorm(); + CoalScalar dir_sq_norm = dir.squaredNorm(); - if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel + if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { if (new_s1.n.dot(new_s2.n) > 0) { // If the two halfspaces have the same normal, one is inside the other // and they can't be separated. They have inifinte penetration depth. - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); if (new_s1.d <= new_s2.d) { normal = new_s1.n; p1 = normal * distance; p2 = new_s2.n * new_s2.d; assert(new_s2.distance(p2) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); } else { normal = -new_s1.n; p1 << new_s1.n * new_s1.d; p2 = -(normal * distance); assert(new_s1.distance(p1) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); } } else { distance = -(new_s1.d + new_s2.d); @@ -546,7 +550,7 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, // If the halfspaces are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. @@ -558,8 +562,8 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -581,18 +585,19 @@ inline FCL_REAL halfspaceHalfspaceDistance(const Halfspace& s1, /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. -inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, - const Transform3f& tf1, const Plane& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { +inline CoalScalar halfspacePlaneDistance(const Halfspace& s1, + const Transform3f& tf1, + const Plane& s2, + const Transform3f& tf2, Vec3f& p1, + Vec3f& p2, Vec3f& normal) { Halfspace new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); - FCL_REAL distance; + CoalScalar distance; Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_sq_norm = dir.squaredNorm(); + CoalScalar dir_sq_norm = dir.squaredNorm(); - if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel + if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { normal = new_s1.n; distance = new_s1.n.dot(new_s2.n) > 0 ? (new_s2.d - new_s1.d) @@ -600,14 +605,14 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, p1 = new_s1.n * new_s1.d; p2 = new_s2.n * new_s2.d; assert(new_s1.distance(p1) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); assert(new_s2.distance(p2) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); } else { // If the halfspace and plane are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. @@ -619,8 +624,8 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -642,27 +647,27 @@ inline FCL_REAL halfspacePlaneDistance(const Halfspace& s1, /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. -inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { +inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3f& tf1, + const Plane& s2, const Transform3f& tf2, + Vec3f& p1, Vec3f& p2, Vec3f& normal) { Plane new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); - FCL_REAL distance; + CoalScalar distance; Vec3f dir = (new_s1.n).cross(new_s2.n); - FCL_REAL dir_sq_norm = dir.squaredNorm(); + CoalScalar dir_sq_norm = dir.squaredNorm(); - if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel + if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel { p1 = new_s1.n * new_s1.d; p2 = new_s2.n * new_s2.d; assert(new_s1.distance(p1) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); assert(new_s2.distance(p2) <= - Eigen::NumTraits::dummy_precision()); + Eigen::NumTraits::dummy_precision()); distance = (p1 - p2).norm(); - if (distance > Eigen::NumTraits::dummy_precision()) { + if (distance > Eigen::NumTraits::dummy_precision()) { normal = (p2 - p1).normalized(); } else { normal = new_s1.n; @@ -671,7 +676,7 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, // If the planes are not parallel, they are in collision. // Their distance, in the sens of the norm of separation vector, is infinite // (it's impossible to find a translation which separates them) - distance = -(std::numeric_limits::max)(); + distance = -(std::numeric_limits::max)(); // p1 and p2 are the same point, corresponding to a point on the // intersection line between the two objects. Normal is the direction of // that line. @@ -683,8 +688,8 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, } // Take swept-sphere radius into account - const FCL_REAL ssr1 = s1.getSweptSphereRadius(); - const FCL_REAL ssr2 = s2.getSweptSphereRadius(); + const CoalScalar ssr1 = s1.getSweptSphereRadius(); + const CoalScalar ssr2 = s2.getSweptSphereRadius(); if (ssr1 > 0 || ssr2 > 0) { p1 += ssr1 * normal; p2 -= ssr2 * normal; @@ -695,15 +700,15 @@ inline FCL_REAL planePlaneDistance(const Plane& s1, const Transform3f& tf1, } /// See the prototype below -inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Vec3f& Q1, - const Vec3f& Q2, const Vec3f& Q3, - Vec3f& normal) { +inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Vec3f& Q1, + const Vec3f& Q2, const Vec3f& Q3, + Vec3f& normal) { Vec3f u((P2 - P1).cross(P3 - P1)); normal = u.normalized(); - FCL_REAL depth1((P1 - Q1).dot(normal)); - FCL_REAL depth2((P1 - Q2).dot(normal)); - FCL_REAL depth3((P1 - Q3).dot(normal)); + CoalScalar depth1((P1 - Q1).dot(normal)); + CoalScalar depth2((P1 - Q2).dot(normal)); + CoalScalar depth3((P1 - Q3).dot(normal)); return std::max(depth1, std::max(depth2, depth3)); } @@ -714,11 +719,11 @@ inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, // // Note that we compute here an upper bound of the penetration distance, // not the exact value. -inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Vec3f& Q1, - const Vec3f& Q2, const Vec3f& Q3, - const Transform3f& tf1, - const Transform3f& tf2, Vec3f& normal) { +inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2, + const Vec3f& P3, const Vec3f& Q1, + const Vec3f& Q2, const Vec3f& Q3, + const Transform3f& tf1, + const Transform3f& tf2, Vec3f& normal) { Vec3f globalP1(tf1.transform(P1)); Vec3f globalP2(tf1.transform(P2)); Vec3f globalP3(tf1.transform(P3)); diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index f6959bbdd..e8093de60 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -48,14 +48,14 @@ namespace coal { namespace details { void GJK::initialize() { - distance_upper_bound = (std::numeric_limits::max)(); + distance_upper_bound = (std::numeric_limits::max)(); gjk_variant = GJKVariant::DefaultGJK; convergence_criterion = GJKConvergenceCriterion::Default; convergence_criterion_type = GJKConvergenceCriterionType::Relative; reset(max_iterations, tolerance); } -void GJK::reset(size_t max_iterations_, FCL_REAL tolerance_) { +void GJK::reset(size_t max_iterations_, CoalScalar tolerance_) { max_iterations = max_iterations_; tolerance = tolerance_; COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", @@ -106,7 +106,7 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { case 2: { const Vec3f &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, b0 = vs[1]->w0, b1 = vs[1]->w1; - FCL_REAL la, lb; + CoalScalar la, lb; Vec3f N(b - a); la = N.dot(-a); if (la <= 0) { @@ -158,12 +158,12 @@ template void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0, Vec3f& w1) { #ifndef NDEBUG - const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); + const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); assert((normal.norm() - 1) < dummy_precision); #endif - const Eigen::Array& I(shape.swept_sphere_radius); + const Eigen::Array& I(shape.swept_sphere_radius); Eigen::Array inflate(I > 0); if (!inflate.any()) return; @@ -176,7 +176,7 @@ void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0, void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1, Vec3f& normal) const { details::getClosestPoints(*simplex, w0, w1); - if ((w1 - w0).norm() > Eigen::NumTraits::dummy_precision()) { + if ((w1 - w0).norm() > Eigen::NumTraits::dummy_precision()) { normal = (w1 - w0).normalized(); } else { normal = -this->ray.normalized(); @@ -186,10 +186,10 @@ void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, const support_func_guess_t& supportHint) { - FCL_REAL alpha = 0; + CoalScalar alpha = 0; iterations = 0; - const FCL_REAL swept_sphere_radius = shape_.swept_sphere_radius.sum(); - const FCL_REAL upper_bound = distance_upper_bound + swept_sphere_radius; + const CoalScalar swept_sphere_radius = shape_.swept_sphere_radius.sum(); + const CoalScalar upper_bound = distance_upper_bound + swept_sphere_radius; free_v[0] = &store_v[0]; free_v[1] = &store_v[1]; @@ -204,7 +204,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, simplices[current].rank = 0; support_hint = supportHint; - FCL_REAL rl = guess.norm(); + CoalScalar rl = guess.norm(); if (rl < tolerance) { ray = Vec3f(-1, 0, 0); rl = 1; @@ -216,7 +216,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, Vec3f w = ray; Vec3f dir = ray; Vec3f y; - FCL_REAL momentum; + CoalScalar momentum; bool normalize_support_direction = shape->normalize_support_direction; do { vertex_id_t next = (vertex_id_t)(1 - current); @@ -251,9 +251,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // Normalize heuristic for collision pairs involving convex but not // strictly-convex shapes This corresponds to most use cases. if (normalize_support_direction) { - momentum = (FCL_REAL(iterations) + 2) / (FCL_REAL(iterations) + 3); + momentum = + (CoalScalar(iterations) + 2) / (CoalScalar(iterations) + 3); y = momentum * ray + (1 - momentum) * w; - FCL_REAL y_norm = y.norm(); + CoalScalar y_norm = y.norm(); // ray is the point of the Minkowski difference which currently the // closest to the origin. Therefore, y.norm() > ray.norm() Hence, if // check A above has not stopped the algorithm, we necessarily have @@ -261,14 +262,15 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, assert(y_norm > tolerance); dir = momentum * dir / dir.norm() + (1 - momentum) * y / y_norm; } else { - momentum = (FCL_REAL(iterations) + 1) / (FCL_REAL(iterations) + 3); + momentum = + (CoalScalar(iterations) + 1) / (CoalScalar(iterations) + 3); y = momentum * ray + (1 - momentum) * w; dir = momentum * dir + (1 - momentum) * y; } break; case PolyakAcceleration: - momentum = 1 / (FCL_REAL(iterations) + 1); + momentum = 1 / (CoalScalar(iterations) + 1); dir = momentum * dir + (1 - momentum) * ray; break; @@ -284,7 +286,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, w = curr_simplex.vertex[curr_simplex.rank - 1]->w; // check B: no collision if omega > 0 - FCL_REAL omega = dir.dot(w) / dir.norm(); + CoalScalar omega = dir.dot(w) / dir.norm(); if (omega > upper_bound) { distance = omega - swept_sphere_radius; status = NoCollisionEarlyStopped; @@ -293,7 +295,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // Check to remove acceleration if (current_gjk_variant != DefaultGJK) { - FCL_REAL frank_wolfe_duality_gap = 2 * ray.dot(ray - w); + CoalScalar frank_wolfe_duality_gap = 2 * ray.dot(ray - w); if (frank_wolfe_duality_gap - tolerance <= 0) { removeVertex(simplices[current]); current_gjk_variant = DefaultGJK; // move back to classic GJK @@ -368,8 +370,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, return status; } -bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega) const { +bool GJK::checkConvergence(const Vec3f& w, const CoalScalar& rl, + CoalScalar& alpha, const CoalScalar& omega) const { // x^* is the optimal solution (projection of origin onto the Minkowski // difference). // x^k is the current iterate (x^k = `ray` in the code). @@ -380,13 +382,13 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^*|| - ||x^k|| <= diff - const FCL_REAL diff = rl - alpha; + const CoalScalar diff = rl - alpha; return ((diff - (tolerance + tolerance * rl)) <= 0); } break; case DualityGap: { // ||x^* - x^k||^2 <= diff - const FCL_REAL diff = 2 * ray.dot(ray - w); + const CoalScalar diff = 2 * ray.dot(ray - w); switch (convergence_criterion_type) { case Absolute: return ((diff - tolerance) <= 0); @@ -404,7 +406,7 @@ bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^* - x^k||^2 <= diff - const FCL_REAL diff = rl * rl - alpha * alpha; + const CoalScalar diff = rl * rl - alpha * alpha; switch (convergence_criterion_type) { case Absolute: return ((diff - tolerance) <= 0); @@ -500,7 +502,7 @@ inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a, inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, GJK::vertex_id_t b, const Vec3f& A, const Vec3f& B, - const Vec3f& AB, const FCL_REAL& ABdotAO, + const Vec3f& AB, const CoalScalar& ABdotAO, GJK::Simplex& next, Vec3f& ray) { // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B ray = AB.dot(B) * A + ABdotAO * B; @@ -515,7 +517,7 @@ inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a, GJK::vertex_id_t b, GJK::vertex_id_t c, - const Vec3f& ABC, const FCL_REAL& ABCdotAO, + const Vec3f& ABC, const CoalScalar& ABCdotAO, GJK::Simplex& next, Vec3f& ray) { next.rank = 3; next.vertex[2] = current.vertex[a]; @@ -546,7 +548,7 @@ bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { const Vec3f& B = current.vertex[b]->w; const Vec3f AB = B - A; - const FCL_REAL d = AB.dot(-A); + const CoalScalar d = AB.dot(-A); assert(d <= AB.squaredNorm()); if (d == 0) { @@ -575,14 +577,14 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { const Vec3f AB = B - A, AC = C - A, ABC = AB.cross(AC); - FCL_REAL edgeAC2o = ABC.cross(AC).dot(-A); + CoalScalar edgeAC2o = ABC.cross(AC).dot(-A); if (edgeAC2o >= 0) { - FCL_REAL towardsC = AC.dot(-A); + CoalScalar towardsC = AC.dot(-A); if (towardsC >= 0) { // Region 1 originToSegment(current, a, c, A, C, AC, towardsC, next, ray); free_v[nfree++] = current.vertex[b]; } else { // Region 4 or 5 - FCL_REAL towardsB = AB.dot(-A); + CoalScalar towardsB = AB.dot(-A); if (towardsB < 0) { // Region 5 // A is the closest to the origin originToPoint(current, a, A, next, ray); @@ -592,9 +594,9 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { free_v[nfree++] = current.vertex[c]; } } else { - FCL_REAL edgeAB2o = AB.cross(ABC).dot(-A); + CoalScalar edgeAB2o = AB.cross(ABC).dot(-A); if (edgeAB2o >= 0) { // Region 4 or 5 - FCL_REAL towardsB = AB.dot(-A); + CoalScalar towardsB = AB.dot(-A); if (towardsB < 0) { // Region 5 // A is the closest to the origin originToPoint(current, a, A, next, ray); @@ -616,30 +618,30 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { const Vec3f& B(current.vertex[b]->w); const Vec3f& C(current.vertex[c]->w); const Vec3f& D(current.vertex[d]->w); - const FCL_REAL aa = A.squaredNorm(); - const FCL_REAL da = D.dot(A); - const FCL_REAL db = D.dot(B); - const FCL_REAL dc = D.dot(C); - const FCL_REAL dd = D.dot(D); - const FCL_REAL da_aa = da - aa; - const FCL_REAL ca = C.dot(A); - const FCL_REAL cb = C.dot(B); - const FCL_REAL cc = C.dot(C); - const FCL_REAL& cd = dc; - const FCL_REAL ca_aa = ca - aa; - const FCL_REAL ba = B.dot(A); - const FCL_REAL bb = B.dot(B); - const FCL_REAL& bc = cb; - const FCL_REAL& bd = db; - const FCL_REAL ba_aa = ba - aa; - const FCL_REAL ba_ca = ba - ca; - const FCL_REAL ca_da = ca - da; - const FCL_REAL da_ba = da - ba; + const CoalScalar aa = A.squaredNorm(); + const CoalScalar da = D.dot(A); + const CoalScalar db = D.dot(B); + const CoalScalar dc = D.dot(C); + const CoalScalar dd = D.dot(D); + const CoalScalar da_aa = da - aa; + const CoalScalar ca = C.dot(A); + const CoalScalar cb = C.dot(B); + const CoalScalar cc = C.dot(C); + const CoalScalar& cd = dc; + const CoalScalar ca_aa = ca - aa; + const CoalScalar ba = B.dot(A); + const CoalScalar bb = B.dot(B); + const CoalScalar& bc = cb; + const CoalScalar& bd = db; + const CoalScalar ba_aa = ba - aa; + const CoalScalar ba_ca = ba - ca; + const CoalScalar ca_da = ca - da; + const CoalScalar da_ba = da - ba; const Vec3f a_cross_b = A.cross(B); const Vec3f a_cross_c = A.cross(C); - const FCL_REAL dummy_precision( - 3 * std::sqrt(std::numeric_limits::epsilon())); + const CoalScalar dummy_precision( + 3 * std::sqrt(std::numeric_limits::epsilon())); COAL_UNUSED_VARIABLE(dummy_precision); #define REGION_INSIDE() \ @@ -1010,7 +1012,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { void EPA::initialize() { reset(max_iterations, tolerance); } -void EPA::reset(size_t max_iterations_, FCL_REAL tolerance_) { +void EPA::reset(size_t max_iterations_, CoalScalar tolerance_) { max_iterations = max_iterations_; tolerance = tolerance_; // EPA creates only 2 faces and 1 vertex per iteration. @@ -1036,18 +1038,18 @@ void EPA::reset(size_t max_iterations_, FCL_REAL tolerance_) { } bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a, - const SimplexVertex& b, FCL_REAL& dist) { + const SimplexVertex& b, CoalScalar& dist) { Vec3f ab = b.w - a.w; Vec3f n_ab = ab.cross(face->n); - FCL_REAL a_dot_nab = a.w.dot(n_ab); + CoalScalar a_dot_nab = a.w.dot(n_ab); if (a_dot_nab < 0) // the origin is on the outside part of ab { // following is similar to projectOrigin for two points // however, as we dont need to compute the parameterization, dont need to // compute 0 or 1 - FCL_REAL a_dot_ab = a.w.dot(ab); - FCL_REAL b_dot_ab = b.w.dot(ab); + CoalScalar a_dot_ab = a.w.dot(ab); + CoalScalar b_dot_ab = b.w.dot(ab); if (a_dot_ab > 0) dist = a.w.norm(); @@ -1079,15 +1081,15 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c, const SimplexVertex& c = sv_store[id_c]; face->n = (b.w - a.w).cross(c.w - a.w); - if (face->n.norm() > Eigen::NumTraits::epsilon()) { + if (face->n.norm() > Eigen::NumTraits::epsilon()) { face->n.normalize(); // If the origin projects outside the face, skip it in the // `findClosestFace` method. // The origin always projects inside the closest face. - FCL_REAL a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); - FCL_REAL b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); - FCL_REAL c_dot_nca = c.w.dot((a.w - c.w).cross(face->n)); + CoalScalar a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); + CoalScalar b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); + CoalScalar c_dot_nca = c.w.dot((a.w - c.w).cross(face->n)); if (a_dot_nab >= -tolerance && // b_dot_nbc >= -tolerance && // c_dot_nca >= -tolerance) { @@ -1096,7 +1098,7 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c, } else { // We will never check this face, so we don't care about // its true distance to the origin. - face->d = std::numeric_limits::max(); + face->d = std::numeric_limits::max(); face->ignore = true; } @@ -1139,10 +1141,10 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c, /** @brief Find the best polytope face to split */ EPA::SimplexFace* EPA::findClosestFace() { SimplexFace* minf = hull.root; - FCL_REAL mind = std::numeric_limits::max(); + CoalScalar mind = std::numeric_limits::max(); for (SimplexFace* f = minf; f; f = f->next_face) { if (f->ignore) continue; - FCL_REAL sqd = f->d * f->d; + CoalScalar sqd = f->d * f->d; if (sqd < mind) { minf = f; mind = sqd; @@ -1245,8 +1247,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { const SimplexVertex& vf1 = sv_store[closest_face->vertex_id[0]]; const SimplexVertex& vf2 = sv_store[closest_face->vertex_id[1]]; const SimplexVertex& vf3 = sv_store[closest_face->vertex_id[2]]; - FCL_REAL fdist = closest_face->n.dot(w.w - vf1.w); - FCL_REAL wnorm = w.w.norm(); + CoalScalar fdist = closest_face->n.dot(w.w - vf1.w); + CoalScalar wnorm = w.w.norm(); // TODO(louis): we might want to use tol_abs and tol_rel; this might // obfuscate the code for the user though. if (fdist <= tolerance + tolerance * wnorm) { @@ -1303,7 +1305,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { // TODO: define a better normal assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance())); normal = -guess; - FCL_REAL nl = normal.norm(); + CoalScalar nl = normal.norm(); if (nl > 0) normal /= nl; else @@ -1406,8 +1408,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, // recursive nature of `expand`, it is safer to go through the first case. // This is because `expand` can potentially loop indefinitly if the // Minkowski difference is very flat (hence the check above). - const FCL_REAL dummy_precision( - 3 * std::sqrt(std::numeric_limits::epsilon())); + const CoalScalar dummy_precision( + 3 * std::sqrt(std::numeric_limits::epsilon())); const SimplexVertex& vf = sv_store[f->vertex_id[e]]; if (f->n.dot(w.w - vf.w) < dummy_precision) { // case 1: the support point is "below" `f`. @@ -1450,7 +1452,7 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1, Vec3f& normal) const { details::getClosestPoints(result, w0, w1); - if ((w0 - w1).norm() > Eigen::NumTraits::dummy_precision()) { + if ((w0 - w1).norm() > Eigen::NumTraits::dummy_precision()) { if (this->depth >= 0) { // The shapes are in collision. normal = (w0 - w1).normalized(); diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index 52fbb09ff..73307e12f 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -49,7 +49,7 @@ void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, const Vec3f& ot1, const Vec3f& dir, Vec3f& support0, Vec3f& support1, support_func_guess_t& hint, ShapeSupportData data[2]) { - assert(dir.norm() > Eigen::NumTraits::epsilon()); + assert(dir.norm() > Eigen::NumTraits::epsilon()); getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]); if (TransformIsIdentity) { @@ -77,7 +77,7 @@ void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, template MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( const ShapeBase* s1, bool identity, - Eigen::Array& swept_sphere_radius, + Eigen::Array& swept_sphere_radius, ShapeSupportData data[2]) { if (_SupportOptions == SupportOptions::WithSweptSphere) { // No need to store the information of swept sphere radius @@ -158,7 +158,7 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1( template MinkowskiDiff::GetSupportFunction makeGetSupportFunction0( const ShapeBase* s0, const ShapeBase* s1, bool identity, - Eigen::Array& swept_sphere_radius, + Eigen::Array& swept_sphere_radius, ShapeSupportData data[2]) { if (_SupportOptions == SupportOptions::WithSweptSphere) { // No need to store the information of swept sphere radius diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp index ef9edf2f6..9c7563f4b 100644 --- a/src/narrowphase/support_functions.cpp +++ b/src/narrowphase/support_functions.cpp @@ -130,15 +130,12 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, support += triangle->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(TriangleP) - // clang-format on +getShapeSupportTplInstantiation(TriangleP); - // ============================================================================ - template - inline void getShapeSupport(const Box* box, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { +// ============================================================================ +template +inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { // The inflate value is simply to make the specialized functions with box // have a preferred side for edge cases. static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.; @@ -153,15 +150,13 @@ getShapeSupportTplInstantiation(TriangleP) support += box->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Box) - // clang-format on +getShapeSupportTplInstantiation(Box); - // ============================================================================ - template - inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { +// ============================================================================ +template +inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { if (_SupportOptions == SupportOptions::WithSweptSphere) { support.noalias() = (sphere->radius + sphere->getSweptSphereRadius()) * dir.normalized(); @@ -169,18 +164,16 @@ getShapeSupportTplInstantiation(Box) support.setZero(); } - COAL_UNUSED_VARIABLE(sphere); - COAL_UNUSED_VARIABLE(dir); + HPP_FCL_UNUSED_VARIABLE(sphere); + HPP_FCL_UNUSED_VARIABLE(dir); } -// clang-format off -getShapeSupportTplInstantiation(Sphere) - // clang-format on +getShapeSupportTplInstantiation(Sphere); - // ============================================================================ - template - inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { +// ============================================================================ +template +inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; @@ -195,15 +188,13 @@ getShapeSupportTplInstantiation(Sphere) support += ellipsoid->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Ellipsoid) - // clang-format on +getShapeSupportTplInstantiation(Ellipsoid); - // ============================================================================ - template - inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { +// ============================================================================ +template +inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, + Vec3f& support, int& /*unused*/, + ShapeSupportData& /*unused*/) { static const FCL_REAL dummy_precision = Eigen::NumTraits::dummy_precision(); support.setZero(); @@ -218,14 +209,12 @@ getShapeSupportTplInstantiation(Ellipsoid) (capsule->radius + capsule->getSweptSphereRadius()) * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Capsule) - // clang-format on +getShapeSupportTplInstantiation(Capsule); - // ============================================================================ - template - void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, - int& /*unused*/, ShapeSupportData& /*unused*/) { +// ============================================================================ +template +void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { static const FCL_REAL dummy_precision = Eigen::NumTraits::dummy_precision(); @@ -270,15 +259,12 @@ getShapeSupportTplInstantiation(Capsule) support += cone->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Cone) - // clang-format on +getShapeSupportTplInstantiation(Cone); - // ============================================================================ - template - void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, - Vec3f& support, int& /*unused*/, - ShapeSupportData& /*unused*/) { +// ============================================================================ +template +void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, + int& /*unused*/, ShapeSupportData& /*unused*/) { static const FCL_REAL dummy_precision = Eigen::NumTraits::dummy_precision(); @@ -313,15 +299,13 @@ getShapeSupportTplInstantiation(Cone) support += cylinder->getSweptSphereRadius() * dir.normalized(); } } -// clang-format off -getShapeSupportTplInstantiation(Cylinder) - // clang-format on +getShapeSupportTplInstantiation(Cylinder); - // ============================================================================ - template - void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, - ShapeSupportData& support_data) { +// ============================================================================ +template +void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, + Vec3f& support, int& hint, + ShapeSupportData& support_data) { assert(convex->neighbors != nullptr && "Convex has no neighbors."); // Use warm start if current support direction is distant from last support @@ -446,31 +430,27 @@ getShapeSupportTplInstantiation(ConvexBase) reinterpret_cast(convex), dir, support, hint, support_data); } -// clang-format off -getShapeSupportTplInstantiation(SmallConvex) - // clang-format on +getShapeSupportTplInstantiation(SmallConvex); - // ============================================================================ - template - inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, - ShapeSupportData& support_data) { +// ============================================================================ +template +inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, + Vec3f& support, int& hint, + ShapeSupportData& support_data) { getShapeSupportLog<_SupportOptions>( reinterpret_cast(convex), dir, support, hint, support_data); } -// clang-format off -getShapeSupportTplInstantiation(LargeConvex) -// clang-format on +getShapeSupportTplInstantiation(LargeConvex); // ============================================================================ #define CALL_GET_SHAPE_SUPPORT_SET(ShapeType) \ getShapeSupportSet<_SupportOptions>(static_cast(shape), \ support_set, hint, support_data, \ max_num_supports, tol) - template - void getSupportSet(const ShapeBase* shape, SupportSet& support_set, - int& hint, size_t max_num_supports, FCL_REAL tol) { +template +void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, + size_t max_num_supports, FCL_REAL tol) { ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: @@ -567,15 +547,13 @@ void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, } } } -// clang-format off -getShapeSupportSetTplInstantiation(TriangleP) +getShapeSupportSetTplInstantiation(TriangleP); // ============================================================================ template void getShapeSupportSet(const Box* box, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, size_t /*unused*/, FCL_REAL tol) { - // clang-format on assert(tol > 0); Vec3f support; const Vec3f& support_dir = support_set.getNormal(); @@ -611,8 +589,7 @@ void getShapeSupportSet(const Box* box, SupportSet& support_set, } computeSupportSetConvexHull(polygon, support_set.points()); } -// clang-format off -getShapeSupportSetTplInstantiation(Box) +getShapeSupportSetTplInstantiation(Box); // ============================================================================ template @@ -620,7 +597,6 @@ void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t /*unused*/, FCL_REAL /*unused*/) { - // clang-format on support_set.points().clear(); Vec3f support; @@ -629,15 +605,13 @@ void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, support_data); support_set.addPoint(support); } -// clang-format off -getShapeSupportSetTplInstantiation(Sphere) +getShapeSupportSetTplInstantiation(Sphere); // ============================================================================ template void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, int& hint, ShapeSupportData& support_data /*unused*/, size_t /*unused*/, FCL_REAL /*unused*/) { - // clang-format on support_set.points().clear(); Vec3f support; @@ -646,8 +620,7 @@ void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, support_data); support_set.addPoint(support); } -// clang-format off -getShapeSupportSetTplInstantiation(Ellipsoid) +getShapeSupportSetTplInstantiation(Ellipsoid); // ============================================================================ template @@ -691,8 +664,7 @@ void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, } } } -// clang-format off -getShapeSupportSetTplInstantiation(Capsule) +getShapeSupportSetTplInstantiation(Capsule); // ============================================================================ template @@ -700,7 +672,6 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t num_sampled_supports, FCL_REAL tol) { - // clang-format on assert(tol > 0); support_set.points().clear(); @@ -764,8 +735,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, } } } -// clang-format off -getShapeSupportSetTplInstantiation(Cone) +getShapeSupportSetTplInstantiation(Cone); // ============================================================================ template @@ -773,7 +743,6 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t num_sampled_supports, FCL_REAL tol) { - // clang-format on assert(tol > 0); support_set.points().clear(); @@ -828,8 +797,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, } } } -// clang-format off -getShapeSupportSetTplInstantiation(Cylinder) +getShapeSupportSetTplInstantiation(Cylinder); // ============================================================================ template @@ -837,7 +805,6 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, size_t /*unused*/, FCL_REAL tol) { - // clang-format on assert(tol > 0); Vec3f support; const Vec3f& support_dir = support_set.getNormal(); @@ -877,7 +844,7 @@ void convexSupportSetRecurse( const Vec3f& support_dir, const FCL_REAL support_value, const Transform3f& tf, std::vector& visited, SupportSet::Polygon& polygon, FCL_REAL tol) { - COAL_UNUSED_VARIABLE(swept_sphere_radius); + HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius); if (visited[vertex_idx]) { return; @@ -954,8 +921,7 @@ void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, convex, support_set, hint, support_data, num_sampled_supports, tol); } } -// clang-format off -getShapeSupportSetTplInstantiation(ConvexBase) +getShapeSupportSetTplInstantiation(ConvexBase); // ============================================================================ template @@ -963,27 +929,22 @@ void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, size_t num_sampled_supports /*unused*/, FCL_REAL tol) { - // clang-format on getShapeSupportSetLinear<_SupportOptions>( reinterpret_cast(convex), support_set, hint, support_data, num_sampled_supports, tol); } -// clang-format off -getShapeSupportSetTplInstantiation(SmallConvex) +getShapeSupportSetTplInstantiation(SmallConvex); // ============================================================================ template void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, size_t num_sampled_supports /*unused*/, FCL_REAL tol) { - // clang-format on getShapeSupportSetLog<_SupportOptions>( reinterpret_cast(convex), support_set, hint, support_data, num_sampled_supports, tol); } -//clang-format off getShapeSupportSetTplInstantiation(LargeConvex); -// clang-format on // ============================================================================ COAL_DLLAPI diff --git a/src/octree.cpp b/src/octree.cpp index 7c8abed28..6ca93eff0 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -58,15 +58,15 @@ struct Neighbors { void computeNeighbors(const std::vector& boxes, std::vector& neighbors) { typedef std::vector VectorVec6f; - FCL_REAL fixedSize = -1; - FCL_REAL e(1e-8); + CoalScalar fixedSize = -1; + CoalScalar e(1e-8); for (std::size_t i = 0; i < boxes.size(); ++i) { const Vec6f& box(boxes[i]); Neighbors& n(neighbors[i]); - FCL_REAL x(box[0]); - FCL_REAL y(box[1]); - FCL_REAL z(box[2]); - FCL_REAL s(box[3]); + CoalScalar x(box[0]); + CoalScalar y(box[1]); + CoalScalar z(box[2]); + CoalScalar s(box[3]); if (fixedSize == -1) fixedSize = s; else @@ -75,9 +75,9 @@ void computeNeighbors(const std::vector& boxes, for (VectorVec6f::const_iterator it = boxes.begin(); it != boxes.end(); ++it) { const Vec6f& otherBox = *it; - FCL_REAL xo(otherBox[0]); - FCL_REAL yo(otherBox[1]); - FCL_REAL zo(otherBox[2]); + CoalScalar xo(otherBox[0]); + CoalScalar yo(otherBox[1]); + CoalScalar zo(otherBox[2]); // if (fabs(x-xo) < e && fabs(y-yo) < e && fabs(z-zo) < e){ // continue; // } @@ -122,10 +122,10 @@ void OcTree::exportAsObjFile(const std::string& filename) const { const Vec6f& box(boxes[i]); internal::Neighbors& n(neighbors[i]); - FCL_REAL x(box[0]); - FCL_REAL y(box[1]); - FCL_REAL z(box[2]); - FCL_REAL size(box[3]); + CoalScalar x(box[0]); + CoalScalar y(box[1]); + CoalScalar z(box[2]); + CoalScalar size(box[3]); vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z - .5 * size)); vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z - .5 * size)); @@ -186,9 +186,9 @@ void OcTree::exportAsObjFile(const std::string& filename) const { } OcTreePtr_t makeOctree( - const Eigen::Matrix& point_cloud, - const FCL_REAL resolution) { - typedef Eigen::Matrix InputType; + const Eigen::Matrix& point_cloud, + const CoalScalar resolution) { + typedef Eigen::Matrix InputType; typedef InputType::ConstRowXpr RowType; shared_ptr octree(new octomap::OcTree(resolution)); diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index b0b68e458..ba7a20785 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -118,9 +118,9 @@ void ConvexBase::computeCenter() { } void Halfspace::unitNormalTest() { - FCL_REAL l = n.norm(); + CoalScalar l = n.norm(); if (l > 0) { - FCL_REAL inv_l = 1.0 / l; + CoalScalar inv_l = 1.0 / l; n *= inv_l; d *= inv_l; } else { @@ -130,9 +130,9 @@ void Halfspace::unitNormalTest() { } void Plane::unitNormalTest() { - FCL_REAL l = n.norm(); + CoalScalar l = n.norm(); if (l > 0) { - FCL_REAL inv_l = 1.0 / l; + CoalScalar inv_l = 1.0 / l; n *= inv_l; d *= inv_l; } else { @@ -143,7 +143,7 @@ void Plane::unitNormalTest() { void Box::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -154,7 +154,7 @@ void Box::computeLocalAABB() { void Sphere::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -165,7 +165,7 @@ void Sphere::computeLocalAABB() { void Ellipsoid::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -176,7 +176,7 @@ void Ellipsoid::computeLocalAABB() { void Capsule::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -187,7 +187,7 @@ void Capsule::computeLocalAABB() { void Cone::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -198,7 +198,7 @@ void Cone::computeLocalAABB() { void Cylinder::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -209,7 +209,7 @@ void Cylinder::computeLocalAABB() { void ConvexBase::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -220,7 +220,7 @@ void ConvexBase::computeLocalAABB() { void Halfspace::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -231,7 +231,7 @@ void Halfspace::computeLocalAABB() { void Plane::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); @@ -242,7 +242,7 @@ void Plane::computeLocalAABB() { void TriangleP::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); - const FCL_REAL ssr = this->getSweptSphereRadius(); + const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3f::Constant(ssr); aabb_local.max_ += Vec3f::Constant(ssr); diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index c03335b83..b5d4bf2fc 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -45,9 +45,9 @@ namespace details { std::vector getBoundVertices(const Box& box, const Transform3f& tf) { std::vector result(8); - FCL_REAL a = box.halfSide[0]; - FCL_REAL b = box.halfSide[1]; - FCL_REAL c = box.halfSide[2]; + CoalScalar a = box.halfSide[0]; + CoalScalar b = box.halfSide[1]; + CoalScalar c = box.halfSide[2]; result[0] = tf.transform(Vec3f(a, b, c)); result[1] = tf.transform(Vec3f(a, b, -c)); result[2] = tf.transform(Vec3f(a, -b, c)); @@ -64,11 +64,11 @@ std::vector getBoundVertices(const Box& box, const Transform3f& tf) { std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf) { std::vector result(12); - const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; - FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); + const CoalScalar m = (1 + sqrt(5.0)) / 2.0; + CoalScalar edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); - FCL_REAL a = edge_size; - FCL_REAL b = m * edge_size; + CoalScalar a = edge_size; + CoalScalar b = m * edge_size; result[0] = tf.transform(Vec3f(0, a, b)); result[1] = tf.transform(Vec3f(0, -a, b)); result[2] = tf.transform(Vec3f(0, a, -b)); @@ -89,21 +89,21 @@ std::vector getBoundVertices(const Sphere& sphere, std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf) { std::vector result(12); - const FCL_REAL phi = (1 + sqrt(5.0)) / 2.0; + const CoalScalar phi = (1 + sqrt(5.0)) / 2.0; - const FCL_REAL a = sqrt(3.0) / (phi * phi); - const FCL_REAL b = phi * a; + const CoalScalar a = sqrt(3.0) / (phi * phi); + const CoalScalar b = phi * a; - const FCL_REAL& A = ellipsoid.radii[0]; - const FCL_REAL& B = ellipsoid.radii[1]; - const FCL_REAL& C = ellipsoid.radii[2]; + const CoalScalar& A = ellipsoid.radii[0]; + const CoalScalar& B = ellipsoid.radii[1]; + const CoalScalar& C = ellipsoid.radii[2]; - FCL_REAL Aa = A * a; - FCL_REAL Ab = A * b; - FCL_REAL Ba = B * a; - FCL_REAL Bb = B * b; - FCL_REAL Ca = C * a; - FCL_REAL Cb = C * b; + CoalScalar Aa = A * a; + CoalScalar Ab = A * b; + CoalScalar Ba = B * a; + CoalScalar Bb = B * b; + CoalScalar Ca = C * a; + CoalScalar Cb = C * b; result[0] = tf.transform(Vec3f(0, Ba, Cb)); result[1] = tf.transform(Vec3f(0, -Ba, Cb)); result[2] = tf.transform(Vec3f(0, Ba, -Cb)); @@ -123,13 +123,13 @@ std::vector getBoundVertices(const Ellipsoid& ellipsoid, std::vector getBoundVertices(const Capsule& capsule, const Transform3f& tf) { std::vector result(36); - const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; + const CoalScalar m = (1 + sqrt(5.0)) / 2.0; - FCL_REAL hl = capsule.halfLength; - FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0)); - FCL_REAL a = edge_size; - FCL_REAL b = m * edge_size; - FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0); + CoalScalar hl = capsule.halfLength; + CoalScalar edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0)); + CoalScalar a = edge_size; + CoalScalar b = m * edge_size; + CoalScalar r2 = capsule.radius * 2 / sqrt(3.0); result[0] = tf.transform(Vec3f(0, a, b + hl)); result[1] = tf.transform(Vec3f(0, -a, b + hl)); @@ -157,8 +157,8 @@ std::vector getBoundVertices(const Capsule& capsule, result[22] = tf.transform(Vec3f(-b, 0, a - hl)); result[23] = tf.transform(Vec3f(-b, 0, -a - hl)); - FCL_REAL c = 0.5 * r2; - FCL_REAL d = capsule.radius; + CoalScalar c = 0.5 * r2; + CoalScalar d = capsule.radius; result[24] = tf.transform(Vec3f(r2, 0, hl)); result[25] = tf.transform(Vec3f(c, d, hl)); result[26] = tf.transform(Vec3f(-c, d, hl)); @@ -179,10 +179,10 @@ std::vector getBoundVertices(const Capsule& capsule, std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { std::vector result(7); - FCL_REAL hl = cone.halfLength; - FCL_REAL r2 = cone.radius * 2 / sqrt(3.0); - FCL_REAL a = 0.5 * r2; - FCL_REAL b = cone.radius; + CoalScalar hl = cone.halfLength; + CoalScalar r2 = cone.radius * 2 / sqrt(3.0); + CoalScalar a = 0.5 * r2; + CoalScalar b = cone.radius; result[0] = tf.transform(Vec3f(r2, 0, -hl)); result[1] = tf.transform(Vec3f(a, b, -hl)); @@ -200,10 +200,10 @@ std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& tf) { std::vector result(12); - FCL_REAL hl = cylinder.halfLength; - FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0); - FCL_REAL a = 0.5 * r2; - FCL_REAL b = cylinder.radius; + CoalScalar hl = cylinder.halfLength; + CoalScalar r2 = cylinder.radius * 2 / sqrt(3.0); + CoalScalar a = 0.5 * r2; + CoalScalar b = cylinder.radius; result[0] = tf.transform(Vec3f(r2, 0, -hl)); result[1] = tf.transform(Vec3f(a, b, -hl)); @@ -253,7 +253,7 @@ Halfspace transform(const Halfspace& a, const Transform3f& tf) { /// and d' = d + n' * T Vec3f n = tf.getRotation() * a.n; - FCL_REAL d = a.d + n.dot(tf.getTranslation()); + CoalScalar d = a.d + n.dot(tf.getTranslation()); Halfspace result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); @@ -268,7 +268,7 @@ Plane transform(const Plane& a, const Transform3f& tf) { /// and d' = d + n' * T Vec3f n = tf.getRotation() * a.n; - FCL_REAL d = a.d + n.dot(tf.getTranslation()); + CoalScalar d = a.d + n.dot(tf.getTranslation()); Plane result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); @@ -280,7 +280,7 @@ std::array transformToHalfspaces(const Plane& a, // A plane can be represented by two halfspaces Vec3f n = tf.getRotation() * a.n; - FCL_REAL d = a.d + n.dot(tf.getTranslation()); + CoalScalar d = a.d + n.dot(tf.getTranslation()); std::array result = {Halfspace(n, d), Halfspace(-n, -d)}; result[0].setSweptSphereRadius(a.getSweptSphereRadius()); result[1].setSweptSphereRadius(a.getSweptSphereRadius()); @@ -334,12 +334,12 @@ void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + - fabs(R(0, 2) * s.halfLength); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + - fabs(R(1, 2) * s.halfLength); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + - fabs(R(2, 2) * s.halfLength); + CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + + fabs(R(0, 2) * s.halfLength); + CoalScalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + + fabs(R(1, 2) * s.halfLength); + CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + + fabs(R(2, 2) * s.halfLength); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; @@ -352,12 +352,12 @@ void computeBV(const Cylinder& s, const Transform3f& tf, const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); - FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + - fabs(R(0, 2) * s.halfLength); - FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + - fabs(R(1, 2) * s.halfLength); - FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + - fabs(R(2, 2) * s.halfLength); + CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + + fabs(R(0, 2) * s.halfLength); + CoalScalar y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + + fabs(R(1, 2) * s.halfLength); + CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + + fabs(R(2, 2) * s.halfLength); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; @@ -391,24 +391,24 @@ void computeBV(const Halfspace& s, const Transform3f& tf, AABB& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; AABB bv_; - bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); - bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); + bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with x axis if (n[0] < 0) bv_.min_[0] = -d; else if (n[0] > 0) bv_.max_[0] = d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with y axis if (n[1] < 0) bv_.min_[1] = -d; else if (n[1] > 0) bv_.max_[1] = d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { // normal aligned with z axis if (n[2] < 0) bv_.min_[2] = -d; @@ -423,26 +423,26 @@ template <> void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; AABB bv_; - bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); - bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); + bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with x axis if (n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } else if (n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with y axis if (n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } else if (n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { // normal aligned with z axis if (n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; @@ -551,7 +551,7 @@ void computeBV(const Halfspace& s, const Transform3f&, /// Half space can only have very rough OBB bv.axes.setIdentity(); bv.To.setZero(); - bv.extent.setConstant(((std::numeric_limits::max)())); + bv.extent.setConstant(((std::numeric_limits::max)())); } template <> @@ -565,7 +565,7 @@ void computeBV(const Halfspace& s, const Transform3f&, bv.axes.setIdentity(); bv.Tr.setZero(); bv.length[0] = bv.length[1] = bv.radius = - (std::numeric_limits::max)(); + (std::numeric_limits::max)(); } template <> @@ -589,7 +589,7 @@ void computeBV(const Halfspace& s, const Transform3f& tf, bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3f(); - bv.spheres[0].r = (std::numeric_limits::max)(); + bv.spheres[0].r = (std::numeric_limits::max)(); } template <> @@ -601,50 +601,50 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; const short D = 8; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else @@ -661,56 +661,56 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; const short D = 9; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else @@ -727,71 +727,71 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; const short D = 12; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { if (n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { if (n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { if (n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else bv.dist(8) = n[1] * d * 2; - } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[0] + n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; else bv.dist(9) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; else bv.dist(10) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; else @@ -809,8 +809,8 @@ void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; - bv.extent << 0, (std::numeric_limits::max)(), - (std::numeric_limits::max)(); + bv.extent << 0, (std::numeric_limits::max)(), + (std::numeric_limits::max)(); Vec3f p = s.n * s.d; bv.To = @@ -828,8 +828,8 @@ void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; - bv.length[0] = (std::numeric_limits::max)(); - bv.length[1] = (std::numeric_limits::max)(); + bv.length[0] = (std::numeric_limits::max)(); + bv.length[1] = (std::numeric_limits::max)(); bv.radius = 0; @@ -857,7 +857,7 @@ void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3f(); - bv.spheres[0].r = (std::numeric_limits::max)(); + bv.spheres[0].r = (std::numeric_limits::max)(); } template <> @@ -869,39 +869,39 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, } Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; const short D = 8; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } } @@ -915,41 +915,41 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, } Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; const short D = 9; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; } } @@ -963,47 +963,47 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, } Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; - const FCL_REAL& d = new_s.d; + const CoalScalar& d = new_s.d; const short D = 12; for (short i = 0; i < D; ++i) - bv.dist(i) = -(std::numeric_limits::max)(); + bv.dist(i) = -(std::numeric_limits::max)(); for (short i = D; i < 2 * D; ++i) - bv.dist(i) = (std::numeric_limits::max)(); + bv.dist(i) = (std::numeric_limits::max)(); - if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { if (n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == (CoalScalar)0.0) { if (n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; - } else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { + } else if (n[2] == (CoalScalar)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { + } else if (n[1] == (CoalScalar)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { + } else if (n[0] == (CoalScalar)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; - } else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { + } else if (n[0] + n[2] == (CoalScalar)0.0 && n[0] + n[1] == (CoalScalar)0.0) { bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[1] + n[2] == (CoalScalar)0.0) { bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; - } else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { + } else if (n[0] + n[1] == (CoalScalar)0.0 && n[0] + n[2] == (CoalScalar)0.0) { bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; } } diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp index 01ed51e1c..909e2979b 100644 --- a/src/traversal/traversal_recurse.cpp +++ b/src/traversal/traversal_recurse.cpp @@ -42,8 +42,8 @@ namespace coal { void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, unsigned int b2, BVHFrontList* front_list, - FCL_REAL& sqrDistLowerBound) { - FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; + CoalScalar& sqrDistLowerBound) { + CoalScalar sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0; bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if (l1 && l2) { @@ -85,15 +85,15 @@ void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1, void collisionNonRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list, - FCL_REAL& sqrDistLowerBound) { + CoalScalar& sqrDistLowerBound) { typedef std::pair BVPair_t; // typedef std::stack > Stack_t; typedef std::vector Stack_t; Stack_t pairs; pairs.reserve(1000); - sqrDistLowerBound = std::numeric_limits::infinity(); - FCL_REAL sdlb = std::numeric_limits::infinity(); + sqrDistLowerBound = std::numeric_limits::infinity(); + CoalScalar sdlb = std::numeric_limits::infinity(); pairs.push_back(BVPair_t(0, 0)); @@ -175,8 +175,8 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, c2 = (unsigned int)node->getSecondRightChild(b2); } - FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2); - FCL_REAL d2 = node->BVDistanceLowerBound(c1, c2); + CoalScalar d1 = node->BVDistanceLowerBound(a1, a2); + CoalScalar d2 = node->BVDistanceLowerBound(c1, c2); if (d2 < d1) { if (!node->canStop(d2)) @@ -204,7 +204,7 @@ void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1, /** @brief Bounding volume test structure */ struct COAL_LOCAL BVT { /** @brief distance between bvs */ - FCL_REAL d; + CoalScalar d; /** @brief bv indices for a pair of bvs in two models */ unsigned int b1, b2; @@ -308,8 +308,8 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, const CollisionRequest& /*request*/, CollisionResult& result, BVHFrontList* front_list) { - FCL_REAL sqrDistLowerBound = -1, sqrDistLowerBound1 = 0, - sqrDistLowerBound2 = 0; + CoalScalar sqrDistLowerBound = -1, sqrDistLowerBound1 = 0, + sqrDistLowerBound2 = 0; BVHFrontList::iterator front_iter; BVHFrontList append; for (front_iter = front_list->begin(); front_iter != front_list->end(); diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp index ed744b014..0526e4994 100644 --- a/test/accelerated_gjk.cpp +++ b/test/accelerated_gjk.cpp @@ -46,10 +46,10 @@ using coal::Box; using coal::Capsule; +using coal::CoalScalar; using coal::constructPolytopeFromEllipsoid; using coal::Convex; using coal::Ellipsoid; -using coal::FCL_REAL; using coal::GJKSolver; using coal::GJKVariant; using coal::ShapeBase; @@ -107,7 +107,7 @@ BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) { void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Solvers unsigned int max_iterations = 128; - FCL_REAL tolerance = 1e-6; + CoalScalar tolerance = 1e-6; GJK gjk(max_iterations, tolerance); GJK gjk_nesterov(max_iterations, tolerance); gjk_nesterov.gjk_variant = GJKVariant::NesterovAcceleration; @@ -119,7 +119,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Generate random transforms size_t n = 1000; - FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.}; + CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3f identity = Transform3f::Identity(); diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 6cca3275c..584ceb649 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -30,7 +30,7 @@ using namespace coal; bool verbose = false; -FCL_REAL DELTA = 0.001; +CoalScalar DELTA = 0.001; template void makeModel(const std::vector& vertices, @@ -208,7 +208,7 @@ int main(int, char*[]) { makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]); std::vector transforms; // t0 - FCL_REAL extents[] = {-3000, -3000, -3000, 3000, 3000, 3000}; + CoalScalar extents[] = {-3000, -3000, -3000, 3000, 3000, 3000}; std::size_t n = 10000; generateRandomTransforms(extents, transforms, n); diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp index fdbb4dda1..2633febdc 100644 --- a/test/box_box_collision.cpp +++ b/test/box_box_collision.cpp @@ -9,11 +9,11 @@ #include "utility.h" using coal::Box; +using coal::CoalScalar; using coal::collide; using coal::CollisionRequest; using coal::CollisionResult; using coal::ComputeCollision; -using coal::FCL_REAL; using coal::Transform3f; using coal::Vec3f; diff --git a/test/broadphase.cpp b/test/broadphase.cpp index a7d18d070..4bb9b642f 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -75,7 +75,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh = false); -FCL_REAL DELTA = 0.01; +CoalScalar DELTA = 0.01; #if USE_GOOGLEHASH template @@ -146,9 +146,9 @@ void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n) { int n_edge = static_cast(std::floor(std::pow(n, 1 / 3.0))); - FCL_REAL step_size = env_scale * 2 / n_edge; - FCL_REAL delta_size = step_size * 0.05; - FCL_REAL single_size = step_size - 2 * delta_size; + CoalScalar step_size = env_scale * 2 / n_edge; + CoalScalar delta_size = step_size * 0.05; + CoalScalar single_size = step_size - 2 * delta_size; int i = 0; for (; i < n_edge * n_edge * n_edge / 4; ++i) { @@ -216,9 +216,9 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) { int n_edge = static_cast(std::floor(std::pow(n, 1 / 3.0))); - FCL_REAL step_size = env_scale * 2 / n_edge; - FCL_REAL delta_size = step_size * 0.05; - FCL_REAL single_size = step_size - 2 * delta_size; + CoalScalar step_size = env_scale * 2 / n_edge; + CoalScalar delta_size = step_size * 0.05; + CoalScalar single_size = step_size - 2 * delta_size; int i = 0; for (; i < n_edge * n_edge * n_edge / 4; ++i) { @@ -310,9 +310,10 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, - (upper_limit[1] - lower_limit[1]) / 5), - (upper_limit[2] - lower_limit[2]) / 5); + CoalScalar cell_size = + std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, + (upper_limit[1] - lower_limit[1]) / 5), + (upper_limit[2] - lower_limit[2]) / 5); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, // lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager< @@ -459,7 +460,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); @@ -564,7 +565,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::cout << "distance time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp index 1fc940362..2655390fe 100644 --- a/test/broadphase_collision_1.cpp +++ b/test/broadphase_collision_1.cpp @@ -65,11 +65,13 @@ using namespace coal; /// @brief make sure if broadphase algorithms doesn't check twice for the same /// collision object pair -void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_duplicate_check_test(CoalScalar env_scale, + std::size_t env_size, bool verbose = false); /// @brief test for broad phase update -void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_update_collision_test(CoalScalar env_scale, + std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, @@ -197,8 +199,8 @@ struct CollisionFunctionForUniquenessChecking : CollisionCallBackBase { }; //============================================================================== -void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, - bool verbose) { +void broad_phase_duplicate_check_test(CoalScalar env_scale, + std::size_t env_size, bool verbose) { std::vector ts; std::vector timers; @@ -212,7 +214,7 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, managers.push_back(new IntervalTreeCollisionManager()); Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); @@ -264,22 +266,22 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, } // update the environment - FCL_REAL delta_angle_max = - 10 / 360.0 * 2 * boost::math::constants::pi(); - FCL_REAL delta_trans_max = 0.01 * env_scale; + CoalScalar delta_angle_max = + 10 / 360.0 * 2 * boost::math::constants::pi(); + CoalScalar delta_trans_max = 0.01 * env_scale; for (size_t i = 0; i < env.size(); ++i) { - FCL_REAL rand_angle_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * @@ -340,7 +342,7 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } @@ -353,7 +355,8 @@ void broad_phase_duplicate_check_test(FCL_REAL env_scale, std::size_t env_size, std::cout << std::endl; } -void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_update_collision_test(CoalScalar env_scale, + std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { @@ -382,7 +385,7 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - FCL_REAL cell_size = + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2]) / 20); @@ -436,22 +439,22 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, } // update the environment - FCL_REAL delta_angle_max = - 10 / 360.0 * 2 * boost::math::constants::pi(); - FCL_REAL delta_trans_max = 0.01 * env_scale; + CoalScalar delta_angle_max = + 10 / 360.0 * 2 * boost::math::constants::pi(); + CoalScalar delta_trans_max = 0.01 * env_scale; for (size_t i = 0; i < env.size(); ++i) { - FCL_REAL rand_angle_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_x = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_y = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; - FCL_REAL rand_angle_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; - FCL_REAL rand_trans_z = - 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_x = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_y = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; + CoalScalar rand_angle_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_angle_max; + CoalScalar rand_trans_z = + 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * @@ -578,7 +581,7 @@ void broad_phase_update_collision_test(FCL_REAL env_scale, std::size_t env_size, std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp index 7f6f5ca5f..329ebbed3 100644 --- a/test/broadphase_collision_2.cpp +++ b/test/broadphase_collision_2.cpp @@ -62,7 +62,7 @@ using namespace coal; /// @brief test for broad phase collision and self collision -void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); @@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) { #endif } -void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, +void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { @@ -210,9 +210,9 @@ void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); - // FCL_REAL ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); - FCL_REAL ncell_per_axis = 20; - FCL_REAL cell_size = + // CoalScalar ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); + CoalScalar ncell_per_axis = 20; + CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); @@ -365,7 +365,7 @@ void broad_phase_collision_test(FCL_REAL env_scale, std::size_t env_size, std::cout << "collision time" << std::endl; for (size_t i = 0; i < ts.size(); ++i) { - FCL_REAL tmp = 0; + CoalScalar tmp = 0; for (size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } diff --git a/test/broadphase_dynamic_AABB_tree.cpp b/test/broadphase_dynamic_AABB_tree.cpp index 25c64d43d..f13c73e80 100644 --- a/test/broadphase_dynamic_AABB_tree.cpp +++ b/test/broadphase_dynamic_AABB_tree.cpp @@ -58,16 +58,16 @@ struct CallBackData { // the dynamic tree against the `data`. We assume that the first two // parameters are always objects[0] and objects[1] in two possible orders, // so we can safely ignore the second parameter. We do not use the last -// FCL_REAL& parameter, which specifies the distance beyond which the +// CoalScalar& parameter, which specifies the distance beyond which the // pair of objects will be skipped. struct DistanceCallBackDerived : DistanceCallBackBase { - bool distance(CollisionObject* o1, CollisionObject* o2, FCL_REAL& dist) { + bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist) { return distance_callback(o1, o2, &data, dist); } bool distance_callback(CollisionObject* a, CollisionObject*, - void* callback_data, FCL_REAL&) { + void* callback_data, CoalScalar&) { // Unpack the data. CallBackData* data = static_cast(callback_data); const std::vector& objects = *(data->objects); diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 2d3f5e502..4b1f8aed5 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -205,7 +205,7 @@ void testBVHModelTriangles() { BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); pose.setTranslation(Vec3f(0, 0, 0)); - FCL_REAL sqrt2_2 = std::sqrt(2) / 2; + CoalScalar sqrt2_2 = std::sqrt(2) / 2; pose.setQuatRotation(Quatf(sqrt2_2, sqrt2_2, 0, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index 1f429914d..a20f0fe8a 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -285,7 +285,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; - FCL_REAL expected = sqrt(800) - 10; + CoalScalar expected = sqrt(800) - 10; BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6); } diff --git a/test/collision.cpp b/test/collision.cpp index de61e1a8b..76bd88244 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -73,7 +73,7 @@ namespace utf = boost::unit_test::framework; int num_max_contacts = (std::numeric_limits::max)(); BOOST_AUTO_TEST_CASE(OBB_Box_test) { - FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); @@ -87,7 +87,7 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { Transform3f box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; @@ -123,7 +123,7 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { } BOOST_AUTO_TEST_CASE(OBB_shape_test) { - FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); @@ -137,14 +137,14 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) { Transform3f box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; + CoalScalar len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; OBB obb2; GJKSolver solver; @@ -206,7 +206,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) { } BOOST_AUTO_TEST_CASE(OBB_AABB_test) { - FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; + CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; @@ -624,7 +624,7 @@ struct mesh_mesh_run_test { // BOOST_AUTO_TEST_CASE(mesh_mesh) { std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; #else @@ -655,7 +655,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) { std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG std::size_t n = 0; #else diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp index 57506e595..d1dbc9e3f 100644 --- a/test/collision_node_asserts.cpp +++ b/test/collision_node_asserts.cpp @@ -7,7 +7,7 @@ using namespace coal; -constexpr FCL_REAL pi = boost::math::constants::pi(); +constexpr CoalScalar pi = boost::math::constants::pi(); double DegToRad(const double& deg) { static double degToRad = pi / 180.; diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp index 36725fd03..0a02bbe97 100644 --- a/test/contact_patch.cpp +++ b/test/contact_patch.cpp @@ -44,14 +44,14 @@ using namespace coal; BOOST_AUTO_TEST_CASE(box_box_no_collision) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); const Transform3f tf1; Transform3f tf2; // set translation to separate the shapes - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, 2 * halfside + offset)); const size_t num_max_contact = 1; @@ -71,14 +71,14 @@ BOOST_AUTO_TEST_CASE(box_box_no_collision) { } BOOST_AUTO_TEST_CASE(box_sphere) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box(2 * halfside, 2 * halfside, 2 * halfside); const Sphere sphere(halfside); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; @@ -99,7 +99,7 @@ BOOST_AUTO_TEST_CASE(box_sphere) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); - const FCL_REAL tol = 1e-8; + const CoalScalar tol = 1e-8; EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); @@ -109,14 +109,14 @@ BOOST_AUTO_TEST_CASE(box_sphere) { } BOOST_AUTO_TEST_CASE(box_box) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; @@ -141,7 +141,7 @@ BOOST_AUTO_TEST_CASE(box_box) { if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); const size_t expected_size = 4; @@ -168,13 +168,13 @@ BOOST_AUTO_TEST_CASE(box_box) { BOOST_AUTO_TEST_CASE(halfspace_box) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Box box(2 * halfside, 2 * halfside, 2 * halfside); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, halfside - offset)); const size_t num_max_contact = 1; @@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3f(0, 0, 1), tol); @@ -227,14 +227,14 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { BOOST_AUTO_TEST_CASE(halfspace_capsule) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Capsule capsule(radius, height); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; @@ -254,7 +254,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 1; @@ -285,7 +285,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 1; @@ -317,7 +317,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { BOOST_CHECK(patch_res.numContactPatches() == 1); if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = 2; @@ -339,14 +339,14 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { BOOST_AUTO_TEST_CASE(halfspace_cone) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Cone cone(radius, height); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; @@ -366,7 +366,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { if (patch_res.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); const size_t expected_size = ContactPatch::default_preallocated_size; @@ -376,10 +376,10 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; std::array points; - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6)); + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; + const CoalScalar theta = (CoalScalar)(i)*angle_increment; Vec3f point_on_cone_base(std::cos(theta) * cone.radius, std::sin(theta) * cone.radius, -cone.halfLength); expected.addPoint(tf2.transform(point_on_cone_base)); @@ -406,7 +406,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); - const FCL_REAL tol = 1e-8; + const CoalScalar tol = 1e-8; EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); @@ -442,7 +442,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { const Contact& contact = col_res.getContact(0); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(contact_patch.size() == 1); - const FCL_REAL tol = 1e-8; + const CoalScalar tol = 1e-8; EIGEN_VECTOR_IS_APPROX(contact_patch.getPoint(0), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.tf.translation(), contact.pos, tol); EIGEN_VECTOR_IS_APPROX(contact_patch.getNormal(), contact.normal, tol); @@ -464,14 +464,14 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { BOOST_AUTO_TEST_CASE(halfspace_cylinder) { const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Cylinder cylinder(radius, height); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; @@ -484,17 +484,17 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { if (col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const size_t expected_size = ContactPatch::default_preallocated_size; - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; std::array points; - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(6)); + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; + const CoalScalar theta = (CoalScalar)(i)*angle_increment; Vec3f point_on_cone_base(std::cos(theta) * cylinder.radius, std::sin(theta) * cylinder.radius, -cylinder.halfLength); @@ -554,7 +554,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { BOOST_CHECK(patch_res.numContactPatches() == 1); if (col_res.isCollision() && patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; const size_t expected_size = 2; ContactPatch expected(expected_size); @@ -573,14 +573,14 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { } BOOST_AUTO_TEST_CASE(convex_convex) { - const FCL_REAL halfside = 0.5; + const CoalScalar halfside = 0.5; const Convex box1(buildBox(halfside, halfside, halfside)); const Convex box2(buildBox(halfside, halfside, halfside)); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; @@ -605,7 +605,7 @@ BOOST_AUTO_TEST_CASE(convex_convex) { if (patch_res1.numContactPatches() > 0 && patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); const size_t expected_size = 4; @@ -682,7 +682,7 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); // GJK/EPA can return any normal which is in the dual cone @@ -733,7 +733,7 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -782,7 +782,7 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -848,7 +848,7 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -896,7 +896,7 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -944,7 +944,7 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = @@ -1009,7 +1009,7 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { if (patch_res.numContactPatches() > 0) { const Contact& contact = col_res.getContact(0); - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; ContactPatch expected(expected_size); expected.tf.rotation() = diff --git a/test/convex.cpp b/test/convex.cpp index ebcc4a093..bc48ca8d0 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -46,7 +46,7 @@ using namespace coal; BOOST_AUTO_TEST_CASE(convex) { - FCL_REAL l = 1, w = 1, d = 1; + CoalScalar l = 1, w = 1, d = 1; Convex box(buildBox(l, w, d)); // Check neighbors @@ -89,7 +89,7 @@ BOOST_AUTO_TEST_CASE(convex) { template void compareShapeIntersection(const Sa& sa, const Sb& sb, const Transform3f& tf1, const Transform3f& tf2, - FCL_REAL tol = 1e-9) { + CoalScalar tol = 1e-9) { CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); CollisionResult resA, resB; @@ -118,7 +118,7 @@ void compareShapeIntersection(const Sa& sa, const Sb& sb, template void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1, - const Transform3f& tf2, FCL_REAL tol = 1e-9) { + const Transform3f& tf2, CoalScalar tol = 1e-9) { DistanceRequest request(true); DistanceResult resA, resB; @@ -149,8 +149,8 @@ void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1, } BOOST_AUTO_TEST_CASE(compare_convex_box) { - FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; - FCL_REAL l = 1, w = 1, d = 1, eps = 1e-4; + CoalScalar extents[6] = {0, 0, 0, 10, 10, 10}; + CoalScalar l = 1, w = 1, d = 1, eps = 1e-4; Box box(l * 2, w * 2, d * 2); Convex convex_box(buildBox(l, w, d)); diff --git a/test/distance.cpp b/test/distance.cpp index 8066dfa7b..603d57bad 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -53,7 +53,7 @@ using namespace coal; namespace utf = boost::unit_test::framework; bool verbose = false; -FCL_REAL DELTA = 0.001; +CoalScalar DELTA = 0.001; template void distance_Test(const Transform3f& tf, const std::vector& vertices1, @@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE(mesh_distance) { loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); std::vector transforms; // t0 - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; #else diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index 50695a27d..7c034f69a 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -47,13 +47,13 @@ #include "fcl_resources/config.h" using coal::BVHModel; +using coal::CoalScalar; using coal::CollisionGeometryPtr_t; using coal::CollisionObject; using coal::CollisionRequest; using coal::CollisionResult; using coal::DistanceRequest; using coal::DistanceResult; -using coal::FCL_REAL; using coal::OBBRSS; using coal::shared_ptr; using coal::Transform3f; @@ -63,7 +63,7 @@ using coal::Vec3f; bool testDistanceLowerBound(const Transform3f& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2, - FCL_REAL& distance) { + CoalScalar& distance) { Transform3f pose1(tf), pose2; CollisionRequest request; @@ -94,7 +94,7 @@ bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1, } bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1, - const CollisionGeometryPtr_t& m2, FCL_REAL& distance) { + const CollisionGeometryPtr_t& m2, CoalScalar& distance) { Transform3f pose1(tf), pose2; DistanceRequest request; @@ -132,14 +132,14 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { m2->endModel(); std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; generateRandomTransforms(extents, transforms, n); // collision for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2, col3; col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); col2 = testDistance(transforms[i], m1, m2, distance); @@ -167,12 +167,12 @@ BOOST_AUTO_TEST_CASE(box_sphere) { M2.setIdentity(); std::vector transforms; - FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; + CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(M1, sphere, box, distanceLowerBound); col2 = testDistance(M1, sphere, box, distance); @@ -180,7 +180,7 @@ BOOST_AUTO_TEST_CASE(box_sphere) { BOOST_CHECK(distanceLowerBound <= distance); for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], sphere, box, distanceLowerBound); @@ -204,12 +204,12 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { M2.setIdentity(); std::vector transforms; - FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; + CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; generateRandomTransforms(extents, transforms, n); - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(M1, sphere1, sphere2, distanceLowerBound); col2 = testDistance(M1, sphere1, sphere2, distance); @@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { BOOST_CHECK(distanceLowerBound <= distance); for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], sphere1, sphere2, distanceLowerBound); @@ -246,14 +246,14 @@ BOOST_AUTO_TEST_CASE(box_mesh) { m1->endModel(); std::vector transforms; - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; generateRandomTransforms(extents, transforms, n); // collision for (std::size_t i = 0; i < transforms.size(); ++i) { - FCL_REAL distanceLowerBound, distance; + CoalScalar distanceLowerBound, distance; bool col1, col2; col1 = testDistanceLowerBound(transforms[i], m1, m2, distanceLowerBound); col2 = testDistance(transforms[i], m1, m2, distance); diff --git a/test/frontlist.cpp b/test/frontlist.cpp index 0728f6bb9..2ed4dd76c 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -86,8 +86,8 @@ BOOST_AUTO_TEST_CASE(front_list) { std::vector transforms; // t0 std::vector transforms2; // t1 - FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; - FCL_REAL delta_trans[] = {1, 1, 1}; + CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; + CoalScalar delta_trans[] = {1, 1, 1}; #ifndef NDEBUG // if debug mode std::size_t n = 2; #else diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index ec07c2d50..c391e6a29 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -50,9 +50,9 @@ using namespace coal; -FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; +CoalScalar extents[6] = {0, 0, 0, 10, 10, 10}; -FCL_REAL tol_gjk = 0.01; +CoalScalar tol_gjk = 0.01; GJKSolver solver1; GJKSolver solver2; @@ -82,7 +82,7 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3f& tf2, const Vec3f& contact_or_normal, const Vec3f& expected_contact_or_normal, - bool check_opposite_normal, FCL_REAL tol) { + bool check_opposite_normal, CoalScalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << ".\n" @@ -110,8 +110,8 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, template void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, FCL_REAL depth, - FCL_REAL expected_depth, FCL_REAL tol) { + const Transform3f& tf2, CoalScalar depth, + CoalScalar expected_depth, CoalScalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << ".\n" @@ -128,10 +128,10 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, template void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, const Vec3f& contact, - Vec3f* expected_point, FCL_REAL depth, - FCL_REAL* expected_depth, const Vec3f& normal, + Vec3f* expected_point, CoalScalar depth, + CoalScalar* expected_depth, const Vec3f& normal, Vec3f* expected_normal, bool check_opposite_normal, - FCL_REAL tol) { + CoalScalar tol) { if (expected_point) { bool contact_equal = isEqual(contact, *expected_point, tol); FCL_CHECK(contact_equal); @@ -165,9 +165,10 @@ template void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, bool expect_collision, Vec3f* expected_point = NULL, - FCL_REAL* expected_depth = NULL, + CoalScalar* expected_depth = NULL, Vec3f* expected_normal = NULL, - bool check_opposite_normal = false, FCL_REAL tol = 1e-9) { + bool check_opposite_normal = false, + CoalScalar tol = 1e-9) { CollisionRequest request; CollisionResult result; @@ -244,7 +245,7 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) { generateRandomTransform(extents, transform); // Vec3f point; - // FCL_REAL depth; + // CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -363,7 +364,7 @@ void testBoxBoxContactPoints(const Matrix3f& R) { solver1.gjk_tolerance = 1e-5; solver1.epa_tolerance = 1e-5; const bool compute_penetration = true; - FCL_REAL distance = solver1.shapeDistance( + CoalScalar distance = solver1.shapeDistance( s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); FCL_CHECK(distance <= 0); @@ -391,11 +392,11 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { generateRandomTransform(extents, transform); // Vec3f point; - // FCL_REAL depth; + // CoalScalar depth; Vec3f normal; Quatf q; - q = AngleAxis((FCL_REAL)3.140 / 6, UnitZ); + q = AngleAxis((CoalScalar)3.140 / 6, UnitZ); tf1 = Transform3f(); tf2 = Transform3f(); @@ -461,7 +462,7 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { generateRandomTransform(extents, transform); // Vec3f point; - // FCL_REAL depth; + // CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -518,7 +519,7 @@ BOOST_AUTO_TEST_CASE(distance_spherebox) { distance(&sphere, Transform3f(rotSphere, trSphere), &box, Transform3f(rotBox, trBox), DistanceRequest(true), result); - FCL_REAL eps = Eigen::NumTraits::epsilon(); + CoalScalar eps = Eigen::NumTraits::epsilon(); BOOST_CHECK_CLOSE(result.min_distance, 3., eps); EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3f(0, 1, 0), eps); EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3f(0, 4, 0), eps); @@ -536,7 +537,7 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) { generateRandomTransform(extents, transform); // Vec3f point; - // FCL_REAL depth; + // CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -601,7 +602,7 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { generateRandomTransform(extents, transform); // Vec3f point; - // FCL_REAL depth; + // CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -664,7 +665,7 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { generateRandomTransform(extents, transform); // Vec3f point; - // FCL_REAL depth; + // CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -742,7 +743,7 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { generateRandomTransform(extents, transform); // Vec3f point; - // FCL_REAL depth; + // CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -1281,7 +1282,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -1370,11 +1371,11 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; Vec3f p1, p2; - FCL_REAL eps = 1e-6; + CoalScalar eps = 1e-6; tf1 = Transform3f(Vec3f(eps, 0, 0)); tf2 = Transform3f(); depth = -10 + eps; @@ -1479,7 +1480,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -1573,7 +1574,7 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -1666,7 +1667,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -1907,7 +1908,7 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -2130,7 +2131,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -2371,11 +2372,11 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; Vec3f p1, p2; - FCL_REAL eps = 1e-6; + CoalScalar eps = 1e-6; tf1 = Transform3f(Vec3f(eps, 0, 0)); tf2 = Transform3f(); p1 << -5 + eps, 0, 0; @@ -2675,7 +2676,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; tf1 = Transform3f(); @@ -2916,11 +2917,11 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { generateRandomTransform(extents, transform); Vec3f contact; - FCL_REAL depth; + CoalScalar depth; Vec3f normal; Vec3f p1, p2; - FCL_REAL eps = 1e-6; + CoalScalar eps = 1e-6; tf1 = Transform3f(Vec3f(eps, 0, 0)); tf2 = Transform3f(); p1 << -5 + eps, 0, -5; @@ -3215,14 +3216,14 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { Vec3f normal; Vec3f contact; - FCL_REAL distance; + CoalScalar distance; Transform3f transform; generateRandomTransform(extents, transform); { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset = 3.14; + CoalScalar offset = 3.14; Plane plane1(n, offset); Plane plane2(n, offset); @@ -3249,8 +3250,8 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 + 1.19841; + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 + 1.19841; Plane plane1(n, offset1); Plane plane2(n, offset2); @@ -3267,8 +3268,8 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 - 1.19841; + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 - 1.19841; Plane plane1(n, offset1); Plane plane2(n, offset2); @@ -3285,10 +3286,10 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { { Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + CoalScalar offset1 = 3.14; Plane plane1(n1, offset1); Vec3f n2(0, 0, 1); - FCL_REAL offset2 = -2.13; + CoalScalar offset2 = -2.13; Plane plane2(n2, offset2); tf1.setIdentity(); @@ -3307,10 +3308,10 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { { Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + CoalScalar offset1 = 3.14; Plane plane1(n1, offset1); Vec3f n2(1, 1, 1); - FCL_REAL offset2 = -2.13; + CoalScalar offset2 = -2.13; Plane plane2(n2, offset2); tf1.setIdentity(); @@ -3335,14 +3336,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { Vec3f normal; Vec3f contact; - FCL_REAL distance; + CoalScalar distance; Transform3f transform; generateRandomTransform(extents, transform); { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset = 3.14; + CoalScalar offset = 3.14; Halfspace hf1(n, offset); Halfspace hf2(n, offset); @@ -3361,8 +3362,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 + 1.19841; + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 + 1.19841; Halfspace hf1(n, offset1); Halfspace hf2(n, offset2); @@ -3381,8 +3382,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 - 1.19841; + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 - 1.19841; Halfspace hf1(n, offset1); Halfspace hf2(-n, -offset2); @@ -3402,10 +3403,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { { Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + CoalScalar offset1 = 3.14; Halfspace hf1(n1, offset1); Vec3f n2(0, 0, 1); - FCL_REAL offset2 = -2.13; + CoalScalar offset2 = -2.13; Halfspace hf2(n2, offset2); tf1.setIdentity(); @@ -3423,10 +3424,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { { Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + CoalScalar offset1 = 3.14; Halfspace hf1(n1, offset1); Vec3f n2(1, 1, 1); - FCL_REAL offset2 = -2.13; + CoalScalar offset2 = -2.13; Halfspace hf2(n2, offset2); tf1.setIdentity(); @@ -3451,14 +3452,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { Vec3f normal; Vec3f contact; - FCL_REAL distance; + CoalScalar distance; Transform3f transform; generateRandomTransform(extents, transform); { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset = 3.14; + CoalScalar offset = 3.14; Halfspace hf(n, offset); Plane plane(n, offset); @@ -3478,8 +3479,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 + 1.19841; + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 + 1.19841; Halfspace hf(n, offset1); Plane plane(n, offset2); @@ -3499,8 +3500,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { { Vec3f n = Vec3f::Random().normalized(); - FCL_REAL offset1 = 3.14; - FCL_REAL offset2 = offset1 - 1.19841; + CoalScalar offset1 = 3.14; + CoalScalar offset2 = offset1 - 1.19841; Halfspace hf(n, offset1); Plane plane(n, offset2); @@ -3520,10 +3521,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { { Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + CoalScalar offset1 = 3.14; Halfspace hf(n1, offset1); Vec3f n2(0, 0, 1); - FCL_REAL offset2 = -2.13; + CoalScalar offset2 = -2.13; Plane plane(n2, offset2); tf1.setIdentity(); @@ -3541,10 +3542,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { { Vec3f n1(1, 0, 0); - FCL_REAL offset1 = 3.14; + CoalScalar offset1 = 3.14; Halfspace hf(n1, offset1); Vec3f n2(1, 1, 1); - FCL_REAL offset2 = -2.13; + CoalScalar offset2 = -2.13; Plane plane(n2, offset2); tf1.setIdentity(); @@ -3572,7 +3573,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { Transform3f transform; generateRandomTransform(extents, transform); - FCL_REAL dist = -1; + CoalScalar dist = -1; dist = solver1.shapeDistance( s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, @@ -3644,7 +3645,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { Transform3f transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), compute_penetration, closest_p1, closest_p2, @@ -3773,11 +3774,11 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { Transform3f transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; int N = 10; for (int i = 0; i < N + 1; ++i) { - FCL_REAL dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N); + CoalScalar dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N); dist = solver1.shapeDistance(s1, Transform3f(Vec3f(dbox, 0., 0.)), s2, Transform3f(), compute_penetration, closest_p1, closest_p2, normal); @@ -3834,7 +3835,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { Transform3f transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; { // The following situations corresponds to the case where the two cylinders @@ -3852,7 +3853,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. - FCL_REAL epa_tolerance_backup = solver1.epa_tolerance; + CoalScalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; @@ -3902,7 +3903,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { Transform3f transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; { // The following situations corresponds to the case where the two cones @@ -3920,7 +3921,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. - FCL_REAL epa_tolerance_backup = solver1.epa_tolerance; + CoalScalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; @@ -3970,7 +3971,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { Transform3f transform; generateRandomTransform(extents, transform); - FCL_REAL dist; + CoalScalar dist; { // The following situations corresponds to the case where the two cones @@ -3988,7 +3989,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { // To handle the superposing case, we have to decrease the tolerance of EPA // and allow it to work with more vertices and faces. - FCL_REAL epa_tolerance_backup = solver1.epa_tolerance; + CoalScalar epa_tolerance_backup = solver1.epa_tolerance; size_t epa_max_iterations_backup = solver1.epa_max_iterations; solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; @@ -4031,12 +4032,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { template void testReversibleShapeDistance(const S1& s1, const S2& s2, - FCL_REAL distance) { + CoalScalar distance) { Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); - FCL_REAL distA; - FCL_REAL distB; + CoalScalar distA; + CoalScalar distB; Vec3f p1A; Vec3f p1B; Vec3f p2A; @@ -4087,7 +4088,7 @@ BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) { // Use sufficiently long distance so that all the primitive shapes CANNOT // intersect - FCL_REAL distance = 15.0; + CoalScalar distance = 15.0; // If new shape distance algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection diff --git a/test/gjk.cpp b/test/gjk.cpp index ee54597bd..cdfb4d5e7 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -46,7 +46,7 @@ #include "utility.h" -using coal::FCL_REAL; +using coal::CoalScalar; using coal::GJKSolver; using coal::GJKVariant; using coal::Matrix3f; @@ -55,10 +55,10 @@ using coal::Transform3f; using coal::TriangleP; using coal::Vec3f; -typedef Eigen::Matrix vector_t; -typedef Eigen::Matrix vector6_t; -typedef Eigen::Matrix vector4_t; -typedef Eigen::Matrix matrix_t; +typedef Eigen::Matrix vector_t; +typedef Eigen::Matrix vector6_t; +typedef Eigen::Matrix vector4_t; +typedef Eigen::Matrix matrix_t; struct Result { bool collision; @@ -81,11 +81,11 @@ void test_gjk_distance_triangle_triangle( Transform3f tf1, tf2; Vec3f p1, p2, a1, a2; Matrix3f M; - FCL_REAL distance(sqrt(-1)); + CoalScalar distance(sqrt(-1)); clock_t start, end; std::size_t nCol = 0, nDiff = 0; - FCL_REAL eps = 1e-7; + CoalScalar eps = 1e-7; Results_t results(N); for (std::size_t i = 0; i < N; ++i) { Vec3f P1_loc(Vec3f::Random()), P2_loc(Vec3f::Random()), @@ -159,7 +159,7 @@ void test_gjk_distance_triangle_triangle( ++nCol; // check that moving triangle 2 by the penetration depth in the // direction of the normal makes the triangles collision free. - FCL_REAL penetration_depth(-distance); + CoalScalar penetration_depth(-distance); assert(penetration_depth >= 0); tf2.setTranslation((penetration_depth + 10 - 4) * normal); result.clear(); @@ -312,17 +312,17 @@ void test_gjk_distance_triangle_triangle( } std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl + totalTimeGjkColl << ", " - << FCL_REAL(totalTimeGjkNoColl + totalTimeGjkColl) / - FCL_REAL(CLOCKS_PER_SEC * N) + << CoalScalar(totalTimeGjkNoColl + totalTimeGjkColl) / + CoalScalar(CLOCKS_PER_SEC * N) << "s" << std::endl; std::cerr << "-- Collisions -------------------------" << std::endl; std::cerr << "Total / average time gjk: " << totalTimeGjkColl << ", " - << FCL_REAL(totalTimeGjkColl) / FCL_REAL(CLOCKS_PER_SEC * nCol) + << CoalScalar(totalTimeGjkColl) / CoalScalar(CLOCKS_PER_SEC * nCol) << "s" << std::endl; std::cerr << "-- No collisions -------------------------" << std::endl; std::cerr << "Total / average time gjk: " << totalTimeGjkNoColl << ", " - << FCL_REAL(totalTimeGjkNoColl) / - FCL_REAL(CLOCKS_PER_SEC * (N - nCol)) + << CoalScalar(totalTimeGjkNoColl) / + CoalScalar(CLOCKS_PER_SEC * (N - nCol)) << "s" << std::endl; } @@ -334,15 +334,15 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) { test_gjk_distance_triangle_triangle(true); } -void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, +void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray, double swept_sphere_radius, bool use_gjk_nesterov_acceleration) { using namespace coal; - const FCL_REAL r = 1.0; + const CoalScalar r = 1.0; Sphere sphere(r); sphere.setSweptSphereRadius(swept_sphere_radius); - typedef Eigen::Matrix Vec4f; + typedef Eigen::Matrix Vec4f; Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3f::Zero()); Transform3f tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray); diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp index 926ed28a3..4028b8690 100644 --- a/test/gjk_asserts.cpp +++ b/test/gjk_asserts.cpp @@ -7,7 +7,7 @@ using namespace coal; -constexpr FCL_REAL pi = boost::math::constants::pi(); +constexpr CoalScalar pi = boost::math::constants::pi(); double DegToRad(const double& deg) { static double degToRad = pi / 180.; diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index 6fccdb200..5283e4514 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -47,7 +47,7 @@ #include "utility.h" using coal::Box; -using coal::FCL_REAL; +using coal::CoalScalar; using coal::GJKConvergenceCriterion; using coal::GJKConvergenceCriterionType; using coal::GJKSolver; @@ -96,7 +96,7 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, // by default GJK uses the VDB convergence criterion, which is relative. GJK gjk1(max_iterations, 1e-6); - FCL_REAL tol; + CoalScalar tol; switch (cv_type) { // need to lower the tolerance when absolute case GJKConvergenceCriterionType::Absolute: @@ -122,7 +122,7 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, // Generate random transforms size_t n = 1000; - FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.}; + CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3f identity = Transform3f::Identity(); diff --git a/test/hfields.cpp b/test/hfields.cpp index 7492bd9d4..0042cd802 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -58,9 +58,9 @@ using namespace coal; template void test_constant_hfields(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, - const FCL_REAL min_altitude, - const FCL_REAL max_altitude) { - const FCL_REAL x_dim = 1., y_dim = 2.; + const CoalScalar min_altitude, + const CoalScalar max_altitude) { + const CoalScalar x_dim = 1., y_dim = 2.; const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); @@ -110,7 +110,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // No collision case { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( @@ -137,7 +137,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Collision case { - const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( @@ -169,7 +169,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // No collision case { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( @@ -196,7 +196,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Collision case { - const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( @@ -228,7 +228,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Collision case { - const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( @@ -256,7 +256,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, } BOOST_AUTO_TEST_CASE(building_constant_hfields) { - const FCL_REAL max_altitude = 1., min_altitude = 0.; + const CoalScalar max_altitude = 1., min_altitude = 0.; test_constant_hfields(2, 2, min_altitude, max_altitude); // Simple case @@ -272,9 +272,9 @@ BOOST_AUTO_TEST_CASE(building_constant_hfields) { template void test_negative_security_margin(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny, - const FCL_REAL min_altitude, - const FCL_REAL max_altitude) { - const FCL_REAL x_dim = 1., y_dim = 2.; + const CoalScalar min_altitude, + const CoalScalar max_altitude) { + const CoalScalar x_dim = 1., y_dim = 2.; const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); @@ -294,7 +294,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // No collision case { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( @@ -321,7 +321,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // Collision case - positive security_margin { - const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( @@ -349,7 +349,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // Collision case { - const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( @@ -376,7 +376,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // No collision case - negative security_margin { - const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( @@ -404,7 +404,7 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, } BOOST_AUTO_TEST_CASE(negative_security_margin) { - const FCL_REAL max_altitude = 1., min_altitude = 0.; + const CoalScalar max_altitude = 1., min_altitude = 0.; // test_negative_security_margin(100, 100, min_altitude, // max_altitude); @@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); - const FCL_REAL dim_square = 0.5; + const CoalScalar dim_square = 0.5; const Eigen::Array hole = (X.array().abs() < dim_square) && (Y.array().abs() < dim_square); @@ -463,7 +463,7 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); - const FCL_REAL dim_hole = 1; + const CoalScalar dim_hole = 1; const Eigen::Array hole = (X.array().square() + Y.array().square() <= dim_hole); @@ -483,7 +483,7 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { const Transform3f sphere_pos(Vec3f(0., 0., 1.)); const Transform3f hfield_pos; - const FCL_REAL thresholds[3] = {0., 0.01, -0.005}; + const CoalScalar thresholds[3] = {0., 0.01, -0.005}; for (int i = 0; i < 3; ++i) { CollisionResult result; @@ -515,7 +515,8 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { } } -bool isApprox(const FCL_REAL v1, const FCL_REAL v2, const FCL_REAL tol = 1e-6) { +bool isApprox(const CoalScalar v1, const CoalScalar v2, + const CoalScalar tol = 1e-6) { return std::fabs(v1 - v2) <= tol; } @@ -529,10 +530,10 @@ Vec3f computeFaceNormal(const Triangle& triangle, } BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { - const FCL_REAL sphere_radius = 1.; + const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); MatrixXf altitutes(2, 2); - FCL_REAL altitude_value = 1.; + CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; @@ -676,10 +677,10 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { typedef HFNodeBase::FaceOrientation FaceOrientation; - const FCL_REAL sphere_radius = 1.; + const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); MatrixXf altitutes(3, 3); - FCL_REAL altitude_value = 1.; + CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; @@ -713,10 +714,10 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { } BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { - const FCL_REAL sphere_radius = 1.; + const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); MatrixXf altitutes(2, 2); - FCL_REAL altitude_value = 1.; + CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); typedef AABB BV; diff --git a/test/math.cpp b/test/math.cpp index 03827c6ff..ad44b43f1 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -105,7 +105,7 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { BOOST_CHECK(v1.dot(v2) == 26); } -Vec3f rotate(Vec3f input, FCL_REAL w, Vec3f vec) { +Vec3f rotate(Vec3f input, CoalScalar w, Vec3f vec) { return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input + 2 * w * vec.cross(input); } diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp index 81176f5ee..e440afdc7 100644 --- a/test/normal_and_nearest_points.cpp +++ b/test/normal_and_nearest_points.cpp @@ -75,9 +75,9 @@ template void test_normal_and_nearest_points( const ShapeType1& o1, const ShapeType2& o2, size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS, - FCL_REAL gjk_tolerance = GJK_DEFAULT_TOLERANCE, + CoalScalar gjk_tolerance = GJK_DEFAULT_TOLERANCE, size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS, - FCL_REAL epa_tolerance = EPA_DEFAULT_TOLERANCE) { + CoalScalar epa_tolerance = EPA_DEFAULT_TOLERANCE) { // Generate random poses for o2 #ifndef NDEBUG // if debug mode std::size_t n = 10; @@ -86,13 +86,13 @@ void test_normal_and_nearest_points( #endif // We want to make sure we generate poses that are in collision // so we take a relatively small extent for the random poses - FCL_REAL extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5}; + CoalScalar extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3f tf1 = Transform3f::Identity(); CollisionRequest colreq; - colreq.distance_upper_bound = std::numeric_limits::max(); + colreq.distance_upper_bound = std::numeric_limits::max(); // For strictly convex shapes, the default tolerance of EPA is way too low. // Because EPA is basically trying to fit a polytope to a strictly convex // surface, it might take it a lot of iterations to converge to a low @@ -118,10 +118,10 @@ void test_normal_and_nearest_points( CollisionResult colres; DistanceResult distres; size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); - FCL_REAL dist = distance(&o1, tf1, &o2, tf2, distreq, distres); + CoalScalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres); - const FCL_REAL dummy_precision(100 * - std::numeric_limits::epsilon()); + const CoalScalar dummy_precision( + 100 * std::numeric_limits::epsilon()); if (col) { BOOST_CHECK(dist <= 0.); BOOST_CHECK_CLOSE(dist, distres.min_distance, dummy_precision); @@ -147,13 +147,13 @@ void test_normal_and_nearest_points( // Separate the shapes Transform3f new_tf1 = tf1; - FCL_REAL eps = 1e-2; + CoalScalar eps = 1e-2; new_tf1.setTranslation(tf1.getTranslation() + separation_vector - eps * contact.normal); CollisionResult new_colres; DistanceResult new_distres; size_t new_col = collide(&o1, new_tf1, &o2, tf2, colreq, new_colres); - FCL_REAL new_dist = + CoalScalar new_dist = distance(&o1, new_tf1, &o2, tf2, distreq, new_distres); BOOST_CHECK(new_dist > 0); BOOST_CHECK(!new_col); @@ -199,14 +199,14 @@ void test_normal_and_nearest_points( // If you translate one of the cones by the separation vector and it // happens to be parallel to the axis of the cone, the two shapes will // still be disjoint. - FCL_REAL eps = 1e-2; + CoalScalar eps = 1e-2; Transform3f new_tf1 = tf1; new_tf1.setTranslation(tf1.getTranslation() + separation_vector + eps * distres.normal); CollisionResult new_colres; DistanceResult new_distres; collide(&o1, new_tf1, &o2, tf2, colreq, new_colres); - FCL_REAL new_dist = + CoalScalar new_dist = distance(&o1, new_tf1, &o2, tf2, distreq, new_distres); BOOST_CHECK(new_dist < dist); BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist, @@ -241,17 +241,18 @@ void test_normal_and_nearest_points( } template -Eigen::Matrix generateRandomVector(FCL_REAL min, - FCL_REAL max) { - typedef Eigen::Matrix VecType; +Eigen::Matrix generateRandomVector(CoalScalar min, + CoalScalar max) { + typedef Eigen::Matrix VecType; // Generate a random vector in the [min, max] range VecType v = VecType::Random() * (max - min) * 0.5 + VecType::Ones() * (max + min) * 0.5; return v; } -FCL_REAL generateRandomNumber(FCL_REAL min, FCL_REAL max) { - FCL_REAL r = static_cast(rand()) / static_cast(RAND_MAX); +CoalScalar generateRandomNumber(CoalScalar min, CoalScalar max) { + CoalScalar r = + static_cast(rand()) / static_cast(RAND_MAX); r = 2 * r - 1.0; return r * (max - min) * 0.5 + (max + min) * 0.5; } @@ -269,7 +270,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_sphere) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_capsule) { for (size_t i = 0; i < 10; ++i) { Vec2d radii = generateRandomVector<2>(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Sphere(radii(0))); shared_ptr o2(new Capsule(radii(1), h)); @@ -300,9 +301,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_mesh) { o2_.points, o2_.num_points, o2_.polygons, o2_.num_polygons)); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = GJK_DEFAULT_TOLERANCE; + CoalScalar gjk_tolerance = GJK_DEFAULT_TOLERANCE; size_t epa_max_iterations = EPA_DEFAULT_MAX_ITERATIONS; - FCL_REAL epa_tolerance = EPA_DEFAULT_TOLERANCE; + CoalScalar epa_tolerance = EPA_DEFAULT_TOLERANCE; test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); @@ -328,12 +329,12 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_ellipsoid) { o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); shared_ptr o2(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; // With EPA's tolerance set at 1e-3, the precision on the normal, contact // points and penetration depth is on the order of the milimeter. However, // EPA (currently) cannot converge to lower tolerances on strictly convex // shapes in a reasonable amount of iterations. - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; test_normal_and_nearest_points(*o1.get(), *o2.get(), GJK_DEFAULT_MAX_ITERATIONS, gjk_tolerance, EPA_DEFAULT_MAX_ITERATIONS, epa_tolerance); @@ -349,13 +350,13 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_ellipsoid) { shared_ptr o2(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; // With EPA's tolerance set at 1e-3, the precision on the normal, contact // points and penetration depth is on the order of the milimeter. However, // EPA (currently) cannot converge to lower tolerances on strictly convex // shapes in a reasonable amount of iterations. size_t epa_max_iterations = 250; - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; // For EPA on ellipsoids, we need to increase the number of iterations in // this test. This is simply because this test checks **a lot** of cases and // it can generate some of the worst cases for EPA. We don't want to @@ -370,7 +371,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_ellipsoid) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(0.05, 1.0))); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -383,7 +384,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(0.05, 1.0))); - FCL_REAL offset = 0.1; + CoalScalar offset = 0.1; Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -395,10 +396,10 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Capsule(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -411,7 +412,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(0.05, 1.0))); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -424,7 +425,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(0.05, 1.0))); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -440,7 +441,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_halfspace) { Ellipsoid(generateRandomVector<3>(0.05, 1.0))); shared_ptr> o1(new Convex( o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -458,9 +459,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_cylinder) { shared_ptr o2(new Cylinder(r(1), h(1))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; size_t epa_max_iterations = 250; - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); @@ -478,9 +479,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_ellipsoid) { shared_ptr o2(new Cylinder(r(1), h(1))); size_t gjk_max_iterations = GJK_DEFAULT_MAX_ITERATIONS; - FCL_REAL gjk_tolerance = 1e-6; + CoalScalar gjk_tolerance = 1e-6; size_t epa_max_iterations = 250; - FCL_REAL epa_tolerance = 1e-3; + CoalScalar epa_tolerance = 1e-3; test_normal_and_nearest_points(*o1.get(), *o2.get(), gjk_max_iterations, gjk_tolerance, epa_max_iterations, epa_tolerance); @@ -492,10 +493,10 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_ellipsoid) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cone(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -507,10 +508,10 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cylinder(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -522,10 +523,10 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cone(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -537,10 +538,10 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cylinder(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -552,10 +553,10 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL r = generateRandomNumber(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar r = generateRandomNumber(0.05, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Capsule(r, h)); - FCL_REAL offset = generateRandomNumber(-0.5, 0.5); + CoalScalar offset = generateRandomNumber(-0.5, 0.5); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -580,7 +581,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_capsule) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) { for (size_t i = 0; i < 10; ++i) { Vec2d r = generateRandomVector<2>(0.05, 1.0); - FCL_REAL h = generateRandomNumber(0.15, 1.0); + CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Sphere(r(0))); shared_ptr o2(new Cylinder(r(1), h)); @@ -591,7 +592,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL offset = generateRandomNumber(0.15, 1.0); + CoalScalar offset = generateRandomNumber(0.15, 1.0); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); @@ -604,7 +605,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_plane) { for (size_t i = 0; i < 10; ++i) { - FCL_REAL offset = generateRandomNumber(0.15, 1.0); + CoalScalar offset = generateRandomNumber(0.15, 1.0); Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); @@ -623,14 +624,14 @@ void test_normal_and_nearest_points(const BVHModel& o1, #else size_t n = 10000; #endif - FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.}; + CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Transform3f tf1 = Transform3f::Identity(); transforms[0] = Transform3f::Identity(); CollisionRequest colreq; - colreq.distance_upper_bound = std::numeric_limits::max(); + colreq.distance_upper_bound = std::numeric_limits::max(); colreq.num_max_contacts = 100; CollisionResult colres; DistanceRequest distreq; @@ -641,7 +642,7 @@ void test_normal_and_nearest_points(const BVHModel& o1, colres.clear(); distres.clear(); size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); - FCL_REAL dist = distance(&o1, tf1, &o2, tf2, distreq, distres); + CoalScalar dist = distance(&o1, tf1, &o2, tf2, distreq, distres); if (col) { BOOST_CHECK(dist <= 0.); @@ -685,7 +686,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) { generateBVHModel(o1, *box_ptr, Transform3f()); o1.buildConvexRepresentation(false); - FCL_REAL offset = 0.1; + CoalScalar offset = 0.1; Vec3f n = Vec3f::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); diff --git a/test/obb.cpp b/test/obb.cpp index 8405484b8..d8db1dd85 100644 --- a/test/obb.cpp +++ b/test/obb.cpp @@ -48,7 +48,7 @@ using namespace coal; -void randomOBBs(Vec3f& a, Vec3f& b, FCL_REAL extentNorm) { +void randomOBBs(Vec3f& a, Vec3f& b, CoalScalar extentNorm) { // Extent norm is between 0 and extentNorm on each axis // a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); // b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); @@ -58,13 +58,13 @@ void randomOBBs(Vec3f& a, Vec3f& b, FCL_REAL extentNorm) { } void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b, - const FCL_REAL extentNorm) { + const CoalScalar extentNorm) { // TODO Should we scale T to a and b norm ? (void)a; (void)b; (void)extentNorm; - FCL_REAL N = a.norm() + b.norm(); + CoalScalar N = a.norm() + b.norm(); // A translation of norm N ensures there is no collision. // Set translation to be between 0 and 2 * N; T = (Vec3f::Random() / sqrt(3)) * 1.5 * N; @@ -90,7 +90,7 @@ typedef std::chrono::high_resolution_clock clock_type; typedef clock_type::duration duration_type; const char* sep = ",\t"; -const FCL_REAL eps = 1.5e-7; +const CoalScalar eps = 1.5e-7; const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, ", ", // Coeff separator @@ -104,7 +104,7 @@ const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, namespace obbDisjoint_impls { /// @return true if OBB are disjoint. bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - FCL_REAL& distance) { + CoalScalar& distance) { GJKSolver gjk; Box ba(2 * a), bb(2 * b); Transform3f tfa, tfb(B, T); @@ -116,8 +116,8 @@ bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, return (distance > gjk.getDistancePrecision(compute_penetration)); } -inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf) { +inline CoalScalar _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, + const Vec3f& b, const Matrix3f& Bf) { Vec3f AABB_corner; /* This seems to be slower AABB_corner.noalias() = T.cwiseAbs () - a; @@ -132,19 +132,19 @@ inline FCL_REAL _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a; #endif // */ - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm(); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); } -inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf) { +inline CoalScalar _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, + const Vec3f& a, const Vec3f& b, + const Matrix3f& Bf) { /* Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a); - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm (); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm (); /*/ #if MANUAL_PRODUCT - FCL_REAL s, t = 0; + CoalScalar s, t = 0; s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; if (s > 0) t += s * s; s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; @@ -154,14 +154,14 @@ inline FCL_REAL _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, return t; #else Vec3f AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); - return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm(); + return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); #endif // */ } int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { + const Vec3f& b, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { int id = 0; Matrix3f Bf(B.cwiseAbs()); @@ -179,14 +179,15 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, int ja = 1, ka = 2, jb = 1, kb = 2; for (int ia = 0; ia < 3; ++ia) { for (int ib = 0; ib < 3; ++ib) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + const CoalScalar diff = + fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -194,7 +195,7 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -216,8 +217,8 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, // ------------------------ 0 -------------------------------------- bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { + const Vec3f& b, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { Matrix3f Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin @@ -231,14 +232,15 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, int ja = 1, ka = 2, jb = 1, kb = 2; for (int ia = 0; ia < 3; ++ia) { for (int ib = 0; ib < 3; ++ib) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + const CoalScalar diff = + fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -246,7 +248,7 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -268,10 +270,10 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, // ------------------------ 1 -------------------------------------- bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - FCL_REAL t, s; - FCL_REAL diff; + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + CoalScalar t, s; + CoalScalar diff; // Matrix3f Bf = abs(B); // Bf += reps; @@ -299,7 +301,7 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); - FCL_REAL sinus2; + CoalScalar sinus2; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || @@ -458,8 +460,8 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, // ------------------------ 2 -------------------------------------- bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { Matrix3f Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin @@ -470,13 +472,13 @@ bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, if (squaredLowerBoundDistance > breakDistance2) return true; // A0 x B0 - FCL_REAL t, s; + CoalScalar t, s; s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); assert(t == fabs(s)); - FCL_REAL sinus2; - FCL_REAL diff; + CoalScalar sinus2; + CoalScalar diff; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || @@ -638,16 +640,16 @@ template 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -655,7 +657,7 @@ struct loop_case_1 { } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -669,8 +671,8 @@ struct loop_case_1 { bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { Matrix3f Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin @@ -718,16 +720,16 @@ template struct loop_case_2 { static inline bool run(int ia, int ja, int ka, const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + const Matrix3f& Bf, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); + const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); // We need to divide by the norm || Aia x Bib || // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 if (diff > 0) { - FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); + CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 > 1e-6) { squaredLowerBoundDistance = diff * diff / sinus2; if (squaredLowerBoundDistance > breakDistance2) { @@ -735,7 +737,7 @@ struct loop_case_2 { } } /* // or - FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); + CoalScalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); squaredLowerBoundDistance = diff * diff; if (squaredLowerBoundDistance > breakDistance2 * sinus2) { squaredLowerBoundDistance /= sinus2; @@ -749,8 +751,8 @@ struct loop_case_2 { bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, - const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { + const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { Matrix3f Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin @@ -781,10 +783,10 @@ bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, // ------------------------ 5 -------------------------------------- bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL& breakDistance2, - FCL_REAL& squaredLowerBoundDistance) { - FCL_REAL t, s; - FCL_REAL diff; + const Vec3f& b, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance) { + CoalScalar t, s; + CoalScalar diff; Matrix3f Bf(B.cwiseAbs()); squaredLowerBoundDistance = 0; @@ -852,7 +854,7 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); - FCL_REAL sinus2; + CoalScalar sinus2; diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)); // We need to divide by the norm || A0 x B0 || @@ -1002,10 +1004,10 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, // ------------------------ 6 -------------------------------------- bool originalWithNoLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const FCL_REAL&, - FCL_REAL& squaredLowerBoundDistance) { - FCL_REAL t, s; - const FCL_REAL reps = 1e-6; + const Vec3f& b, const CoalScalar&, + CoalScalar& squaredLowerBoundDistance) { + CoalScalar t, s; + const CoalScalar reps = 1e-6; squaredLowerBoundDistance = 0; @@ -1137,8 +1139,8 @@ struct BenchmarkResult { /// - 0-10 identifies a separating axes. /// - 11 means no separatins axes could be found. (distance should be <= 0) int ifId; - FCL_REAL distance; - FCL_REAL squaredLowerBoundDistance; + CoalScalar distance; + CoalScalar squaredLowerBoundDistance; duration_type duration[NB_METHODS]; bool failure; @@ -1175,9 +1177,9 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, const CollisionRequest& request, std::size_t N) { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); - const FCL_REAL breakDistance2 = breakDistance * breakDistance; + const CoalScalar breakDistance(request.break_distance + + request.security_margin); + const CoalScalar breakDistance2 = breakDistance * breakDistance; BenchmarkResult result; // First determine which axis provide the answer @@ -1189,7 +1191,7 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, // Sanity check result.failure = true; bool overlap = (result.ifId == 11); - FCL_REAL dist_thr = request.break_distance + request.security_margin; + CoalScalar dist_thr = request.break_distance + request.security_margin; if (!overlap && result.distance <= 0) { std::cerr << "Failure: negative distance for disjoint OBBs."; } else if (!overlap && result.squaredLowerBoundDistance < 0) { @@ -1216,7 +1218,7 @@ BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, } // Compute time - FCL_REAL tmp; + CoalScalar tmp; clock_type::time_point start, end; // ------------------------ 0 -------------------------------------- @@ -1295,7 +1297,7 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) { static const size_t nbTransformPerOBB = 100; static const size_t nbRunForTimeMeas = 1000; #endif - static const FCL_REAL extentNorm = 1.; + static const CoalScalar extentNorm = 1.; if (output != NULL) *output << BenchmarkResult::headers << '\n'; diff --git a/test/octree.cpp b/test/octree.cpp index 87bf39c64..894c28031 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -67,7 +67,7 @@ void makeMesh(const std::vector& vertices, } coal::OcTree makeOctree(const BVHModel& mesh, - const FCL_REAL& resolution) { + const CoalScalar& resolution) { Vec3f m(mesh.aabb_local.min_); Vec3f M(mesh.aabb_local.max_); coal::Box box(resolution, resolution, resolution); @@ -76,11 +76,11 @@ coal::OcTree makeOctree(const BVHModel& mesh, Transform3f tfBox; octomap::OcTreePtr_t octree(new octomap::OcTree(resolution)); - for (FCL_REAL x = resolution * floor(m[0] / resolution); x <= M[0]; + for (CoalScalar x = resolution * floor(m[0] / resolution); x <= M[0]; x += resolution) { - for (FCL_REAL y = resolution * floor(m[1] / resolution); y <= M[1]; + for (CoalScalar y = resolution * floor(m[1] / resolution); y <= M[1]; y += resolution) { - for (FCL_REAL z = resolution * floor(m[2] / resolution); z <= M[2]; + for (CoalScalar z = resolution * floor(m[2] / resolution); z <= M[2]; z += resolution) { Vec3f center(x + .5 * resolution, y + .5 * resolution, z + .5 * resolution); @@ -104,7 +104,7 @@ coal::OcTree makeOctree(const BVHModel& mesh, BOOST_AUTO_TEST_CASE(octree_mesh) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); - FCL_REAL resolution(10.); + CoalScalar resolution(10.); std::vector pRob, pEnv; std::vector tRob, tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -136,11 +136,11 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { { const std::vector bytes = envOctree.tobytes(); BOOST_CHECK(bytes.size() > 0 && bytes.size() <= envOctree.toBoxes().size() * - 3 * sizeof(FCL_REAL)); + 3 * sizeof(CoalScalar)); } std::vector transforms; - FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; + CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 100; #else @@ -178,7 +178,7 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { BOOST_AUTO_TEST_CASE(octree_height_field) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); - FCL_REAL resolution(10.); + CoalScalar resolution(10.); std::vector pEnv; std::vector tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -196,16 +196,16 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { std::cout << "Finished loading octree." << std::endl; // Building hfield - const FCL_REAL x_dim = 10, y_dim = 20; + const CoalScalar x_dim = 10, y_dim = 20; const int nx = 100, ny = 100; - const FCL_REAL max_altitude = 1., min_altitude = 0.; + const CoalScalar max_altitude = 1., min_altitude = 0.; const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); hfield.computeLocalAABB(); std::vector transforms; - FCL_REAL extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; + CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 1000; #else diff --git a/test/profiling.cpp b/test/profiling.cpp index 5ac747714..bdc5eb152 100644 --- a/test/profiling.cpp +++ b/test/profiling.cpp @@ -117,7 +117,7 @@ size_t Ntransform = 1; #else size_t Ntransform = 100; #endif -FCL_REAL limit = 20; +CoalScalar limit = 20; bool verbose = false; #define OUT(x) \ @@ -231,14 +231,14 @@ int main(int argc, char** argv) { Geometry first = makeGeomFromParam(iarg, argc, argv); Geometry second = makeGeomFromParam(iarg, argc, argv); - FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit}; + CoalScalar extents[] = {-limit, -limit, -limit, limit, limit, limit}; generateRandomTransforms(extents, transforms, Ntransform); printResultHeaders(); Results results(Ntransform); collide(transforms, first.o.get(), second.o.get(), request, results); printResults(first, second, results); } else { - FCL_REAL extents[] = {-limit, -limit, -limit, limit, limit, limit}; + CoalScalar extents[] = {-limit, -limit, -limit, limit, limit, limit}; generateRandomTransforms(extents, transforms, Ntransform); boost::filesystem::path path(TEST_RESOURCES_DIR); diff --git a/test/security_margin.cpp b/test/security_margin.cpp index 59ef596f8..1f05503f2 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionRequest collisionRequest(CONTACT, 1); AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); @@ -93,7 +93,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); @@ -109,7 +109,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); @@ -125,7 +125,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance))); AABB bv2_transformed; computeBV(s2, tf2, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(res); @@ -139,7 +139,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { collisionRequest.security_margin = distance; AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); @@ -169,7 +169,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { collisionRequest.security_margin = distance; AABB bv2_transformed; computeBV(s2, tf2_collision, bv2_transformed); - FCL_REAL sqrDistLowerBound; + CoalScalar sqrDistLowerBound; bool res = bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); BOOST_CHECK(!res); @@ -394,7 +394,7 @@ BOOST_AUTO_TEST_CASE(box_box) { // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); - const FCL_REAL distance = -0.01; + const CoalScalar distance = -0.01; collisionRequest.security_margin = distance; CollisionResult collisionResult; @@ -413,7 +413,7 @@ BOOST_AUTO_TEST_CASE(box_box) { template void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, const ShapeType2& shape2, - const Transform3f& tf2_collision, const FCL_REAL tol) { + const Transform3f& tf2_collision, const CoalScalar tol) { // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); @@ -471,7 +471,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); - const FCL_REAL distance = -0.01; + const CoalScalar distance = -0.01; collisionRequest.security_margin = distance; CollisionResult collisionResult; diff --git a/test/serialization.cpp b/test/serialization.cpp index 58e1ac889..3d0769f8c 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -282,14 +282,14 @@ BOOST_AUTO_TEST_CASE(test_collision_data) { { // Serializing contact patches. const Halfspace hspace(0, 0, 1, 0); - const FCL_REAL radius = 0.25; - const FCL_REAL height = 1.; + const CoalScalar radius = 0.25; + const CoalScalar height = 1.; const Cylinder cylinder(radius, height); const Transform3f tf1; Transform3f tf2; // set translation to have a collision - const FCL_REAL offset = 0.001; + const CoalScalar offset = 0.001; tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; @@ -417,8 +417,8 @@ BOOST_AUTO_TEST_CASE(test_Convex) { #endif BOOST_AUTO_TEST_CASE(test_HeightField) { - const FCL_REAL min_altitude = -1.; - const FCL_REAL x_dim = 1., y_dim = 2.; + const CoalScalar min_altitude = -1.; + const CoalScalar x_dim = 1., y_dim = 2.; const Eigen::DenseIndex nx = 100, ny = 200; const MatrixXf heights = MatrixXf::Random(ny, nx); @@ -531,7 +531,7 @@ BOOST_AUTO_TEST_CASE(test_shapes) { #ifdef COAL_HAS_OCTOMAP BOOST_AUTO_TEST_CASE(test_octree) { - const FCL_REAL resolution = 1e-2; + const CoalScalar resolution = 1e-2; const Matrixx3f points = Matrixx3f::Random(1000, 3); OcTreePtr_t octree_ptr = makeOctree(points, resolution); const OcTree& octree = *octree_ptr.get(); diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index 33f285da3..3a32cb6ce 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -59,10 +59,11 @@ using coal::Vec3f; #define MATH_SQUARED(x) (x * x) template -bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol); +bool isApprox(const Shape &s1, const Shape &s2, const CoalScalar tol); -bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) { - typedef Eigen::Matrix Matrix; +bool isApprox(const CoalScalar &v1, const CoalScalar &v2, + const CoalScalar tol) { + typedef Eigen::Matrix Matrix; Matrix m1; m1 << v1; Matrix m2; @@ -70,48 +71,48 @@ bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) { return m1.isApprox(m2, tol); } -bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol) { +bool isApprox(const Box &s1, const Box &s2, const CoalScalar tol) { return s1.halfSide.isApprox(s2.halfSide, tol); } -bool isApprox(const Sphere &s1, const Sphere &s2, const FCL_REAL tol) { +bool isApprox(const Sphere &s1, const Sphere &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol); } -bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const FCL_REAL tol) { +bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const CoalScalar tol) { return s1.radii.isApprox(s2.radii, tol); } -bool isApprox(const Capsule &s1, const Capsule &s2, const FCL_REAL tol) { +bool isApprox(const Capsule &s1, const Capsule &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } -bool isApprox(const Cylinder &s1, const Cylinder &s2, const FCL_REAL tol) { +bool isApprox(const Cylinder &s1, const Cylinder &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } -bool isApprox(const Cone &s1, const Cone &s2, const FCL_REAL tol) { +bool isApprox(const Cone &s1, const Cone &s2, const CoalScalar tol) { return isApprox(s1.radius, s2.radius, tol) && isApprox(s1.halfLength, s2.halfLength, tol); } -bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) { +bool isApprox(const TriangleP &s1, const TriangleP &s2, const CoalScalar tol) { return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) && s1.c.isApprox(s2.c, tol); } -bool isApprox(const Halfspace &s1, const Halfspace &s2, const FCL_REAL tol) { +bool isApprox(const Halfspace &s1, const Halfspace &s2, const CoalScalar tol) { return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol); } template -void test(const Shape &original_shape, const FCL_REAL inflation, - const FCL_REAL tol = 1e-8) { +void test(const Shape &original_shape, const CoalScalar inflation, + const CoalScalar tol = 1e-8) { // Zero inflation { - const FCL_REAL inflation = 0.; + const CoalScalar inflation = 0.; const auto &inflation_result = original_shape.inflated(inflation); const Transform3f &shift = inflation_result.second; const Shape &inflated_shape = inflation_result.first; @@ -154,12 +155,12 @@ void test(const Shape &original_shape, const FCL_REAL inflation, } template -void test_throw(const Shape &shape, const FCL_REAL inflation) { +void test_throw(const Shape &shape, const CoalScalar inflation) { BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument); } template -void test_no_throw(const Shape &shape, const FCL_REAL inflation) { +void test_no_throw(const Shape &shape, const CoalScalar inflation) { BOOST_REQUIRE_NO_THROW(shape.inflated(inflation)); } diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index 2bfd3fa92..9dfd5db84 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -48,7 +48,7 @@ using namespace coal; #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) -FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; +CoalScalar extents[6] = {0, 0, 0, 10, 10, 10}; BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { Sphere s1(20); diff --git a/test/simple.cpp b/test/simple.cpp index 94527919b..cbeceab0b 100644 --- a/test/simple.cpp +++ b/test/simple.cpp @@ -9,9 +9,11 @@ using namespace coal; -static FCL_REAL epsilon = 1e-6; +static CoalScalar epsilon = 1e-6; -static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; } +static bool approx(CoalScalar x, CoalScalar y) { + return std::abs(x - y) < epsilon; +} BOOST_AUTO_TEST_CASE(projection_test_line) { Vec3f v1(0, 0, 0); diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index 6d2c33642..a89c89374 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -72,7 +72,7 @@ int line; COAL_CHECK(((v1) - (v2)).isZero(tol)) #define COAL_CHECK_REAL_CLOSE(v1, v2, tol) \ - FCL_REAL_IS_APPROX(v1, v2, tol); \ + CoalScalar_IS_APPROX(v1, v2, tol); \ COAL_CHECK(std::abs((v1) - (v2)) < tol) #define COAL_CHECK_CONDITION(cond) \ @@ -113,19 +113,19 @@ int line; struct SweptSphereGJKSolver : public GJKSolver { template - FCL_REAL shapeDistance( + CoalScalar shapeDistance( const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, bool compute_penetration, Vec3f& p1, Vec3f& p2, Vec3f& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const { if (use_swept_sphere_radius_in_gjk_epa_iterations) { - FCL_REAL distance; + CoalScalar distance; this->runGJKAndEPA( s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal); return distance; } // Default behavior of hppfcl's GJKSolver - FCL_REAL distance; + CoalScalar distance; this->runGJKAndEPA( s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal); return distance; @@ -138,23 +138,23 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { // The swept sphere radius is detrimental to the convergence of GJK // and EPA. This gets worse as the radius of the swept sphere increases. // So we need to increase the number of iterations to get a good result. - const FCL_REAL tol = 1e-6; + const CoalScalar tol = 1e-6; solver.gjk_tolerance = tol; solver.epa_tolerance = tol; solver.epa_max_iterations = 1000; const bool compute_penetration = true; - FCL_REAL extents[] = {-2, -2, -2, 2, 2, 2}; + CoalScalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 10; std::vector tf1s; std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); - const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; + const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; - for (const FCL_REAL& ssr1 : swept_sphere_radius) { + for (const CoalScalar& ssr1 : swept_sphere_radius) { shape1.setSweptSphereRadius(ssr1); - for (const FCL_REAL& ssr2 : swept_sphere_radius) { + for (const CoalScalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { Transform3f tf1 = tf1s[i]; @@ -162,7 +162,7 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { SET_LINE; - std::array distance; + std::array distance; std::array p1; std::array p2; std::array normal; @@ -182,7 +182,7 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { // The issue of precision does not come from the default behavior of // hppfcl, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. - const FCL_REAL precision = + const CoalScalar precision = 3 * sqrt(tol) + (1 / 100.0) * std::max(shape1.getSweptSphereRadius(), shape2.getSweptSphereRadius()); @@ -203,8 +203,8 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { } } -static const FCL_REAL min_shape_size = 0.1; -static const FCL_REAL max_shape_size = 0.5; +static const CoalScalar min_shape_size = 0.1; +static const CoalScalar max_shape_size = 0.5; BOOST_AUTO_TEST_CASE(ssr_mesh_mesh) { Convex shape1 = makeRandomConvex(min_shape_size, max_shape_size); @@ -281,17 +281,17 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { << std::string(get_node_type_name(shape1.getNodeType())) << " and " << std::string(get_node_type_name(shape2.getNodeType())) << '\n'; - FCL_REAL extents[] = {-2, -2, -2, 2, 2, 2}; + CoalScalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 1; std::vector tf1s; std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); - const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; - for (const FCL_REAL& ssr1 : swept_sphere_radius) { + const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; + for (const CoalScalar& ssr1 : swept_sphere_radius) { shape1.setSweptSphereRadius(ssr1); - for (const FCL_REAL& ssr2 : swept_sphere_radius) { + for (const CoalScalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { Transform3f tf1 = tf1s[i]; @@ -303,16 +303,16 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { // We make sure we get witness points by setting the security margin to // infinity. That way shape1 and shape2 will always be considered in // collision. - request.security_margin = std::numeric_limits::max(); - const FCL_REAL tol = 1e-6; + request.security_margin = std::numeric_limits::max(); + const CoalScalar tol = 1e-6; request.gjk_tolerance = tol; request.epa_tolerance = tol; std::array result; // Without swept sphere radius - const FCL_REAL ssr1 = shape1.getSweptSphereRadius(); - const FCL_REAL ssr2 = shape2.getSweptSphereRadius(); + const CoalScalar ssr1 = shape1.getSweptSphereRadius(); + const CoalScalar ssr2 = shape2.getSweptSphereRadius(); shape1.setSweptSphereRadius(0.); shape2.setSweptSphereRadius(0.); coal::collide(&shape1, tf1, &shape2, tf2, request, result[0]); @@ -333,9 +333,9 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { // The issue of precision does not come from the default behavior of // hppfcl, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. - const FCL_REAL precision = + const CoalScalar precision = 3 * sqrt(tol) + (1 / 100.0) * std::max(ssr1, ssr2); - const FCL_REAL ssr = ssr1 + ssr2; + const CoalScalar ssr = ssr1 + ssr2; // Check that the distance is the same COAL_CHECK_REAL_CLOSE(contact[0].penetration_depth - ssr, diff --git a/test/utility.cpp b/test/utility.cpp index 113902690..bddb9b4da 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -89,8 +89,8 @@ const Vec3f UnitX = Vec3f(1, 0, 0); const Vec3f UnitY = Vec3f(0, 1, 0); const Vec3f UnitZ = Vec3f(0, 0, 1); -FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) { - FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1); +CoalScalar rand_interval(CoalScalar rmin, CoalScalar rmax) { + CoalScalar t = rand() / ((CoalScalar)RAND_MAX + 1); return (t * (rmax - rmin) + rmin); } @@ -121,9 +121,9 @@ void loadOBJFile(const char* filename, std::vector& points, strtok(NULL, "\t "); has_texture = true; } else { - FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t ")); - FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t ")); - FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t ")); + CoalScalar x = (CoalScalar)atof(strtok(NULL, "\t ")); + CoalScalar y = (CoalScalar)atof(strtok(NULL, "\t ")); + CoalScalar z = (CoalScalar)atof(strtok(NULL, "\t ")); Vec3f p(x, y, z); points.push_back(p); } @@ -182,7 +182,8 @@ void saveOBJFile(const char* filename, std::vector& points, } #ifdef COAL_HAS_OCTOMAP -OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) { +OcTree loadOctreeFile(const std::string& filename, + const CoalScalar& resolution) { octomap::OcTreePtr_t octree(new octomap::OcTree(filename)); if (octree->getResolution() != resolution) { std::ostringstream oss; @@ -194,27 +195,27 @@ OcTree loadOctreeFile(const std::string& filename, const FCL_REAL& resolution) { } #endif -void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) { - FCL_REAL c1 = cos(a); - FCL_REAL c2 = cos(b); - FCL_REAL c3 = cos(c); - FCL_REAL s1 = sin(a); - FCL_REAL s2 = sin(b); - FCL_REAL s3 = sin(c); +void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3f& R) { + CoalScalar c1 = cos(a); + CoalScalar c2 = cos(b); + CoalScalar c3 = cos(c); + CoalScalar s1 = sin(a); + CoalScalar s2 = sin(b); + CoalScalar s3 = sin(c); R << c1 * c2, -c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; } -void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); +void generateRandomTransform(CoalScalar extents[6], Transform3f& transform) { + CoalScalar x = rand_interval(extents[0], extents[3]); + CoalScalar y = rand_interval(extents[1], extents[4]); + CoalScalar z = rand_interval(extents[2], extents[5]); - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); + const CoalScalar pi = 3.1415926; + CoalScalar a = rand_interval(0, 2 * pi); + CoalScalar b = rand_interval(0, 2 * pi); + CoalScalar c = rand_interval(0, 2 * pi); Matrix3f R; eulerToMatrix(a, b, c, R); @@ -222,19 +223,19 @@ void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) { transform.setTransform(R, T); } -void generateRandomTransforms(FCL_REAL extents[6], +void generateRandomTransforms(CoalScalar extents[6], std::vector& transforms, std::size_t n) { transforms.resize(n); for (std::size_t i = 0; i < n; ++i) { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); + CoalScalar x = rand_interval(extents[0], extents[3]); + CoalScalar y = rand_interval(extents[1], extents[4]); + CoalScalar z = rand_interval(extents[2], extents[5]); - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); + const CoalScalar pi = 3.1415926; + CoalScalar a = rand_interval(0, 2 * pi); + CoalScalar b = rand_interval(0, 2 * pi); + CoalScalar c = rand_interval(0, 2 * pi); { Matrix3f R; @@ -245,22 +246,22 @@ void generateRandomTransforms(FCL_REAL extents[6], } } -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], - FCL_REAL delta_rot, +void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], + CoalScalar delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n) { transforms.resize(n); transforms2.resize(n); for (std::size_t i = 0; i < n; ++i) { - FCL_REAL x = rand_interval(extents[0], extents[3]); - FCL_REAL y = rand_interval(extents[1], extents[4]); - FCL_REAL z = rand_interval(extents[2], extents[5]); + CoalScalar x = rand_interval(extents[0], extents[3]); + CoalScalar y = rand_interval(extents[1], extents[4]); + CoalScalar z = rand_interval(extents[2], extents[5]); - const FCL_REAL pi = 3.1415926; - FCL_REAL a = rand_interval(0, 2 * pi); - FCL_REAL b = rand_interval(0, 2 * pi); - FCL_REAL c = rand_interval(0, 2 * pi); + const CoalScalar pi = 3.1415926; + CoalScalar a = rand_interval(0, 2 * pi); + CoalScalar b = rand_interval(0, 2 * pi); + CoalScalar c = rand_interval(0, 2 * pi); { Matrix3f R; @@ -269,13 +270,13 @@ void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], transforms[i].setTransform(R, T); } - FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); - FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); - FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); + CoalScalar deltax = rand_interval(-delta_trans[0], delta_trans[0]); + CoalScalar deltay = rand_interval(-delta_trans[1], delta_trans[1]); + CoalScalar deltaz = rand_interval(-delta_trans[2], delta_trans[2]); - FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); - FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); + CoalScalar deltaa = rand_interval(-delta_rot, delta_rot); + CoalScalar deltab = rand_interval(-delta_rot, delta_rot); + CoalScalar deltac = rand_interval(-delta_rot, delta_rot); { Matrix3f R; @@ -304,7 +305,7 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, } bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* cdata_, FCL_REAL& dist) { + void* cdata_, CoalScalar& dist) { DistanceData* cdata = static_cast(cdata_); const DistanceRequest& request = cdata->request; DistanceResult& result = cdata->result; @@ -366,7 +367,7 @@ std::string getNodeTypeName(NODE_TYPE node_type) { return std::string("invalid"); } -Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z) { +Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z) { Quatf q; q.w() = w; q.x() = x; @@ -389,9 +390,9 @@ std::size_t getNbRun(const int& argc, char const* const* argv, } void generateEnvironments(std::vector& env, - FCL_REAL env_scale, std::size_t n) { - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, - env_scale, -env_scale, env_scale}; + CoalScalar env_scale, std::size_t n) { + CoalScalar extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; std::vector transforms(n); generateRandomTransforms(extents, transforms, n); @@ -420,9 +421,9 @@ void generateEnvironments(std::vector& env, } void generateEnvironmentsMesh(std::vector& env, - FCL_REAL env_scale, std::size_t n) { - FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, - env_scale, -env_scale, env_scale}; + CoalScalar env_scale, std::size_t n) { + CoalScalar extents[] = {-env_scale, env_scale, -env_scale, + env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); @@ -456,7 +457,7 @@ void generateEnvironmentsMesh(std::vector& env, } } -Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) { +Convex buildBox(CoalScalar l, CoalScalar w, CoalScalar d) { std::shared_ptr> pts(new std::vector( {Vec3f(l, w, d), Vec3f(l, w, -d), Vec3f(l, -w, d), Vec3f(l, -w, -d), Vec3f(-l, w, d), Vec3f(-l, w, -d), Vec3f(-l, -w, d), @@ -498,7 +499,7 @@ void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) { } Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) { - FCL_REAL PHI = (1 + std::sqrt(5)) / 2; + CoalScalar PHI = (1 + std::sqrt(5)) / 2; // vertices std::shared_ptr> pts(new std::vector({ @@ -555,50 +556,50 @@ Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) { ); } -Box makeRandomBox(FCL_REAL min_size, FCL_REAL max_size) { +Box makeRandomBox(CoalScalar min_size, CoalScalar max_size) { return Box(Vec3f(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } -Sphere makeRandomSphere(FCL_REAL min_size, FCL_REAL max_size) { +Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size) { return Sphere(rand_interval(min_size, max_size)); } -Ellipsoid makeRandomEllipsoid(FCL_REAL min_size, FCL_REAL max_size) { +Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size) { return Ellipsoid(Vec3f(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } -Capsule makeRandomCapsule(std::array min_size, - std::array max_size) { +Capsule makeRandomCapsule(std::array min_size, + std::array max_size) { return Capsule(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } -Cone makeRandomCone(std::array min_size, - std::array max_size) { +Cone makeRandomCone(std::array min_size, + std::array max_size) { return Cone(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } -Cylinder makeRandomCylinder(std::array min_size, - std::array max_size) { +Cylinder makeRandomCylinder(std::array min_size, + std::array max_size) { return Cylinder(rand_interval(min_size[0], max_size[0]), rand_interval(min_size[1], max_size[1])); } -Convex makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size) { +Convex makeRandomConvex(CoalScalar min_size, CoalScalar max_size) { Ellipsoid ellipsoid = makeRandomEllipsoid(min_size, max_size); return constructPolytopeFromEllipsoid(ellipsoid); } -Plane makeRandomPlane(FCL_REAL min_size, FCL_REAL max_size) { +Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size) { return Plane(Vec3f::Random().normalized(), rand_interval(min_size, max_size)); } -Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size) { +Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size) { return Halfspace(Vec3f::Random().normalized(), rand_interval(min_size, max_size)); } diff --git a/test/utility.h b/test/utility.h index 78f4b05b9..70c868092 100644 --- a/test/utility.h +++ b/test/utility.h @@ -70,7 +70,7 @@ << (Va) << "\n!=\n" \ << (Vb) << "\n]") -#define FCL_REAL_IS_APPROX(Va, Vb, precision) \ +#define CoalScalar_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE(std::abs((Va) - (Vb)) < precision, \ "check " #Va ".isApprox(" #Vb ") failed at precision " \ << precision << " [\n" \ @@ -125,7 +125,7 @@ struct TStruct { extern const Eigen::IOFormat vfmt; extern const Eigen::IOFormat pyfmt; -typedef Eigen::AngleAxis AngleAxis; +typedef Eigen::AngleAxis AngleAxis; extern const Vec3f UnitX; extern const Vec3f UnitY; extern const Vec3f UnitZ; @@ -139,26 +139,26 @@ void saveOBJFile(const char* filename, std::vector& points, #ifdef COAL_HAS_OCTOMAP coal::OcTree loadOctreeFile(const std::string& filename, - const FCL_REAL& resolution); + const CoalScalar& resolution); #endif /// @brief Generate one random transform whose translation is constrained by /// extents and rotation without constraints. The translation is (x, y, z), and /// extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= /// z <= extents[5] -void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform); +void generateRandomTransform(CoalScalar extents[6], Transform3f& transform); /// @brief Generate n random transforms whose translations are constrained by /// extents. -void generateRandomTransforms(FCL_REAL extents[6], +void generateRandomTransforms(CoalScalar extents[6], std::vector& transforms, std::size_t n); /// @brief Generate n random transforms whose translations are constrained by /// extents. Also generate another transforms2 which have additional random /// translation & rotation to the transforms generated. -void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], - FCL_REAL delta_rot, +void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], + CoalScalar delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n); @@ -180,11 +180,11 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, /// return value means whether the broad phase can stop now. also return dist, /// i.e. the bmin distance till now bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, - void* cdata, FCL_REAL& dist); + void* cdata, CoalScalar& dist); std::string getNodeTypeName(NODE_TYPE node_type); -Quatf makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z); +Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z); std::ostream& operator<<(std::ostream& os, const Transform3f& tf); @@ -193,15 +193,15 @@ std::size_t getNbRun(const int& argc, char const* const* argv, std::size_t defaultValue); void generateEnvironments(std::vector& env, - FCL_REAL env_scale, std::size_t n); + CoalScalar env_scale, std::size_t n); void generateEnvironmentsMesh(std::vector& env, - FCL_REAL env_scale, std::size_t n); + CoalScalar env_scale, std::size_t n); /// @brief Constructs a box with halfsides (l, w, d), centered around 0. /// The box is 2*l wide on the x-axis, 2*w wide on the y-axis and 2*d wide on /// the z-axis. -Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d); +Convex buildBox(CoalScalar l, CoalScalar w, CoalScalar d); /// @brief We give an ellipsoid as input. The output is a 20 faces polytope /// which vertices belong to the original ellipsoid surface. The procedure is @@ -210,26 +210,26 @@ Convex buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d); /// ellipsoid tranformation to each vertex of the icosahedron. Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid); -Box makeRandomBox(FCL_REAL min_size, FCL_REAL max_size); +Box makeRandomBox(CoalScalar min_size, CoalScalar max_size); -Sphere makeRandomSphere(FCL_REAL min_size, FCL_REAL max_size); +Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size); -Ellipsoid makeRandomEllipsoid(FCL_REAL min_size, FCL_REAL max_size); +Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size); -Capsule makeRandomCapsule(std::array min_size, - std::array max_size); +Capsule makeRandomCapsule(std::array min_size, + std::array max_size); -Cone makeRandomCone(std::array min_size, - std::array max_size); +Cone makeRandomCone(std::array min_size, + std::array max_size); -Cylinder makeRandomCylinder(std::array min_size, - std::array max_size); +Cylinder makeRandomCylinder(std::array min_size, + std::array max_size); -Convex makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size); +Convex makeRandomConvex(CoalScalar min_size, CoalScalar max_size); -Plane makeRandomPlane(FCL_REAL min_size, FCL_REAL max_size); +Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size); -Halfspace makeRandomHalfspace(FCL_REAL min_size, FCL_REAL max_size); +Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size); std::shared_ptr makeRandomGeometry(NODE_TYPE node_type); From 120757884a855edfa20d7ac7b1eaa6b7aa8ffb02 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 17:30:22 +0200 Subject: [PATCH 15/57] all: rename `Matrix/Vec[..]f` typedefs to `Matrix/Vec[...]s` We keep the old typedefs for backward compatibility --- README.md | 4 +- doc/gjk.py | 4 +- doc/mesh-mesh-collision-call.dot | 14 +- doc/shape-mesh-collision-call.dot | 20 +- doc/shape-shape-collision-call.dot | 4 +- include/coal/BV/AABB.h | 34 +- include/coal/BV/BV.h | 34 +- include/coal/BV/BV_node.h | 16 +- include/coal/BV/OBB.h | 26 +- include/coal/BV/OBBRSS.h | 20 +- include/coal/BV/RSS.h | 24 +- include/coal/BV/kDOP.h | 20 +- include/coal/BV/kIOS.h | 26 +- include/coal/BVH/BVH_model.h | 72 +- include/coal/BVH/BVH_utility.h | 22 +- include/coal/broadphase/broadphase_SaP.h | 4 +- .../broadphase/broadphase_spatialhash-inl.h | 8 +- .../coal/broadphase/broadphase_spatialhash.h | 8 +- .../broadphase/detail/hierarchy_tree-inl.h | 24 +- .../coal/broadphase/detail/hierarchy_tree.h | 4 +- .../detail/hierarchy_tree_array-inl.h | 20 +- .../broadphase/detail/hierarchy_tree_array.h | 4 +- include/coal/broadphase/detail/morton-inl.h | 6 +- include/coal/broadphase/detail/morton.h | 18 +- include/coal/collision_data.h | 76 +- include/coal/collision_object.h | 32 +- .../coal/contact_patch/contact_patch_solver.h | 4 +- .../contact_patch/contact_patch_solver.hxx | 54 +- include/coal/data_types.h | 8 +- include/coal/hfield.h | 50 +- include/coal/internal/BV_fitter.h | 20 +- include/coal/internal/BV_splitter.h | 18 +- include/coal/internal/intersect.h | 72 +- include/coal/internal/shape_shape_func.h | 24 +- include/coal/internal/traversal.h | 14 +- .../coal/internal/traversal_node_bvh_hfield.h | 48 +- .../coal/internal/traversal_node_bvh_shape.h | 16 +- include/coal/internal/traversal_node_bvhs.h | 52 +- .../internal/traversal_node_hfield_shape.h | 142 +- include/coal/internal/traversal_node_octree.h | 22 +- include/coal/internal/traversal_node_setup.h | 48 +- include/coal/math/transform.h | 32 +- include/coal/mesh_loader/assimp.h | 8 +- include/coal/mesh_loader/loader.h | 8 +- include/coal/narrowphase/gjk.h | 32 +- .../coal/narrowphase/minkowski_difference.h | 14 +- include/coal/narrowphase/narrowphase.h | 64 +- include/coal/narrowphase/support_functions.h | 38 +- include/coal/octree.h | 22 +- include/coal/serialization/collision_data.h | 6 +- include/coal/serialization/convex.h | 4 +- include/coal/serialization/kIOS.h | 4 +- include/coal/shape/convex.h | 8 +- include/coal/shape/details/convex.hxx | 48 +- .../coal/shape/geometric_shape_to_BVH_model.h | 48 +- include/coal/shape/geometric_shapes.h | 96 +- include/coal/shape/geometric_shapes_utility.h | 18 +- python/broadphase/broadphase.cc | 2 +- python/collision-geometries.cc | 68 +- python/collision.cc | 8 +- python/distance.cc | 4 +- python/gjk.cc | 4 +- python/math.cc | 30 +- python/octree.cc | 4 +- src/BV/AABB.cpp | 28 +- src/BV/OBB.cpp | 114 +- src/BV/OBB.h | 8 +- src/BV/OBBRSS.cpp | 2 +- src/BV/RSS.cpp | 94 +- src/BV/kDOP.cpp | 28 +- src/BV/kIOS.cpp | 18 +- src/BVH/BVH_model.cpp | 124 +- src/BVH/BVH_utility.cpp | 122 +- src/BVH/BV_fitter.cpp | 150 +- src/BVH/BV_splitter.cpp | 60 +- src/broadphase/broadphase_SSaP.cpp | 8 +- src/broadphase/broadphase_SaP.cpp | 14 +- src/broadphase/broadphase_interval_tree.cpp | 6 +- src/broadphase/detail/morton-inl.h | 6 +- src/distance/box_halfspace.cpp | 4 +- src/distance/box_plane.cpp | 4 +- src/distance/box_sphere.cpp | 4 +- src/distance/capsule_capsule.cpp | 22 +- src/distance/capsule_halfspace.cpp | 4 +- src/distance/capsule_plane.cpp | 4 +- src/distance/cone_halfspace.cpp | 4 +- src/distance/cone_plane.cpp | 4 +- src/distance/convex_halfspace.cpp | 4 +- src/distance/convex_plane.cpp | 4 +- src/distance/cylinder_halfspace.cpp | 4 +- src/distance/cylinder_plane.cpp | 4 +- src/distance/ellipsoid_halfspace.cpp | 4 +- src/distance/ellipsoid_plane.cpp | 4 +- src/distance/halfspace_halfspace.cpp | 2 +- src/distance/halfspace_plane.cpp | 4 +- src/distance/plane_plane.cpp | 2 +- src/distance/sphere_capsule.cpp | 4 +- src/distance/sphere_cylinder.cpp | 4 +- src/distance/sphere_halfspace.cpp | 4 +- src/distance/sphere_plane.cpp | 4 +- src/distance/sphere_sphere.cpp | 2 +- src/distance/triangle_halfspace.cpp | 4 +- src/distance/triangle_plane.cpp | 4 +- src/distance/triangle_sphere.cpp | 4 +- src/distance/triangle_triangle.cpp | 4 +- src/intersect.cpp | 162 +-- src/math/transform.cpp | 2 +- src/mesh_loader/assimp.cpp | 6 +- src/mesh_loader/loader.cpp | 6 +- src/narrowphase/details.h | 180 +-- src/narrowphase/gjk.cpp | 100 +- src/narrowphase/minkowski_difference.cpp | 18 +- src/narrowphase/support_functions.cpp | 385 ++--- src/octree.cpp | 38 +- src/shape/convex.cpp | 22 +- src/shape/geometric_shapes.cpp | 50 +- src/shape/geometric_shapes_utility.cpp | 318 ++--- test/accelerated_gjk.cpp | 10 +- test/benchmark.cpp | 6 +- test/box_box_collision.cpp | 6 +- test/box_box_distance.cpp | 40 +- test/broadphase.cpp | 20 +- test/broadphase_collision_1.cpp | 28 +- test/broadphase_collision_2.cpp | 2 +- test/bvh_models.cpp | 32 +- test/capsule_box_1.cpp | 10 +- test/capsule_box_2.cpp | 6 +- test/capsule_capsule.cpp | 12 +- test/collision.cpp | 16 +- test/collision_node_asserts.cpp | 16 +- test/contact_patch.cpp | 226 +-- test/convex.cpp | 88 +- test/distance.cpp | 32 +- test/distance_lower_bound.cpp | 6 +- test/frontlist.cpp | 28 +- test/geometric_shapes.cpp | 1250 ++++++++--------- test/gjk.cpp | 130 +- test/gjk_asserts.cpp | 12 +- test/gjk_convergence_criterion.cpp | 10 +- test/hfields.cpp | 162 +-- test/math.cpp | 42 +- test/normal_and_nearest_points.cpp | 58 +- test/obb.cpp | 114 +- test/octree.cpp | 14 +- test/profiling.cpp | 14 +- test/python_unit/collision.py | 2 +- test/python_unit/geometric_shapes.py | 2 +- test/python_unit/pickling.py | 2 +- test/security_margin.cpp | 40 +- test/serialization.cpp | 34 +- test/shape_inflation.cpp | 8 +- test/shape_mesh_consistency.cpp | 212 +-- test/simple.cpp | 68 +- test/swept_sphere_radius.cpp | 10 +- test/utility.cpp | 82 +- test/utility.h | 14 +- 156 files changed, 3379 insertions(+), 3376 deletions(-) diff --git a/README.md b/README.md index d8a6d1af4..7cb262f3d 100644 --- a/README.md +++ b/README.md @@ -113,10 +113,10 @@ int main() { // Define the shapes' placement in 3D space hpp::fcl::Transform3f T1; T1.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); - T1.setTranslation(hpp::fcl::Vec3f::Random()); + T1.setTranslation(hpp::fcl::Vec3s::Random()); hpp::fcl::Transform3f T2 = hpp::fcl::Transform3f::Identity(); T2.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); - T2.setTranslation(hpp::fcl::Vec3f::Random()); + T2.setTranslation(hpp::fcl::Vec3s::Random()); // Define collision requests and results. // diff --git a/doc/gjk.py b/doc/gjk.py index 2b7019f99..bee0d547a 100644 --- a/doc/gjk.py +++ b/doc/gjk.py @@ -444,7 +444,7 @@ def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]): for v in "abcd": print( indent - + "const Vec3f& {} (current.vertex[{}]->w);".format(v.upper(), v), + + "const Vec3s& {} (current.vertex[{}]->w);".format(v.upper(), v), file=file, ) print(indent + "const CoalScalar aa = A.squaredNorm();".format(), file=file) @@ -471,7 +471,7 @@ def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]): ) for v in "bc": print( - indent + "const Vec3f a_cross_{0} = A.cross({1});".format(v, v.upper()), + indent + "const Vec3s a_cross_{0} = A.cross({1});".format(v, v.upper()), file=file, ) print("", file=file) diff --git a/doc/mesh-mesh-collision-call.dot b/doc/mesh-mesh-collision-call.dot index 8c91a3821..5069bbc49 100644 --- a/doc/mesh-mesh-collision-call.dot +++ b/doc/mesh-mesh-collision-call.dot @@ -7,8 +7,8 @@ digraph CD { "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box] "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] "bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] @@ -17,19 +17,19 @@ digraph CD { "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" [shape = box] "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] - "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box] + "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box] "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" - "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)" - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)" - "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" + "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" + "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" +"bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [color=red] "bool MeshCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [color = red] "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" diff --git a/doc/shape-mesh-collision-call.dot b/doc/shape-mesh-collision-call.dot index c08fbdfef..3eb293642 100644 --- a/doc/shape-mesh-collision-call.dot +++ b/doc/shape-mesh-collision-call.dot @@ -13,11 +13,11 @@ digraph CD { "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box] "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] - "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] - "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] + "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] + "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" @@ -28,9 +28,9 @@ digraph CD { "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" "virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red] "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red] - "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" - "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" - "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3f& P1, const Vec3f& P2, const Vec3f& P3,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h:156" + "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" + "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" + "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" } diff --git a/doc/shape-shape-collision-call.dot b/doc/shape-shape-collision-call.dot index be01df254..59e8467ed 100644 --- a/doc/shape-shape-collision-call.dot +++ b/doc/shape-shape-collision-call.dot @@ -9,11 +9,11 @@ digraph CD { "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box] "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box] "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box] - "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" [shape = box] + "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box] "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" - "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3f* p1, Vec3f* p2) const\nnarrowphase/narrowphase.h" + "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" } diff --git a/include/coal/BV/AABB.h b/include/coal/BV/AABB.h index 22ae0ab0d..8d46a5323 100644 --- a/include/coal/BV/AABB.h +++ b/include/coal/BV/AABB.h @@ -55,33 +55,33 @@ class Halfspace; class COAL_DLLAPI AABB { public: /// @brief The min point in the AABB - Vec3f min_; + Vec3s min_; /// @brief The max point in the AABB - Vec3f max_; + Vec3s max_; /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf) AABB(); /// @brief Creating an AABB at position v with zero size - AABB(const Vec3f& v) : min_(v), max_(v) {} + AABB(const Vec3s& v) : min_(v), max_(v) {} /// @brief Creating an AABB with two endpoints a and b - AABB(const Vec3f& a, const Vec3f& b) + AABB(const Vec3s& a, const Vec3s& b) : min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {} /// @brief Creating an AABB centered as core and is of half-dimension delta - AABB(const AABB& core, const Vec3f& delta) + AABB(const AABB& core, const Vec3s& delta) : min_(core.min_ - delta), max_(core.max_ + delta) {} /// @brief Creating an AABB contains three points - AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) + AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c) : min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {} AABB(const AABB& other) = default; AABB& operator=(const AABB& other) = default; - AABB& update(const Vec3f& a, const Vec3f& b) { + AABB& update(const Vec3s& a, const Vec3s& b) { min_ = a.cwiseMin(b); max_ = a.cwiseMax(b); return *this; @@ -99,7 +99,7 @@ class COAL_DLLAPI AABB { /// @{ /// @brief Check whether the AABB contains a point - inline bool contain(const Vec3f& p) const { + inline bool contain(const Vec3s& p) const { if (p[0] < min_[0] || p[0] > max_[0]) return false; if (p[1] < min_[1] || p[1] > max_[1]) return false; if (p[2] < min_[2] || p[2] > max_[2]) return false; @@ -135,10 +135,10 @@ class COAL_DLLAPI AABB { /// @brief Distance between two AABBs; P and Q, should not be NULL, return the /// nearest points - CoalScalar distance(const AABB& other, Vec3f* P, Vec3f* Q) const; + CoalScalar distance(const AABB& other, Vec3s* P, Vec3s* Q) const; /// @brief Merge the AABB and a point - inline AABB& operator+=(const Vec3f& p) { + inline AABB& operator+=(const Vec3s& p) { min_ = min_.cwiseMin(p); max_ = max_.cwiseMax(p); return *this; @@ -161,7 +161,7 @@ class COAL_DLLAPI AABB { inline CoalScalar size() const { return (max_ - min_).squaredNorm(); } /// @brief Center of the AABB - inline Vec3f center() const { return (min_ + max_) * 0.5; } + inline Vec3s center() const { return (min_ + max_) * 0.5; } /// @brief Width of the AABB inline CoalScalar width() const { return max_[0] - min_[0]; } @@ -205,7 +205,7 @@ class COAL_DLLAPI AABB { /// @brief expand the half size of the AABB by delta, and keep the center /// unchanged. - inline AABB& expand(const Vec3f& delta) { + inline AABB& expand(const Vec3s& delta) { min_ -= delta; max_ += delta; return *this; @@ -230,16 +230,16 @@ class COAL_DLLAPI AABB { }; /// @brief translate the center of AABB by t -static inline AABB translate(const AABB& aabb, const Vec3f& t) { +static inline AABB translate(const AABB& aabb, const Vec3s& t) { AABB res(aabb); res.min_ += t; res.max_ += t; return res; } -static inline AABB rotate(const AABB& aabb, const Matrix3f& R) { +static inline AABB rotate(const AABB& aabb, const Matrix3s& R) { AABB res(R * aabb.min_); - Vec3f corner(aabb.min_); + Vec3s corner(aabb.min_); const Eigen::DenseIndex bit[3] = {1, 2, 4}; for (Eigen::DenseIndex ic = 1; ic < 8; ++ic) { // ic = 0 corresponds to aabb.min_. Skip it. @@ -253,12 +253,12 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& R) { /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) /// and b2 is in identity. -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2); /// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) /// and b2 is in identity. -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound); } // namespace coal diff --git a/include/coal/BV/BV.h b/include/coal/BV/BV.h index d6c6a7e08..aba2d1d49 100644 --- a/include/coal/BV/BV.h +++ b/include/coal/BV/BV.h @@ -64,11 +64,11 @@ struct Converter { template <> struct Converter { static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) { - const Vec3f& center = bv1.center(); + const Vec3s& center = bv1.center(); CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5; - const Vec3f center2 = tf1.transform(center); - bv2.min_ = center2 - Vec3f::Constant(r); - bv2.max_ = center2 + Vec3f::Constant(r); + const Vec3s center2 = tf1.transform(center); + bv2.min_ = center2 - Vec3s::Constant(r); + bv2.max_ = center2 + Vec3s::Constant(r); } static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; } @@ -114,14 +114,14 @@ struct Converter { template <> struct Converter { static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) { - bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, + bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius); bv2.To = tf1.transform(bv1.Tr); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; } static void convert(const RSS& bv1, OBB& bv2) { - bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, + bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius); bv2.To = bv1.Tr; bv2.axes = bv1.axes; @@ -131,18 +131,18 @@ struct Converter { template struct Converter { static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) { - const Vec3f& center = bv1.center(); - CoalScalar r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; - const Vec3f center2 = tf1.transform(center); - bv2.min_ = center2 - Vec3f::Constant(r); - bv2.max_ = center2 + Vec3f::Constant(r); + const Vec3s& center = bv1.center(); + CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + const Vec3s center2 = tf1.transform(center); + bv2.min_ = center2 - Vec3s::Constant(r); + bv2.max_ = center2 + Vec3s::Constant(r); } static void convert(const BV1& bv1, AABB& bv2) { - const Vec3f& center = bv1.center(); - CoalScalar r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; - bv2.min_ = center - Vec3f::Constant(r); - bv2.max_ = center + Vec3f::Constant(r); + const Vec3s& center = bv1.center(); + CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + bv2.min_ = center - Vec3s::Constant(r); + bv2.max_ = center + Vec3s::Constant(r); } }; @@ -233,12 +233,12 @@ struct Converter { } } - const Vec3f extent = (bv1.max_ - bv1.min_) * 0.5; + const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5; bv2.radius = extent[id[2]]; bv2.length[0] = (extent[id[0]] - bv2.radius) * 2; bv2.length[1] = (extent[id[1]] - bv2.radius) * 2; - const Matrix3f& R = tf1.getRotation(); + const Matrix3s& R = tf1.getRotation(); const bool left_hand = (id[0] == (id[1] + 1) % 3); if (left_hand) bv2.axes.col(0) = -R.col(id[0]); diff --git a/include/coal/BV/BV_node.h b/include/coal/BV/BV_node.h index 43f33863b..b858308c5 100644 --- a/include/coal/BV/BV_node.h +++ b/include/coal/BV/BV_node.h @@ -127,17 +127,17 @@ struct COAL_DLLAPI BVNode : public BVNodeBase { /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and /// the underlying BV supports distance, return the nearest points. - CoalScalar distance(const BVNode& other, Vec3f* P1 = NULL, - Vec3f* P2 = NULL) const { + CoalScalar distance(const BVNode& other, Vec3s* P1 = NULL, + Vec3s* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } /// @brief Access to the center of the BV - Vec3f getCenter() const { return bv.center(); } + Vec3s getCenter() const { return bv.center(); } /// @brief Access to the orientation of the BV - const Matrix3f& getOrientation() const { - static const Matrix3f id3 = Matrix3f::Identity(); + const Matrix3s& getOrientation() const { + static const Matrix3s id3 = Matrix3s::Identity(); return id3; } @@ -147,17 +147,17 @@ struct COAL_DLLAPI BVNode : public BVNodeBase { }; template <> -inline const Matrix3f& BVNode::getOrientation() const { +inline const Matrix3s& BVNode::getOrientation() const { return bv.axes; } template <> -inline const Matrix3f& BVNode::getOrientation() const { +inline const Matrix3s& BVNode::getOrientation() const { return bv.axes; } template <> -inline const Matrix3f& BVNode::getOrientation() const { +inline const Matrix3s& BVNode::getOrientation() const { return bv.obb.axes; } diff --git a/include/coal/BV/OBB.h b/include/coal/BV/OBB.h index b16f3d01f..8255f023b 100644 --- a/include/coal/BV/OBB.h +++ b/include/coal/BV/OBB.h @@ -56,15 +56,15 @@ struct COAL_DLLAPI OBB { /// assume that axis[0] corresponds to the axis with the longest box edge, /// axis[1] corresponds to the shorter one and axis[2] corresponds to the /// shortest one. - Matrix3f axes; + Matrix3s axes; /// @brief Center of OBB - Vec3f To; + Vec3s To; /// @brief Half dimensions of OBB - Vec3f extent; + Vec3s extent; - OBB() : axes(Matrix3f::Zero()), To(Vec3f::Zero()), extent(Vec3f::Zero()) {} + OBB() : axes(Matrix3s::Zero()), To(Vec3s::Zero()), extent(Vec3s::Zero()) {} /// @brief Equality operator bool operator==(const OBB& other) const { @@ -75,7 +75,7 @@ struct COAL_DLLAPI OBB { bool operator!=(const OBB& other) const { return !(*this == other); } /// @brief Check whether the OBB contains a point. - bool contain(const Vec3f& p) const; + bool contain(const Vec3s& p) const; /// Check collision between two OBB /// @return true if collision happens. @@ -89,11 +89,11 @@ struct COAL_DLLAPI OBB { CoalScalar& sqrDistLowerBound) const; /// @brief Distance between two OBBs, not implemented. - CoalScalar distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; + CoalScalar distance(const OBB& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; /// @brief A simple way to merge the OBB and a point (the result is not /// compact). - OBB& operator+=(const Vec3f& p); + OBB& operator+=(const Vec3s& p); /// @brief Merge the OBB and another OBB (the result is not compact). OBB& operator+=(const OBB& other) { @@ -109,7 +109,7 @@ struct COAL_DLLAPI OBB { inline CoalScalar size() const { return extent.squaredNorm(); } /// @brief Center of the OBB - inline const Vec3f& center() const { return To; } + inline const Vec3s& center() const { return To; } /// @brief Width of the OBB. inline CoalScalar width() const { return 2 * extent[0]; } @@ -125,16 +125,16 @@ struct COAL_DLLAPI OBB { }; /// @brief Translate the OBB bv -COAL_DLLAPI OBB translate(const OBB& bv, const Vec3f& t); +COAL_DLLAPI OBB translate(const OBB& bv, const Vec3s& t); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and /// b2 is in identity. -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and /// b2 is in identity. -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound); @@ -143,8 +143,8 @@ COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, /// @param a half dimensions of first box, /// @param b half dimensions of second box. /// The second box is in identity configuration. -COAL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b); +COAL_DLLAPI bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b); } // namespace coal #endif diff --git a/include/coal/BV/OBBRSS.h b/include/coal/BV/OBBRSS.h index 003b396e1..97ea6bb10 100644 --- a/include/coal/BV/OBBRSS.h +++ b/include/coal/BV/OBBRSS.h @@ -68,7 +68,7 @@ struct COAL_DLLAPI OBBRSS { bool operator!=(const OBBRSS& other) const { return !(*this == other); } /// @brief Check whether the OBBRSS contains a point - inline bool contain(const Vec3f& p) const { return obb.contain(p); } + inline bool contain(const Vec3s& p) const { return obb.contain(p); } /// @brief Check collision between two OBBRSS bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); } @@ -83,13 +83,13 @@ struct COAL_DLLAPI OBBRSS { /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the /// nearest points - CoalScalar distance(const OBBRSS& other, Vec3f* P = NULL, - Vec3f* Q = NULL) const { + CoalScalar distance(const OBBRSS& other, Vec3s* P = NULL, + Vec3s* Q = NULL) const { return rss.distance(other.rss, P, Q); } /// @brief Merge the OBBRSS and a point - OBBRSS& operator+=(const Vec3f& p) { + OBBRSS& operator+=(const Vec3s& p) { obb += p; rss += p; return *this; @@ -113,7 +113,7 @@ struct COAL_DLLAPI OBBRSS { inline CoalScalar size() const { return obb.size(); } /// @brief Center of the OBBRSS - inline const Vec3f& center() const { return obb.center(); } + inline const Vec3s& center() const { return obb.center(); } /// @brief Width of the OBRSS inline CoalScalar width() const { return obb.width(); } @@ -130,7 +130,7 @@ struct COAL_DLLAPI OBBRSS { /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) /// and b2 is in indentity -inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, +inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1, const OBBRSS& b2) { return overlap(R0, T0, b1.obb, b2.obb); } @@ -141,7 +141,7 @@ inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, /// @param b2 second OBBRSS in identity position /// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do /// not overlap. -inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, +inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1, const OBBRSS& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound); @@ -149,9 +149,9 @@ inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) /// and b2 is in indentity; P and Q, is not NULL, returns the nearest points -inline CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, - const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, - Vec3f* Q = NULL) { +inline CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, + const OBBRSS& b1, const OBBRSS& b2, Vec3s* P = NULL, + Vec3s* Q = NULL) { return distance(R0, T0, b1.rss, b2.rss, P, Q); } diff --git a/include/coal/BV/RSS.h b/include/coal/BV/RSS.h index 8d9cfba52..8bf2bd07a 100644 --- a/include/coal/BV/RSS.h +++ b/include/coal/BV/RSS.h @@ -58,10 +58,10 @@ struct COAL_DLLAPI RSS { /// assume that axis[0] corresponds to the axis with the longest length, /// axis[1] corresponds to the shorter one and axis[2] corresponds to the /// shortest one. - Matrix3f axes; + Matrix3s axes; /// @brief Origin of the rectangle in RSS - Vec3f Tr; + Vec3s Tr; /// @brief Side lengths of rectangle CoalScalar length[2]; @@ -70,7 +70,7 @@ struct COAL_DLLAPI RSS { CoalScalar radius; ///  @brief Default constructor with default values - RSS() : axes(Matrix3f::Zero()), Tr(Vec3f::Zero()), radius(-1) { + RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) { length[0] = 0; length[1] = 0; } @@ -86,7 +86,7 @@ struct COAL_DLLAPI RSS { bool operator!=(const RSS& other) const { return !(*this == other); } /// @brief Check whether the RSS contains a point - bool contain(const Vec3f& p) const; + bool contain(const Vec3s& p) const; /// @brief Check collision between two RSS bool overlap(const RSS& other) const; @@ -100,11 +100,11 @@ struct COAL_DLLAPI RSS { /// @brief the distance between two RSS; P and Q, if not NULL, return the /// nearest points - CoalScalar distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; + CoalScalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const; /// @brief A simple way to merge the RSS and a point, not compact. /// @todo This function may have some bug. - RSS& operator+=(const Vec3f& p); + RSS& operator+=(const Vec3s& p); /// @brief Merge the RSS and another RSS inline RSS& operator+=(const RSS& other) { @@ -122,7 +122,7 @@ struct COAL_DLLAPI RSS { } /// @brief The RSS center - inline const Vec3f& center() const { return Tr; } + inline const Vec3s& center() const { return Tr; } /// @brief Width of the RSS inline CoalScalar width() const { return length[0] + 2 * radius; } @@ -153,18 +153,18 @@ struct COAL_DLLAPI RSS { /// not the RSS. But the direction P - Q is the correct direction for cloest /// points Notice that P and Q are both in the local frame of the first RSS (not /// global frame and not even the local frame of object 1) -COAL_DLLAPI CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, - const RSS& b1, const RSS& b2, Vec3f* P = NULL, - Vec3f* Q = NULL); +COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, + const RSS& b1, const RSS& b2, Vec3s* P = NULL, + Vec3s* Q = NULL); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound); diff --git a/include/coal/BV/kDOP.h b/include/coal/BV/kDOP.h index 4803cd4b6..cb8377526 100644 --- a/include/coal/BV/kDOP.h +++ b/include/coal/BV/kDOP.h @@ -100,10 +100,10 @@ class COAL_DLLAPI KDOP { KDOP(); /// @brief Creating kDOP containing only one point - KDOP(const Vec3f& v); + KDOP(const Vec3s& v); /// @brief Creating kDOP containing two points - KDOP(const Vec3f& a, const Vec3f& b); + KDOP(const Vec3s& a, const Vec3s& b); /// @brief Equality operator bool operator==(const KDOP& other) const { @@ -126,11 +126,11 @@ class COAL_DLLAPI KDOP { CoalScalar& sqrDistLowerBound) const; /// @brief The distance between two KDOP. Not implemented. - CoalScalar distance(const KDOP& other, Vec3f* P = NULL, - Vec3f* Q = NULL) const; + CoalScalar distance(const KDOP& other, Vec3s* P = NULL, + Vec3s* Q = NULL) const; /// @brief Merge the point and the KDOP - KDOP& operator+=(const Vec3f& p); + KDOP& operator+=(const Vec3s& p); /// @brief Merge two KDOPs KDOP& operator+=(const KDOP& other); @@ -144,7 +144,7 @@ class COAL_DLLAPI KDOP { } /// @brief The (AABB) center - inline Vec3f center() const { + inline Vec3s center() const { return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2; } @@ -165,17 +165,17 @@ class COAL_DLLAPI KDOP { inline CoalScalar& dist(short i) { return dist_[i]; } //// @brief Check whether one point is inside the KDOP - bool inside(const Vec3f& p) const; + bool inside(const Vec3s& p) const; }; template -bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, +bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/) { COAL_THROW_PRETTY("not implemented", std::logic_error); } template -bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, +bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/, const CollisionRequest& /*request*/, CoalScalar& /*sqrDistLowerBound*/) { COAL_THROW_PRETTY("not implemented", std::logic_error); @@ -183,7 +183,7 @@ bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, /// @brief translate the KDOP BV template -COAL_DLLAPI KDOP translate(const KDOP& bv, const Vec3f& t); +COAL_DLLAPI KDOP translate(const KDOP& bv, const Vec3s& t); } // namespace coal diff --git a/include/coal/BV/kIOS.h b/include/coal/BV/kIOS.h index c663bd2c3..83277ab6a 100644 --- a/include/coal/BV/kIOS.h +++ b/include/coal/BV/kIOS.h @@ -54,7 +54,7 @@ class COAL_DLLAPI kIOS { struct COAL_DLLAPI kIOS_Sphere { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Vec3f o; + Vec3s o; CoalScalar r; bool operator==(const kIOS_Sphere& other) const { @@ -69,7 +69,7 @@ class COAL_DLLAPI kIOS { /// @brief generate one sphere enclosing two spheres static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1) { - Vec3f d = s1.o - s0.o; + Vec3s d = s1.o - s0.o; CoalScalar dist2 = d.squaredNorm(); CoalScalar diff_r = s1.r - s0.r; @@ -122,7 +122,7 @@ class COAL_DLLAPI kIOS { OBB obb; /// @brief Check whether the kIOS contains a point - bool contain(const Vec3f& p) const; + bool contain(const Vec3s& p) const; /// @brief Check collision between two kIOS bool overlap(const kIOS& other) const; @@ -132,11 +132,11 @@ class COAL_DLLAPI kIOS { CoalScalar& sqrDistLowerBound) const; /// @brief The distance between two kIOS - CoalScalar distance(const kIOS& other, Vec3f* P = NULL, - Vec3f* Q = NULL) const; + CoalScalar distance(const kIOS& other, Vec3s* P = NULL, + Vec3s* Q = NULL) const; /// @brief A simple way to merge the kIOS and a point - kIOS& operator+=(const Vec3f& p); + kIOS& operator+=(const Vec3s& p); /// @brief Merge the kIOS and another kIOS kIOS& operator+=(const kIOS& other) { @@ -151,7 +151,7 @@ class COAL_DLLAPI kIOS { CoalScalar size() const; /// @brief Center of the kIOS - const Vec3f& center() const { return spheres[0].o; } + const Vec3s& center() const { return spheres[0].o; } /// @brief Width of the kIOS CoalScalar width() const; @@ -167,26 +167,26 @@ class COAL_DLLAPI kIOS { }; /// @brief Translate the kIOS BV -COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t); +COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t); /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) /// and b2 is in identity. /// @todo Not efficient -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2); /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) /// and b2 is in identity. /// @todo Not efficient -COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, +COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound); /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation -COAL_DLLAPI CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, - const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, - Vec3f* Q = NULL); +COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, + const kIOS& b1, const kIOS& b2, Vec3s* P = NULL, + Vec3s* Q = NULL); } // namespace coal diff --git a/include/coal/BVH/BVH_model.h b/include/coal/BVH/BVH_model.h index de80f330a..722578f48 100644 --- a/include/coal/BVH/BVH_model.h +++ b/include/coal/BVH/BVH_model.h @@ -65,13 +65,13 @@ class BVSplitter; class COAL_DLLAPI BVHModelBase : public CollisionGeometry { public: /// @brief Geometry point data - std::shared_ptr> vertices; + std::shared_ptr> vertices; /// @brief Geometry triangle index data, will be NULL for point clouds std::shared_ptr> tri_indices; /// @brief Geometry point data in previous frame - std::shared_ptr> prev_vertices; + std::shared_ptr> prev_vertices; /// @brief Number of triangles unsigned int num_tris; @@ -114,23 +114,23 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0); /// @brief Add one point in the new BVH model - int addVertex(const Vec3f& p); + int addVertex(const Vec3s& p); /// @brief Add points in the new BVH model - int addVertices(const Matrixx3f& points); + int addVertices(const MatrixX3s& points); /// @brief Add triangles in the new BVH model int addTriangles(const Matrixx3i& triangles); /// @brief Add one triangle in the new BVH model - int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); + int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); /// @brief Add a set of triangles in the new BVH model - int addSubModel(const std::vector& ps, + int addSubModel(const std::vector& ps, const std::vector& ts); /// @brief Add a set of points in the new BVH model - int addSubModel(const std::vector& ps); + int addSubModel(const std::vector& ps); /// @brief End BVH model construction, will build the bounding volume /// hierarchy @@ -141,13 +141,13 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { int beginReplaceModel(); /// @brief Replace one point in the old BVH model - int replaceVertex(const Vec3f& p); + int replaceVertex(const Vec3s& p); /// @brief Replace one triangle in the old BVH model - int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); + int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); /// @brief Replace a set of points in the old BVH model - int replaceSubModel(const std::vector& ps); + int replaceSubModel(const std::vector& ps); /// @brief End BVH model replacement, will also refit or rebuild the bounding /// volume hierarchy @@ -159,13 +159,13 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { int beginUpdateModel(); /// @brief Update one point in the old BVH model - int updateVertex(const Vec3f& p); + int updateVertex(const Vec3s& p); /// @brief Update one triangle in the old BVH model - int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); + int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3); /// @brief Update a set of points in the old BVH model - int updateSubModel(const std::vector& ps); + int updateSubModel(const std::vector& ps); /// @brief End BVH model update, will also refit or rebuild the bounding /// volume hierarchy @@ -198,16 +198,16 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { /// save one matrix transformation. virtual void makeParentRelative() = 0; - Vec3f computeCOM() const { + Vec3s computeCOM() const { CoalScalar vol = 0; - Vec3f com(0, 0, 0); + Vec3s com(0, 0, 0); if (!(vertices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "vertices." << std::endl; return com; } - const std::vector& vertices_ = *vertices; + const std::vector& vertices_ = *vertices; if (!(tri_indices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "triangles." @@ -236,7 +236,7 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { << std::endl; return vol; } - const std::vector& vertices_ = *vertices; + const std::vector& vertices_ = *vertices; if (!(tri_indices.get())) { std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain " "triangles." @@ -254,10 +254,10 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { return vol / 6; } - Matrix3f computeMomentofInertia() const { - Matrix3f C = Matrix3f::Zero(); + Matrix3s computeMomentofInertia() const { + Matrix3s C = Matrix3s::Zero(); - Matrix3f C_canonical; + Matrix3s C_canonical; C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0; @@ -267,7 +267,7 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { << std::endl; return C; } - const std::vector& vertices_ = *vertices; + const std::vector& vertices_ = *vertices; if (!(vertices.get())) { std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does " "not contain vertices." @@ -277,15 +277,15 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry { const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices_[i]; - const Vec3f& v1 = vertices_[tri[0]]; - const Vec3f& v2 = vertices_[tri[1]]; - const Vec3f& v3 = vertices_[tri[2]]; - Matrix3f A; + const Vec3s& v1 = vertices_[tri[0]]; + const Vec3s& v2 = vertices_[tri[1]]; + const Vec3s& v3 = vertices_[tri[2]]; + Matrix3s A; A << v1.transpose(), v2.transpose(), v3.transpose(); C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); } - return C.trace() * Matrix3f::Identity() - C; + return C.trace() * Matrix3s::Identity() - C; } protected: @@ -368,8 +368,8 @@ class COAL_DLLAPI BVHModel : public BVHModelBase { /// transform related to its parent BV node. When traversing the BVH, this can /// save one matrix transformation. void makeParentRelative() { - Matrix3f I(Matrix3f::Identity()); - makeParentRelativeRecurse(0, I, Vec3f::Zero()); + Matrix3s I(Matrix3s::Identity()); + makeParentRelativeRecurse(0, I, Vec3s::Zero()); } protected: @@ -409,8 +409,8 @@ class COAL_DLLAPI BVHModel : public BVHModelBase { /// @ recursively compute each bv's transform related to its parent. For /// default BV, only the translation works. For oriented BV (OBB, RSS, /// OBBRSS), special implementation is provided. - void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c) { + void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; if (!bvs_[static_cast(bv_id)].isLeaf()) { makeParentRelativeRecurse(bvs_[static_cast(bv_id)].first_child, @@ -497,17 +497,17 @@ class COAL_DLLAPI BVHModel : public BVHModelBase { /// @} template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c); +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c); template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c); +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c); template <> void BVHModel::makeParentRelativeRecurse(int bv_id, - Matrix3f& parent_axes, - const Vec3f& parent_c); + Matrix3s& parent_axes, + const Vec3s& parent_c); /// @brief Specialization of getNodeType() for BVHModel with different BV types template <> diff --git a/include/coal/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h index a3687ab42..ef24f8845 100644 --- a/include/coal/BVH/BVH_utility.h +++ b/include/coal/BVH/BVH_utility.h @@ -82,33 +82,33 @@ COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, /// @brief Compute the covariance matrix for a set or subset of points. if ts = /// null, then indices refer to points directly; otherwise refer to triangles -COAL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, +COAL_DLLAPI void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - Matrix3f& M); + Matrix3s& M); /// @brief Compute the RSS bounding volume parameters: radius, rectangle size /// and the origin, given the BV axises. COAL_DLLAPI void getRadiusAndOriginAndRectangleSize( - Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Matrix3f& axes, Vec3f& origin, CoalScalar l[2], CoalScalar& r); + Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, + const Matrix3s& axes, Vec3s& origin, CoalScalar l[2], CoalScalar& r); /// @brief Compute the bounding volume extent and center for a set or subset of /// points, given the BV axises. -COAL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, +COAL_DLLAPI void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - Matrix3f& axes, Vec3f& center, - Vec3f& extent); + Matrix3s& axes, Vec3s& center, + Vec3s& extent); /// @brief Compute the center and radius for a triangle's circumcircle -COAL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b, - const Vec3f& c, Vec3f& center, +COAL_DLLAPI void circumCircleComputation(const Vec3s& a, const Vec3s& b, + const Vec3s& c, Vec3s& center, CoalScalar& radius); /// @brief Compute the maximum distance from a given center point to a point /// cloud -COAL_DLLAPI CoalScalar maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, +COAL_DLLAPI CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Vec3f& query); + const Vec3s& query); } // namespace coal diff --git a/include/coal/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h index 28c06e57c..118ee65a9 100644 --- a/include/coal/broadphase/broadphase_SaP.h +++ b/include/coal/broadphase/broadphase_SaP.h @@ -146,10 +146,10 @@ class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager { EndPoint* next[3]; /// @brief get the value of the end point - const Vec3f& getVal() const; + const Vec3s& getVal() const; /// @brief set the value of the end point - Vec3f& getVal(); + Vec3s& getVal(); CoalScalar getVal(int i) const; diff --git a/include/coal/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h index 1509bcd70..9f155a4e0 100644 --- a/include/coal/broadphase/broadphase_spatialhash-inl.h +++ b/include/coal/broadphase/broadphase_spatialhash-inl.h @@ -45,7 +45,7 @@ namespace coal { //============================================================================== template SpatialHashingCollisionManager::SpatialHashingCollisionManager( - CoalScalar cell_size, const Vec3f& scene_min, const Vec3f& scene_max, + CoalScalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max, unsigned int default_table_size) : scene_limit(AABB(scene_min, scene_max)), hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) { @@ -313,7 +313,7 @@ bool SpatialHashingCollisionManager::distance_( auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; auto aabb = obj->getAABB(); if (min_dist < (std::numeric_limits::max)()) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } @@ -354,7 +354,7 @@ bool SpatialHashingCollisionManager::distance_( break; } else { if (min_dist < old_min_distance) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { @@ -499,7 +499,7 @@ size_t SpatialHashingCollisionManager::size() const { //============================================================================== template void SpatialHashingCollisionManager::computeBound( - std::vector& objs, Vec3f& l, Vec3f& u) { + std::vector& objs, Vec3s& l, Vec3s& u) { AABB bound; for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB(); diff --git a/include/coal/broadphase/broadphase_spatialhash.h b/include/coal/broadphase/broadphase_spatialhash.h index e2828a53d..10c4830ce 100644 --- a/include/coal/broadphase/broadphase_spatialhash.h +++ b/include/coal/broadphase/broadphase_spatialhash.h @@ -56,8 +56,8 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { typedef BroadPhaseCollisionManager Base; using Base::getObjects; - SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3f& scene_min, - const Vec3f& scene_max, + SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3s& scene_min, + const Vec3s& scene_max, unsigned int default_table_size = 1000); ~SpatialHashingCollisionManager(); @@ -117,8 +117,8 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { size_t size() const; /// @brief compute the bound for the environent - static void computeBound(std::vector& objs, Vec3f& l, - Vec3f& u); + static void computeBound(std::vector& objs, Vec3s& l, + Vec3s& u); protected: /// @brief perform collision test between one object and all the objects diff --git a/include/coal/broadphase/detail/hierarchy_tree-inl.h b/include/coal/broadphase/detail/hierarchy_tree-inl.h index b771e77e5..af29cf6a5 100644 --- a/include/coal/broadphase/detail/hierarchy_tree-inl.h +++ b/include/coal/broadphase/detail/hierarchy_tree-inl.h @@ -159,7 +159,7 @@ template struct UpdateImpl { static bool run(const HierarchyTree& tree, typename HierarchyTree::Node* leaf, const BV& bv, - const Vec3f& /*vel*/, CoalScalar /*margin*/) { + const Vec3s& /*vel*/, CoalScalar /*margin*/) { if (leaf->bv.contain(bv)) return false; tree.update_(leaf, bv); return true; @@ -167,7 +167,7 @@ struct UpdateImpl { static bool run(const HierarchyTree& tree, typename HierarchyTree::Node* leaf, const BV& bv, - const Vec3f& /*vel*/) { + const Vec3s& /*vel*/) { if (leaf->bv.contain(bv)) return false; tree.update_(leaf, bv); return true; @@ -176,14 +176,14 @@ struct UpdateImpl { //============================================================================== template -bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel, +bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3s& vel, CoalScalar margin) { return UpdateImpl::run(*this, leaf, bv, vel, margin); } //============================================================================== template -bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3f& vel) { +bool HierarchyTree::update(Node* leaf, const BV& bv, const Vec3s& vel) { return UpdateImpl::run(*this, leaf, bv, vel); } @@ -408,7 +408,7 @@ typename HierarchyTree::Node* HierarchyTree::topdown_1( long num_leaves = lend - lbeg; if (num_leaves > 1) { if (num_leaves > bu_threshold) { - Vec3f split_p = (*lbeg)->bv.center(); + Vec3s split_p = (*lbeg)->bv.center(); BV vol = (*lbeg)->bv; NodeVecIterator it; for (it = lbeg + 1; it < lend; ++it) { @@ -420,7 +420,7 @@ typename HierarchyTree::Node* HierarchyTree::topdown_1( long bestmidp = num_leaves; int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; for (it = lbeg; it < lend; ++it) { - Vec3f x = (*it)->bv.center() - split_p; + Vec3s x = (*it)->bv.center() - split_p; for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } @@ -970,9 +970,9 @@ struct SelectImpl { const AABB& bv = node.bv; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; @@ -983,9 +983,9 @@ struct SelectImpl { const AABB& bv = query; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; diff --git a/include/coal/broadphase/detail/hierarchy_tree.h b/include/coal/broadphase/detail/hierarchy_tree.h index 67a6b7ec9..6b5fe5261 100644 --- a/include/coal/broadphase/detail/hierarchy_tree.h +++ b/include/coal/broadphase/detail/hierarchy_tree.h @@ -100,10 +100,10 @@ class HierarchyTree { bool update(Node* leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction - bool update(Node* leaf, const BV& bv, const Vec3f& vel, CoalScalar margin); + bool update(Node* leaf, const BV& bv, const Vec3s& vel, CoalScalar margin); /// @brief update one leaf's bounding volume, with prediction - bool update(Node* leaf, const BV& bv, const Vec3f& vel); + bool update(Node* leaf, const BV& bv, const Vec3s& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; diff --git a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h index 9ac67f89c..fa902e67c 100644 --- a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h +++ b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h @@ -295,7 +295,7 @@ bool HierarchyTree::update(size_t leaf, const BV& bv) { //============================================================================== template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3s& vel, CoalScalar margin) { COAL_UNUSED_VARIABLE(bv); COAL_UNUSED_VARIABLE(vel); @@ -308,7 +308,7 @@ bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, //============================================================================== template -bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel) { +bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3s& vel) { COAL_UNUSED_VARIABLE(vel); if (nodes[leaf].bv.contain(bv)) return false; @@ -556,7 +556,7 @@ size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { long num_leaves = lend - lbeg; if (num_leaves > 1) { if (num_leaves > bu_threshold) { - Vec3f split_p = nodes[*lbeg].bv.center(); + Vec3s split_p = nodes[*lbeg].bv.center(); BV vol = nodes[*lbeg].bv; for (size_t* i = lbeg + 1; i < lend; ++i) { split_p += nodes[*i].bv.center(); @@ -567,7 +567,7 @@ size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { int bestmidp = (int)num_leaves; int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; for (size_t* i = lbeg; i < lend; ++i) { - Vec3f x = nodes[*i].bv.center() - split_p; + Vec3s x = nodes[*i].bv.center() - split_p; for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } @@ -949,9 +949,9 @@ struct SelectImpl { const AABB& bv = nodes[query].bv; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; @@ -962,9 +962,9 @@ struct SelectImpl { const AABB& bv = query; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; - Vec3f v = bv.min_ + bv.max_; - Vec3f v1 = v - (bv1.min_ + bv1.max_); - Vec3f v2 = v - (bv2.min_ + bv2.max_); + Vec3s v = bv.min_ + bv.max_; + Vec3s v1 = v - (bv1.min_ + bv1.max_); + Vec3s v2 = v - (bv2.min_ + bv2.max_); CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; diff --git a/include/coal/broadphase/detail/hierarchy_tree_array.h b/include/coal/broadphase/detail/hierarchy_tree_array.h index a7edf618c..e4f12d16f 100644 --- a/include/coal/broadphase/detail/hierarchy_tree_array.h +++ b/include/coal/broadphase/detail/hierarchy_tree_array.h @@ -114,10 +114,10 @@ class HierarchyTree { bool update(size_t leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction - bool update(size_t leaf, const BV& bv, const Vec3f& vel, CoalScalar margin); + bool update(size_t leaf, const BV& bv, const Vec3s& vel, CoalScalar margin); /// @brief update one leaf's bounding volume, with prediction - bool update(size_t leaf, const BV& bv, const Vec3f& vel); + bool update(size_t leaf, const BV& bv, const Vec3s& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; diff --git a/include/coal/broadphase/detail/morton-inl.h b/include/coal/broadphase/detail/morton-inl.h index 4f27aef91..b646f299e 100644 --- a/include/coal/broadphase/detail/morton-inl.h +++ b/include/coal/broadphase/detail/morton-inl.h @@ -62,7 +62,7 @@ morton_functor::morton_functor(const AABB& bbox) //============================================================================== template -uint32_t morton_functor::operator()(const Vec3f& point) const { +uint32_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); @@ -82,7 +82,7 @@ morton_functor::morton_functor(const AABB& bbox) //============================================================================== template -uint64_t morton_functor::operator()(const Vec3f& point) const { +uint64_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); @@ -115,7 +115,7 @@ morton_functor>::morton_functor(const AABB& bbox) //============================================================================== template std::bitset morton_functor>::operator()( - const Vec3f& point) const { + const Vec3s& point) const { S x = (point[0] - base[0]) * inv[0]; S y = (point[1] - base[1]) * inv[1]; S z = (point[2] - base[2]) * inv[2]; diff --git a/include/coal/broadphase/detail/morton.h b/include/coal/broadphase/detail/morton.h index f2e8073bb..f7a9c9473 100644 --- a/include/coal/broadphase/detail/morton.h +++ b/include/coal/broadphase/detail/morton.h @@ -71,10 +71,10 @@ template struct morton_functor { morton_functor(const AABB& bbox); - uint32_t operator()(const Vec3f& point) const; + uint32_t operator()(const Vec3s& point) const; - const Vec3f base; - const Vec3f inv; + const Vec3s base; + const Vec3s inv; static constexpr size_t bits(); }; @@ -87,10 +87,10 @@ template struct morton_functor { morton_functor(const AABB& bbox); - uint64_t operator()(const Vec3f& point) const; + uint64_t operator()(const Vec3s& point) const; - const Vec3f base; - const Vec3f inv; + const Vec3s base; + const Vec3s inv; static constexpr size_t bits(); }; @@ -103,10 +103,10 @@ struct morton_functor> { morton_functor(const AABB& bbox); - std::bitset operator()(const Vec3f& point) const; + std::bitset operator()(const Vec3s& point) const; - const Vec3f base; - const Vec3f inv; + const Vec3s base; + const Vec3s inv; static constexpr size_t bits(); }; diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index 488211f6d..7be9827ac 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -85,7 +85,7 @@ struct COAL_DLLAPI Contact { /// one contact point but have a zero intersection volume). If the shapes /// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) = /// (p2-p1).norm(). - Vec3f normal; + Vec3s normal; /// @brief nearest points associated to this contact. /// @note Also referred as "witness points" in other collision libraries. @@ -96,10 +96,10 @@ struct COAL_DLLAPI Contact { /// vector. If o1 and o2 have multiple contacts, the nearest_points are /// associated with the contact which has the greatest penetration depth. /// TODO (louis): rename `nearest_points` to `witness_points`. - std::array nearest_points; + std::array nearest_points; /// @brief contact position, in world space - Vec3f pos; + Vec3s pos; /// @brief penetration depth CoalScalar penetration_depth; @@ -111,7 +111,7 @@ struct COAL_DLLAPI Contact { Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) { penetration_depth = (std::numeric_limits::max)(); nearest_points[0] = nearest_points[1] = normal = pos = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); } Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, @@ -119,11 +119,11 @@ struct COAL_DLLAPI Contact { : o1(o1_), o2(o2_), b1(b1_), b2(b2_) { penetration_depth = (std::numeric_limits::max)(); nearest_points[0] = nearest_points[1] = normal = pos = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); } Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, - int b2_, const Vec3f& pos_, const Vec3f& normal_, CoalScalar depth_) + int b2_, const Vec3s& pos_, const Vec3s& normal_, CoalScalar depth_) : o1(o1_), o2(o2_), b1(b1_), @@ -135,7 +135,7 @@ struct COAL_DLLAPI Contact { penetration_depth(depth_) {} Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, - int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_, + int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_, CoalScalar depth_) : o1(o1_), o2(o2_), @@ -177,7 +177,7 @@ struct COAL_DLLAPI QueryRequest { bool enable_cached_gjk_guess; /// @brief the gjk initial guess set by user - mutable Vec3f cached_gjk_guess; + mutable Vec3s cached_gjk_guess; /// @brief the support function initial guess set by user mutable support_func_guess_t cached_support_func_guess; @@ -274,7 +274,7 @@ struct COAL_DLLAPI QueryRequest { /// @brief base class for all query results struct COAL_DLLAPI QueryResult { /// @brief stores the last GJK ray when relevant. - Vec3f cached_gjk_guess; + Vec3s cached_gjk_guess; /// @brief stores the last support function vertex index, when relevant. support_func_guess_t cached_support_func_guess; @@ -283,7 +283,7 @@ struct COAL_DLLAPI QueryResult { CPUTimes timings; QueryResult() - : cached_gjk_guess(Vec3f::Zero()), + : cached_gjk_guess(Vec3s::Zero()), cached_support_func_guess(support_func_guess_t::Constant(-1)) {} }; @@ -402,7 +402,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// @brief normal associated to nearest_points. /// Same as `CollisionResult::nearest_points` but for the normal. - Vec3f normal; + Vec3s normal; /// @brief nearest points. /// A `CollisionResult` can have multiple contacts. @@ -411,13 +411,13 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// For bounding volumes and BVHs, these nearest points are available /// only when distance_lower_bound is inferior to /// CollisionRequest::break_distance. - std::array nearest_points; + std::array nearest_points; public: CollisionResult() : distance_lower_bound((std::numeric_limits::max)()) { nearest_points[0] = nearest_points[1] = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); } /// @brief Update the lower bound only if the distance is inferior. @@ -485,7 +485,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult { contacts.clear(); timings.clear(); nearest_points[0] = nearest_points[1] = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); } /// @brief reposition Contact objects when fcl inverts them @@ -511,7 +511,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult { /// to compute a contact volume instead of a contact patch. struct COAL_DLLAPI ContactPatch { public: - using Polygon = std::vector; + using Polygon = std::vector; /// @brief Frame of the set, expressed in the world coordinates. /// The z-axis of the frame's rotation is the contact patch normal. @@ -566,7 +566,7 @@ struct COAL_DLLAPI ContactPatch { } /// @brief Normal of the contact patch, expressed in the WORLD frame. - Vec3f getNormal() const { + Vec3s getNormal() const { if (this->direction == PatchDirection::INVERTED) { return -this->tf.rotation().col(2); } @@ -581,14 +581,14 @@ struct COAL_DLLAPI ContactPatch { /// of the set. It then takes only the x and y components of the vector, /// effectively doing a projection onto the plane to which the set belongs. /// TODO(louis): if necessary, we can store the offset to the plane (x, y). - void addPoint(const Vec3f& point_3d) { - const Vec3f point = this->tf.inverseTransform(point_3d); + void addPoint(const Vec3s& point_3d) { + const Vec3s point = this->tf.inverseTransform(point_3d); this->m_points.emplace_back(point.template head<2>()); } /// @brief Get the i-th point of the set, expressed in the 3D world frame. - Vec3f getPoint(const size_t i) const { - Vec3f point(0, 0, 0); + Vec3s getPoint(const size_t i) const { + Vec3s point(0, 0, 0); point.head<2>() = this->point(i); point = tf.transform(point); return point; @@ -597,8 +597,8 @@ struct COAL_DLLAPI ContactPatch { /// @brief Get the i-th point of the contact patch, projected back onto the /// first shape of the collision pair. This point is expressed in the 3D /// world frame. - Vec3f getPointShape1(const size_t i) const { - Vec3f point = this->getPoint(i); + Vec3s getPointShape1(const size_t i) const { + Vec3s point = this->getPoint(i); point -= (this->penetration_depth / 2) * this->getNormal(); return point; } @@ -606,8 +606,8 @@ struct COAL_DLLAPI ContactPatch { /// @brief Get the i-th point of the contact patch, projected back onto the /// first shape of the collision pair. This 3D point is expressed in the world /// frame. - Vec3f getPointShape2(const size_t i) const { - Vec3f point = this->getPoint(i); + Vec3s getPointShape2(const size_t i) const { + Vec3s point = this->getPoint(i); point += (this->penetration_depth / 2) * this->getNormal(); return point; } @@ -619,7 +619,7 @@ struct COAL_DLLAPI ContactPatch { const Polygon& points() const { return this->m_points; } /// @brief Getter for the i-th 2D point in the set. - Vec2f& point(const size_t i) { + Vec2s& point(const size_t i) { COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); if (i < this->m_points.size()) { return this->m_points[i]; @@ -628,7 +628,7 @@ struct COAL_DLLAPI ContactPatch { } /// @brief Const getter for the i-th 2D point in the set. - const Vec2f& point(const size_t i) const { + const Vec2s& point(const size_t i) const { COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error); if (i < this->m_points.size()) { return this->m_points[i]; @@ -680,9 +680,9 @@ struct COAL_DLLAPI ContactPatch { // Check all points of the contact patch. for (size_t i = 0; i < this->size(); ++i) { bool found = false; - const Vec3f pi = this->getPoint(i); + const Vec3s pi = this->getPoint(i); for (size_t j = 0; j < other.size(); ++j) { - const Vec3f other_pj = other.getPoint(j); + const Vec3s other_pj = other.getPoint(j); if (pi.isApprox(other_pj, tol)) { found = true; } @@ -1058,11 +1058,11 @@ struct COAL_DLLAPI DistanceResult : QueryResult { CoalScalar min_distance; /// @brief normal. - Vec3f normal; + Vec3s normal; /// @brief nearest points. /// See CollisionResult::nearest_points. - std::array nearest_points; + std::array nearest_points; /// @brief collision object 1 const CollisionGeometry* o1; @@ -1088,8 +1088,8 @@ struct COAL_DLLAPI DistanceResult : QueryResult { DistanceResult( CoalScalar min_distance_ = (std::numeric_limits::max)()) : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { - const Vec3f nan( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); + const Vec3s nan( + Vec3s::Constant(std::numeric_limits::quiet_NaN())); nearest_points[0] = nearest_points[1] = normal = nan; } @@ -1107,8 +1107,8 @@ struct COAL_DLLAPI DistanceResult : QueryResult { /// @brief add distance information into the result void update(CoalScalar distance, const CollisionGeometry* o1_, - const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, - const Vec3f& p2, const Vec3f& normal_) { + const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1, + const Vec3s& p2, const Vec3s& normal_) { if (min_distance > distance) { min_distance = distance; o1 = o1_; @@ -1137,8 +1137,8 @@ struct COAL_DLLAPI DistanceResult : QueryResult { /// @brief clear the result void clear() { - const Vec3f nan( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); + const Vec3s nan( + Vec3s::Constant(std::numeric_limits::quiet_NaN())); min_distance = (std::numeric_limits::max)(); o1 = NULL; o2 = NULL; @@ -1184,8 +1184,8 @@ inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, CollisionResult& res, const CoalScalar& distance, - const Vec3f& p0, const Vec3f& p1, - const Vec3f& normal) { + const Vec3s& p0, const Vec3s& p1, + const Vec3s& normal) { if (distance < res.distance_lower_bound) { res.distance_lower_bound = distance; res.nearest_points[0] = p0; diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h index 1088ed620..503ca6016 100644 --- a/include/coal/collision_object.h +++ b/include/coal/collision_object.h @@ -94,7 +94,7 @@ enum NODE_TYPE { class COAL_DLLAPI CollisionGeometry { public: CollisionGeometry() - : aabb_center(Vec3f::Constant((std::numeric_limits::max)())), + : aabb_center(Vec3s::Constant((std::numeric_limits::max)())), aabb_radius(-1), cost_density(1), threshold_occupied(1), @@ -148,7 +148,7 @@ class COAL_DLLAPI CollisionGeometry { bool isUncertain() const; /// @brief AABB center in local coordinate - Vec3f aabb_center; + Vec3s aabb_center; /// @brief AABB radius CoalScalar aabb_radius; @@ -170,23 +170,23 @@ class COAL_DLLAPI CollisionGeometry { CoalScalar threshold_free; /// @brief compute center of mass - virtual Vec3f computeCOM() const { return Vec3f::Zero(); } + virtual Vec3s computeCOM() const { return Vec3s::Zero(); } /// @brief compute the inertia matrix, related to the origin - virtual Matrix3f computeMomentofInertia() const { - return Matrix3f::Constant(NAN); + virtual Matrix3s computeMomentofInertia() const { + return Matrix3s::Constant(NAN); } /// @brief compute the volume virtual CoalScalar computeVolume() const { return 0; } /// @brief compute the inertia matrix, related to the com - virtual Matrix3f computeMomentofInertiaRelatedToCOM() const { - Matrix3f C = computeMomentofInertia(); - Vec3f com = computeCOM(); + virtual Matrix3s computeMomentofInertiaRelatedToCOM() const { + Matrix3s C = computeMomentofInertia(); + Vec3s com = computeCOM(); CoalScalar V = computeVolume(); - return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), + return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], C(1, 0) + V * com[1] * com[0], C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), @@ -226,7 +226,7 @@ class COAL_DLLAPI CollisionObject { } CollisionObject(const shared_ptr& cgeom_, - const Matrix3f& R, const Vec3f& T, + const Matrix3s& R, const Vec3s& T, bool compute_local_aabb = true) : cgeom(cgeom_), t(R, T), user_data(nullptr) { init(compute_local_aabb); @@ -261,7 +261,7 @@ class COAL_DLLAPI CollisionObject { } else { aabb.min_ = aabb.max_ = t.getTranslation(); - Vec3f min_world, max_world; + Vec3s min_world, max_world; for (int k = 0; k < 3; ++k) { min_world.array() = t.getRotation().row(k).array() * cgeom->aabb_local.min_.transpose().array(); @@ -281,22 +281,22 @@ class COAL_DLLAPI CollisionObject { void setUserData(void* data) { user_data = data; } /// @brief get translation of the object - inline const Vec3f& getTranslation() const { return t.getTranslation(); } + inline const Vec3s& getTranslation() const { return t.getTranslation(); } /// @brief get matrix rotation of the object - inline const Matrix3f& getRotation() const { return t.getRotation(); } + inline const Matrix3s& getRotation() const { return t.getRotation(); } /// @brief get object's transform inline const Transform3f& getTransform() const { return t; } /// @brief set object's rotation matrix - void setRotation(const Matrix3f& R) { t.setRotation(R); } + void setRotation(const Matrix3s& R) { t.setRotation(R); } /// @brief set object's translation - void setTranslation(const Vec3f& T) { t.setTranslation(T); } + void setTranslation(const Vec3s& T) { t.setTranslation(T); } /// @brief set object's transform - void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); } + void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); } /// @brief set object's transform void setTransform(const Transform3f& tf) { t = tf; } diff --git a/include/coal/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h index 09ae43c13..63c7fd342 100644 --- a/include/coal/contact_patch/contact_patch_solver.h +++ b/include/coal/contact_patch/contact_patch_solver.h @@ -180,8 +180,8 @@ struct COAL_DLLAPI ContactPatchSolver { /// @note we make the following hypothesis: /// 1) c != d (should be when creating initial polytopes) /// 2) (c, d) is not parallel to ray -> if so, we return d. - static Vec2f computeLineSegmentIntersection(const Vec2f& a, const Vec2f& b, - const Vec2f& c, const Vec2f& d); + static Vec2s computeLineSegmentIntersection(const Vec2s& a, const Vec2s& b, + const Vec2s& c, const Vec2s& d); /// @brief Construct support set function for shape. static SupportSetFunction makeSupportSetFunction( diff --git a/include/coal/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx index f730b79da..403612f37 100644 --- a/include/coal/contact_patch/contact_patch_solver.hxx +++ b/include/coal/contact_patch/contact_patch_solver.hxx @@ -44,7 +44,7 @@ namespace coal { // ============================================================================ inline void ContactPatchSolver::set(const ContactPatchRequest& request) { - // Note: it's important for the number of pre-allocated Vec3f in + // Note: it's important for the number of pre-allocated Vec3s in // `m_clipping_sets` to be larger than `request.max_size_patch` // because we don't know in advance how many supports will be discarded to // form the convex-hulls of the shapes supports which will serve as the @@ -138,12 +138,12 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, // We compute the determinant; if it is non-zero, the intersection // has already been computed: it's `Contact::pos`. const Polygon& pts1 = this->support_set_shape1.points(); - const Vec2f& a = pts1[0]; - const Vec2f& b = pts1[1]; + const Vec2s& a = pts1[0]; + const Vec2s& b = pts1[1]; const Polygon& pts2 = this->support_set_shape2.points(); - const Vec2f& c = pts2[0]; - const Vec2f& d = pts2[1]; + const Vec2s& c = pts2[0]; + const Vec2s& d = pts2[1]; const CoalScalar det = (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0)); @@ -153,20 +153,20 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, return; } - const Vec2f cd = (d - c); + const Vec2s cd = (d - c); const CoalScalar l = cd.squaredNorm(); Polygon& patch = contact_patch.points(); // Project a onto [c, d] CoalScalar t1 = (a - c).dot(cd); t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l)); - const Vec2f p1 = c + t1 * cd; + const Vec2s p1 = c + t1 * cd; patch.emplace_back(p1); // Project b onto [c, d] CoalScalar t2 = (b - c).dot(cd); t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l)); - const Vec2f p2 = c + t2 * cd; + const Vec2s p2 = c + t2 * cd; if ((p1 - p2).squaredNorm() >= eps) { patch.emplace_back(p2); } @@ -222,19 +222,19 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, Polygon& current = *(current_ptr); current.clear(); - const Vec2f& a = clipper[i]; - const Vec2f& b = clipper[(i + 1) % clipper_size]; - const Vec2f ab = b - a; + const Vec2s& a = clipper[i]; + const Vec2s& b = clipper[(i + 1) % clipper_size]; + const Vec2s ab = b - a; if (previous.size() == 2) { // // Segment-Polygon case // - const Vec2f& p1 = previous[0]; - const Vec2f& p2 = previous[1]; + const Vec2s& p1 = previous[0]; + const Vec2s& p2 = previous[1]; - const Vec2f ap1 = p1 - a; - const Vec2f ap2 = p2 - a; + const Vec2s ap1 = p1 - a; + const Vec2s ap2 = p2 - a; const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); @@ -256,7 +256,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, // [p1, p2]. if (det1 >= 0) { if (det1 > eps) { - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p1); current.emplace_back(p); continue; @@ -268,7 +268,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, } } else { if (det2 > eps) { - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p2); current.emplace_back(p); continue; @@ -289,11 +289,11 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, const size_t previous_size = previous.size(); for (size_t j = 0; j < previous_size; ++j) { - const Vec2f& p1 = previous[j]; - const Vec2f& p2 = previous[(j + 1) % previous_size]; + const Vec2s& p1 = previous[j]; + const Vec2s& p2 = previous[(j + 1) % previous_size]; - const Vec2f ap1 = p1 - a; - const Vec2f ap2 = p2 - a; + const Vec2s ap1 = p1 - a; + const Vec2s ap2 = p2 - a; const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); @@ -320,7 +320,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, current.emplace_back(p1); this->added_to_patch[j] = true; } - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p); } else { // a, b and p1 are colinear; we add only p1. @@ -331,7 +331,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1, } } else { if (det2 > eps) { - const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2); + const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); current.emplace_back(p); } else { if (!this->added_to_patch[(j + 1) % previous.size()]) { @@ -406,10 +406,10 @@ inline void ContactPatchSolver::reset(const ShapeType1& shape1, } // ========================================================================== -inline Vec2f ContactPatchSolver::computeLineSegmentIntersection( - const Vec2f& a, const Vec2f& b, const Vec2f& c, const Vec2f& d) { - const Vec2f ab = b - a; - const Vec2f n(-ab(1), ab(0)); +inline Vec2s ContactPatchSolver::computeLineSegmentIntersection( + const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) { + const Vec2s ab = b - a; + const Vec2s n(-ab(1), ab(0)); const CoalScalar denominator = n.dot(c - d); if (std::abs(denominator) < std::numeric_limits::epsilon()) { return d; diff --git a/include/coal/data_types.h b/include/coal/data_types.h index b9b64bb76..ea8c97d3e 100644 --- a/include/coal/data_types.h +++ b/include/coal/data_types.h @@ -59,6 +59,8 @@ #endif // COAL_HAS_OCTOMAP namespace coal { +// We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward +// compatibility. typedef double FCL_REAL; typedef Eigen::Matrix Vec3f; typedef Eigen::Matrix Vec2f; @@ -75,15 +77,15 @@ typedef Eigen::Matrix Vec2s; typedef Eigen::Matrix Vec6s; typedef Eigen::Matrix VecXs; typedef Eigen::Matrix Matrix3s; -typedef Eigen::Matrix Matrixx3s; -typedef Eigen::Matrix Matrixx2s; +typedef Eigen::Matrix MatrixX3s; +typedef Eigen::Matrix MatrixX2s; typedef Eigen::Matrix Matrixx3i; typedef Eigen::Matrix MatrixXs; typedef Eigen::Vector2i support_func_guess_t; /// @brief Initial guess to use for the GJK algorithm -/// DefaultGuess: Vec3f(1, 0, 0) +/// DefaultGuess: Vec3s(1, 0, 0) /// CachedGuess: previous vector found by GJK or guess cached by the user /// BoundingVolumeGuess: guess using the centers of the shapes' AABB /// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called diff --git a/include/coal/hfield.h b/include/coal/hfield.h index 4204efc22..9949296ed 100644 --- a/include/coal/hfield.h +++ b/include/coal/hfield.h @@ -151,17 +151,17 @@ struct COAL_DLLAPI HFNode : public HFNodeBase { /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and /// the underlying BV supports distance, return the nearest points. - CoalScalar distance(const HFNode& other, Vec3f* P1 = NULL, - Vec3f* P2 = NULL) const { + CoalScalar distance(const HFNode& other, Vec3s* P1 = NULL, + Vec3s* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } /// @brief Access to the center of the BV - Vec3f getCenter() const { return bv.center(); } + Vec3s getCenter() const { return bv.center(); } /// @brief Access to the orientation of the BV - coal::Matrix3f::IdentityReturnType getOrientation() const { - return Matrix3f::Identity(); + coal::Matrix3s::IdentityReturnType getOrientation() const { + return Matrix3s::Identity(); } virtual ~HFNode() {} @@ -171,7 +171,7 @@ namespace details { template struct UpdateBoundingVolume { - static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) { + static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) { AABB bv_aabb(pointA, pointB); // AABB bv_aabb; // bv_aabb.update(pointA,pointB); @@ -181,7 +181,7 @@ struct UpdateBoundingVolume { template <> struct UpdateBoundingVolume { - static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) { + static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) { AABB bv_aabb(pointA, pointB); convertBV(bv_aabb, bv); // bv.update(pointA,pointB); @@ -226,7 +226,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// \param[in] min_height Minimal height of the height field /// HeightField(const CoalScalar x_dim, const CoalScalar y_dim, - const MatrixXf& heights, + const MatrixXs& heights, const CoalScalar min_height = (CoalScalar)0) : CollisionGeometry() { init(x_dim, y_dim, heights, min_height); @@ -249,12 +249,12 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { num_bvs(other.num_bvs) {} /// @brief Returns a const reference of the grid along the X direction. - const VecXf& getXGrid() const { return x_grid; } + const VecXs& getXGrid() const { return x_grid; } /// @brief Returns a const reference of the grid along the Y direction. - const VecXf& getYGrid() const { return y_grid; } + const VecXs& getYGrid() const { return y_grid; } /// @brief Returns a const reference of the heights - const MatrixXf& getHeights() const { return heights; } + const MatrixXs& getHeights() const { return heights; } /// @brief Returns the dimension of the Height Field along the X direction. CoalScalar getXDim() const { return x_dim; } @@ -276,8 +276,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// @brief Compute the AABB for the HeightField, used for broad-phase /// collision void computeLocalAABB() { - const Vec3f A(x_grid[0], y_grid[0], min_height); - const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1], + const Vec3s A(x_grid[0], y_grid[0], min_height); + const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1], max_height); const AABB aabb_(A, B); @@ -287,7 +287,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { } /// @brief Update Height Field height - void updateHeights(const MatrixXf& new_heights) { + void updateHeights(const MatrixXs& new_heights) { if (new_heights.rows() != heights.rows() || new_heights.cols() != heights.cols()) COAL_THROW_PRETTY( @@ -306,7 +306,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { protected: void init(const CoalScalar x_dim, const CoalScalar y_dim, - const MatrixXf& heights, const CoalScalar min_height) { + const MatrixXs& heights, const CoalScalar min_height) { this->x_dim = x_dim; this->y_dim = y_dim; this->heights = heights.cwiseMax(min_height); @@ -317,8 +317,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { assert(NX >= 2 && "The number of columns is too small."); assert(NY >= 2 && "The number of rows is too small."); - x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim); - y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim); + x_grid = VecXs::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim); + y_grid = VecXs::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim); // Allocate BVS const size_t num_tot_bvs = @@ -333,18 +333,18 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// @brief Get the object type: it is a HFIELD OBJECT_TYPE getObjectType() const { return OT_HFIELD; } - Vec3f computeCOM() const { return Vec3f::Zero(); } + Vec3s computeCOM() const { return Vec3s::Zero(); } CoalScalar computeVolume() const { return 0; } - Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); } + Matrix3s computeMomentofInertia() const { return Matrix3s::Zero(); } protected: /// @brief Dimensions in meters along X and Y directions CoalScalar x_dim, y_dim; /// @brief Elevation values in meters of the Height Field - MatrixXf heights; + MatrixXs heights; /// @brief Minimal height of the Height Field: all values bellow min_height /// will be discarded. @@ -352,7 +352,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { /// @brief Grids along the X and Y directions. Useful for plotting or other /// related things. - VecXf x_grid, y_grid; + VecXs x_grid, y_grid; /// @brief Bounding volume hierarchy BVS bvs; @@ -386,8 +386,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { bv_node.max_height = max_height; - const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); - const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size], + const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); + const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size], y_grid[bv_node.y_id + bv_node.y_size], max_height); details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); @@ -445,10 +445,10 @@ class COAL_DLLAPI HeightField : public CollisionGeometry { bv_node.max_height = max_height; // max_height = std::max(max_height,min_height); - const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height); + const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height); assert(x_id + x_size < x_grid.size()); assert(y_id + y_size < y_grid.size()); - const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], + const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], max_height); details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); diff --git a/include/coal/internal/BV_fitter.h b/include/coal/internal/BV_fitter.h index 93f126782..a115d914f 100644 --- a/include/coal/internal/BV_fitter.h +++ b/include/coal/internal/BV_fitter.h @@ -48,7 +48,7 @@ namespace coal { /// @brief Compute a bounding volume that fits a set of n points. template -void fit(Vec3f* ps, unsigned int n, BV& bv) { +void fit(Vec3s* ps, unsigned int n, BV& bv) { for (unsigned int i = 0; i < n; ++i) // TODO(jcarpent): vectorize { bv += ps[i]; @@ -56,19 +56,19 @@ void fit(Vec3f* ps, unsigned int n, BV& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, OBB& bv); +void fit(Vec3s* ps, unsigned int n, OBB& bv); template <> -void fit(Vec3f* ps, unsigned int n, RSS& bv); +void fit(Vec3s* ps, unsigned int n, RSS& bv); template <> -void fit(Vec3f* ps, unsigned int n, kIOS& bv); +void fit(Vec3s* ps, unsigned int n, kIOS& bv); template <> -void fit(Vec3f* ps, unsigned int n, OBBRSS& bv); +void fit(Vec3s* ps, unsigned int n, OBBRSS& bv); template <> -void fit(Vec3f* ps, unsigned int n, AABB& bv); +void fit(Vec3s* ps, unsigned int n, AABB& bv); /// @brief The class for the default algorithm fitting a bounding volume to a /// set of points @@ -79,7 +79,7 @@ class COAL_DLLAPI BVFitterTpl { virtual ~BVFitterTpl() {} /// @brief Prepare the geometry primitive data for fitting - void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { + void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; @@ -88,7 +88,7 @@ class COAL_DLLAPI BVFitterTpl { /// @brief Prepare the geometry primitive data for fitting, for deformable /// mesh - void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, + void set(Vec3s* vertices_, Vec3s* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; @@ -109,8 +109,8 @@ class COAL_DLLAPI BVFitterTpl { } protected: - Vec3f* vertices; - Vec3f* prev_vertices; + Vec3s* vertices; + Vec3s* prev_vertices; Triangle* tri_indices; BVHModelType type; }; diff --git a/include/coal/internal/BV_splitter.h b/include/coal/internal/BV_splitter.h index 6106a203f..9bfdf459e 100644 --- a/include/coal/internal/BV_splitter.h +++ b/include/coal/internal/BV_splitter.h @@ -64,7 +64,7 @@ class BVSplitter { virtual ~BVSplitter() {} /// @brief Set the geometry data needed by the split rule - void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { + void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; tri_indices = tri_indices_; type = type_; @@ -90,7 +90,7 @@ class BVSplitter { } /// @brief Apply the split rule on a given point - bool apply(const Vec3f& q) const { return q[split_axis] > split_value; } + bool apply(const Vec3s& q) const { return q[split_axis] > split_value; } /// @brief Clear the geometry data set before void clear() { @@ -105,7 +105,7 @@ class BVSplitter { /// is needed. For oriented node, we can use a vector to make a better split /// decision. int split_axis; - Vec3f split_vector; + Vec3s split_vector; /// @brief The split threshold, different primitives are splitted according /// whether their projection on the split_axis is larger or smaller than the @@ -113,7 +113,7 @@ class BVSplitter { CoalScalar split_value; /// @brief The mesh vertices or points handled by the splitter - Vec3f* vertices; + Vec3s* vertices; /// @brief The triangles handled by the splitter Triangle* tri_indices; @@ -126,7 +126,7 @@ class BVSplitter { /// @brief Split algorithm 1: Split the node from center void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) { - Vec3f center = bv.center(); + Vec3s center = bv.center(); int axis = 2; if (bv.width() >= bv.height() && bv.width() >= bv.depth()) @@ -207,16 +207,16 @@ class BVSplitter { }; template <> -bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> -bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> -bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> -bool COAL_DLLAPI BVSplitter::apply(const Vec3f& q) const; +bool COAL_DLLAPI BVSplitter::apply(const Vec3s& q) const; template <> void COAL_DLLAPI BVSplitter::computeRule_bvcenter( diff --git a/include/coal/internal/intersect.h b/include/coal/internal/intersect.h index acbf532f2..4b5ef8d70 100644 --- a/include/coal/internal/intersect.h +++ b/include/coal/internal/intersect.h @@ -47,8 +47,8 @@ namespace coal { /// @brief CCD intersect kernel among primitives class COAL_DLLAPI Intersect { public: - static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, - const Vec3f& v3, Vec3f* n, CoalScalar* t); + static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2, + const Vec3s& v3, Vec3s* n, CoalScalar* t); }; // class Intersect /// @brief Project functions @@ -69,28 +69,28 @@ class COAL_DLLAPI Project { }; /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, - const Vec3f& p); + static ProjectResult projectLine(const Vec3s& a, const Vec3s& b, + const Vec3s& p); /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& p); + static ProjectResult projectTriangle(const Vec3s& a, const Vec3s& b, + const Vec3s& c, const Vec3s& p); /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& d, - const Vec3f& p); + static ProjectResult projectTetrahedra(const Vec3s& a, const Vec3s& b, + const Vec3s& c, const Vec3s& d, + const Vec3s& p); /// @brief Project origin (0) onto line a-b - static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b); + static ProjectResult projectLineOrigin(const Vec3s& a, const Vec3s& b); /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, - const Vec3f& c); + static ProjectResult projectTriangleOrigin(const Vec3s& a, const Vec3s& b, + const Vec3s& c); /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, - const Vec3f& c, const Vec3f& d); + static ProjectResult projectTetrahedraOrigin(const Vec3s& a, const Vec3s& b, + const Vec3s& c, const Vec3s& d); }; /// @brief Triangle distance functions @@ -101,8 +101,8 @@ class COAL_DLLAPI TriangleDistance { /// The second segment is Q + t * B /// X, Y are the closest points on the two segments /// VEC is the vector between X and Y - static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, - const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y); + static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q, + const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -113,13 +113,13 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P, + Vec3s& Q); - static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, Vec3s& P, + Vec3s& Q); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -131,9 +131,9 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Matrix3s& R, const Vec3s& Tl, Vec3s& P, + Vec3s& Q); /// Compute squared distance between triangles /// @param S and T are two triangles @@ -145,8 +145,8 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, Vec3f& P, Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Transform3f& tf, Vec3s& P, Vec3s& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices @@ -158,11 +158,11 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, Vec3f& P, - Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Matrix3s& R, const Vec3s& Tl, Vec3s& P, + Vec3s& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices @@ -174,10 +174,10 @@ class COAL_DLLAPI TriangleDistance { /// if the triangles overlap, P and Q are basically a random pair of points /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. - static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, Vec3f& P, Vec3f& Q); + static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Transform3f& tf, Vec3s& P, Vec3s& Q); }; } // namespace coal diff --git a/include/coal/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h index 07a4a97e6..1d518e7f0 100644 --- a/include/coal/internal/shape_shape_func.h +++ b/include/coal/internal/shape_shape_func.h @@ -58,7 +58,7 @@ struct ShapeShapeDistancer { // Witness points on shape1 and shape2, normal pointing from shape1 to // shape2. - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2, @@ -73,8 +73,8 @@ struct ShapeShapeDistancer { static CoalScalar run(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const bool compute_signed_distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { + const bool compute_signed_distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { const ShapeType1* obj1 = static_cast(o1); const ShapeType2* obj2 = static_cast(o2); return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2, @@ -116,8 +116,8 @@ CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver, - const bool compute_signed_distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { + const bool compute_signed_distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { return ::coal::ShapeShapeDistancer::run( o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal); } @@ -143,7 +143,7 @@ struct ShapeShapeCollider { const bool compute_penetration = request.enable_contact || (request.security_margin < 0); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; CoalScalar distance = internal::ShapeShapeDistance( o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal); @@ -219,14 +219,14 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ - Vec3f& p2, Vec3f& normal); \ + const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ + Vec3s& p2, Vec3s& normal); \ template <> \ COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ - Vec3f& p2, Vec3f& normal); \ + const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ + Vec3s& p2, Vec3s& normal); \ template <> \ inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ @@ -263,8 +263,8 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const bool compute_signed_distance, Vec3f& p1, \ - Vec3f& p2, Vec3f& normal); \ + const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ + Vec3s& p2, Vec3s& normal); \ template <> \ inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ const CollisionGeometry* o1, const Transform3f& tf1, \ diff --git a/include/coal/internal/traversal.h b/include/coal/internal/traversal.h index 36b8004fe..32fcac457 100644 --- a/include/coal/internal/traversal.h +++ b/include/coal/internal/traversal.h @@ -46,21 +46,21 @@ enum { RelativeTransformationIsIdentity = 1 }; namespace details { template struct COAL_DLLAPI RelativeTransformation { - RelativeTransformation() : R(Matrix3f::Identity()) {} + RelativeTransformation() : R(Matrix3s::Identity()) {} - const Matrix3f& _R() const { return R; } - const Vec3f& _T() const { return T; } + const Matrix3s& _R() const { return R; } + const Vec3s& _T() const { return T; } - Matrix3f R; - Vec3f T; + Matrix3s R; + Vec3s T; }; template <> struct COAL_DLLAPI RelativeTransformation { - static const Matrix3f& _R() { + static const Matrix3s& _R() { COAL_THROW_PRETTY("should never reach this point", std::logic_error); } - static const Vec3f& _T() { + static const Vec3s& _T() { COAL_THROW_PRETTY("should never reach this point", std::logic_error); } }; diff --git a/include/coal/internal/traversal_node_bvh_hfield.h b/include/coal/internal/traversal_node_bvh_hfield.h index ad2d203dd..fc550eb21 100644 --- a/include/coal/internal/traversal_node_bvh_hfield.h +++ b/include/coal/internal/traversal_node_bvh_hfield.h @@ -182,9 +182,9 @@ class MeshHeightFieldCollisionTraversalNode int primitive_id1 = node1.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; - const Vec3f& P1 = vertices1[tri_id1[0]]; - const Vec3f& P2 = vertices1[tri_id1[1]]; - const Vec3f& P3 = vertices1[tri_id1[2]]; + const Vec3s& P1 = vertices1[tri_id1[0]]; + const Vec3s& P2 = vertices1[tri_id1[1]]; + const Vec3s& P3 = vertices1[tri_id1[2]]; TriangleP tri1(P1, P2, P3); @@ -193,16 +193,16 @@ class MeshHeightFieldCollisionTraversalNode details::buildConvexTriangles(node2, *this->model2, convex2, convex2); GJKSolver solver; - Vec3f p1, + Vec3s p1, p2; // closest points if no collision contact points if collision. - Vec3f normal; + Vec3s normal; CoalScalar distance; solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2, normal); CoalScalar distToCollision = distance - this->request.security_margin; sqrDistLowerBound = distance * distance; if (distToCollision <= 0) { // collision - Vec3f p(p1); // contact point + Vec3s p(p1); // contact point CoalScalar penetrationDepth(0); if (this->result->numContacts() < this->request.num_max_contacts) { // How much (Q1, Q2, Q3) should be moved so that all vertices are @@ -229,7 +229,7 @@ class MeshHeightFieldCollisionTraversalNode mutable int num_leaf_tests; mutable CoalScalar query_time_seconds; - Vec3f* vertices1 Triangle* tri_indices1; + Vec3s* vertices1 Triangle* tri_indices1; details::RelativeTransformation RT; }; @@ -253,7 +253,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl { static CoalScalar run(const BVNode& b1, const BVNode& b2) { return b1.distance(b2); } - static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { return distance(R, T, b1.bv, b2.bv); } @@ -271,7 +271,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static CoalScalar run(const Matrix3f& R, const Vec3f& T, + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); @@ -296,7 +296,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static CoalScalar run(const Matrix3f& R, const Vec3f& T, + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); @@ -439,16 +439,16 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; - const Vec3f& t11 = vertices1[tri_id1[0]]; - const Vec3f& t12 = vertices1[tri_id1[1]]; - const Vec3f& t13 = vertices1[tri_id1[2]]; + const Vec3s& t11 = vertices1[tri_id1[0]]; + const Vec3s& t12 = vertices1[tri_id1[1]]; + const Vec3s& t13 = vertices1[tri_id1[2]]; - const Vec3f& t21 = vertices2[tri_id2[0]]; - const Vec3f& t22 = vertices2[tri_id2[1]]; - const Vec3f& t23 = vertices2[tri_id2[2]]; + const Vec3s& t21 = vertices2[tri_id2[0]]; + const Vec3s& t22 = vertices2[tri_id2[1]]; + const Vec3s& t23 = vertices2[tri_id2[2]]; // nearest point pair - Vec3f P1, P2, normal; + Vec3s P1, P2, normal; CoalScalar d2; if (RTIsIdentity) @@ -471,8 +471,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { return false; } - Vec3f* vertices1; - Vec3f* vertices2; + Vec3s* vertices1; + Vec3s* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; @@ -489,8 +489,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { const Triangle& init_tri1 = tri_indices1[init_tri_id1]; const Triangle& init_tri2 = tri_indices2[init_tri_id2]; - Vec3f init_tri1_points[3]; - Vec3f init_tri2_points[3]; + Vec3s init_tri1_points[3]; + Vec3s init_tri2_points[3]; init_tri1_points[0] = vertices1[init_tri1[0]]; init_tri1_points[1] = vertices1[init_tri1[1]]; @@ -500,7 +500,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { init_tri2_points[1] = vertices2[init_tri2[1]]; init_tri2_points[2] = vertices2[init_tri2[2]]; - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance( init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), @@ -534,12 +534,12 @@ typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; namespace details { template -inline const Matrix3f& getBVAxes(const BV& bv) { +inline const Matrix3s& getBVAxes(const BV& bv) { return bv.axes; } template <> -inline const Matrix3f& getBVAxes(const OBBRSS& bv) { +inline const Matrix3s& getBVAxes(const OBBRSS& bv) { return bv.obb.axes; } diff --git a/include/coal/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h index ec68e528f..cff2a801b 100644 --- a/include/coal/internal/traversal_node_bvh_shape.h +++ b/include/coal/internal/traversal_node_bvh_shape.h @@ -153,7 +153,7 @@ class MeshShapeCollisionTraversalNode // collision. const bool compute_penetration = this->request.enable_contact || (this->request.security_margin < 0); - Vec3f c1, c2, normal; + Vec3s c1, c2, normal; CoalScalar distance; if (RTIsIdentity) { @@ -186,7 +186,7 @@ class MeshShapeCollisionTraversalNode assert(this->result->isCollision() || sqrDistLowerBound > 0); } // leafCollides - Vec3f* vertices; + Vec3s* vertices; Triangle* tri_indices; const GJKSolver* nsolver; @@ -308,7 +308,7 @@ class MeshShapeDistanceTraversalNode const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]], this->vertices[tri_id[2]]); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &tri, this->tf1, this->model2, this->tf2, this->nsolver, this->request.enable_signed_distance, p1, p2, normal); @@ -325,7 +325,7 @@ class MeshShapeDistanceTraversalNode return false; } - Vec3f* vertices; + Vec3s* vertices; Triangle* tri_indices; CoalScalar rel_err; @@ -340,7 +340,7 @@ namespace details { template void meshShapeDistanceOrientedNodeleafComputeDistance( unsigned int b1, unsigned int /* b2 */, const BVHModel* model1, - const S& model2, Vec3f* vertices, Triangle* tri_indices, + const S& model2, Vec3s* vertices, Triangle* tri_indices, const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver, bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request, DistanceResult& result) { @@ -353,7 +353,7 @@ void meshShapeDistanceOrientedNodeleafComputeDistance( const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], vertices[tri_id[2]]); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); @@ -364,7 +364,7 @@ void meshShapeDistanceOrientedNodeleafComputeDistance( template static inline void distancePreprocessOrientedNode( - const BVHModel* model1, Vec3f* vertices, Triangle* tri_indices, + const BVHModel* model1, Vec3s* vertices, Triangle* tri_indices, int init_tri_id, const S& model2, const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { @@ -372,7 +372,7 @@ static inline void distancePreprocessOrientedNode( const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], vertices[tri_id[2]]); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2, normal); diff --git a/include/coal/internal/traversal_node_bvhs.h b/include/coal/internal/traversal_node_bvhs.h index 99a442f39..a7b2446f4 100644 --- a/include/coal/internal/traversal_node_bvhs.h +++ b/include/coal/internal/traversal_node_bvhs.h @@ -193,12 +193,12 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; - const Vec3f& P1 = vertices1[tri_id1[0]]; - const Vec3f& P2 = vertices1[tri_id1[1]]; - const Vec3f& P3 = vertices1[tri_id1[2]]; - const Vec3f& Q1 = vertices2[tri_id2[0]]; - const Vec3f& Q2 = vertices2[tri_id2[1]]; - const Vec3f& Q3 = vertices2[tri_id2[2]]; + const Vec3s& P1 = vertices1[tri_id1[0]]; + const Vec3s& P2 = vertices1[tri_id1[1]]; + const Vec3s& P3 = vertices1[tri_id1[2]]; + const Vec3s& Q1 = vertices2[tri_id2[0]]; + const Vec3s& Q2 = vertices2[tri_id2[1]]; + const Vec3s& Q3 = vertices2[tri_id2[2]]; TriangleP tri1(P1, P2, P3); TriangleP tri2(Q1, Q2, Q3); @@ -208,7 +208,7 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { const bool compute_penetration = this->request.enable_contact || (this->request.security_margin < 0); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; DistanceResult distanceResult; CoalScalar distance = internal::ShapeShapeDistance( &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1, @@ -231,8 +231,8 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { sqrDistLowerBound = distToCollision * distToCollision; } - Vec3f* vertices1; - Vec3f* vertices2; + Vec3s* vertices1; + Vec3s* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; @@ -255,7 +255,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl { static CoalScalar run(const BVNode& b1, const BVNode& b2) { return b1.distance(b2); } - static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode& b1, + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { return distance(R, T, b1.bv, b2.bv); } @@ -273,7 +273,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static CoalScalar run(const Matrix3f& R, const Vec3f& T, + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); @@ -298,7 +298,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl { } return sqrt(sqrDistLowerBound); } - static CoalScalar run(const Matrix3f& R, const Vec3f& T, + static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode& b1, const BVNode& b2) { CoalScalar sqrDistLowerBound; CollisionRequest request(DISTANCE_LOWER_BOUND, 0); @@ -441,16 +441,16 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; - const Vec3f& t11 = vertices1[tri_id1[0]]; - const Vec3f& t12 = vertices1[tri_id1[1]]; - const Vec3f& t13 = vertices1[tri_id1[2]]; + const Vec3s& t11 = vertices1[tri_id1[0]]; + const Vec3s& t12 = vertices1[tri_id1[1]]; + const Vec3s& t13 = vertices1[tri_id1[2]]; - const Vec3f& t21 = vertices2[tri_id2[0]]; - const Vec3f& t22 = vertices2[tri_id2[1]]; - const Vec3f& t23 = vertices2[tri_id2[2]]; + const Vec3s& t21 = vertices2[tri_id2[0]]; + const Vec3s& t22 = vertices2[tri_id2[1]]; + const Vec3s& t23 = vertices2[tri_id2[2]]; // nearest point pair - Vec3f P1, P2, normal; + Vec3s P1, P2, normal; CoalScalar d2; if (RTIsIdentity) @@ -473,8 +473,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { return false; } - Vec3f* vertices1; - Vec3f* vertices2; + Vec3s* vertices1; + Vec3s* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; @@ -491,8 +491,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { const Triangle& init_tri1 = tri_indices1[init_tri_id1]; const Triangle& init_tri2 = tri_indices2[init_tri_id2]; - Vec3f init_tri1_points[3]; - Vec3f init_tri2_points[3]; + Vec3s init_tri1_points[3]; + Vec3s init_tri2_points[3]; init_tri1_points[0] = vertices1[init_tri1[0]]; init_tri1_points[1] = vertices1[init_tri1[1]]; @@ -502,7 +502,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { init_tri2_points[1] = vertices2[init_tri2[1]]; init_tri2_points[2] = vertices2[init_tri2[2]]; - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance( init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(), @@ -536,12 +536,12 @@ typedef MeshDistanceTraversalNode MeshDistanceTraversalNodeOBBRSS; namespace details { template -inline const Matrix3f& getBVAxes(const BV& bv) { +inline const Matrix3s& getBVAxes(const BV& bv) { return bv.axes; } template <> -inline const Matrix3f& getBVAxes(const OBBRSS& bv) { +inline const Matrix3s& getBVAxes(const OBBRSS& bv) { return bv.obb.axes; } diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h index 1b5f6b1e7..438f5c8cc 100644 --- a/include/coal/internal/traversal_node_hfield_shape.h +++ b/include/coal/internal/traversal_node_hfield_shape.h @@ -59,30 +59,30 @@ namespace details { template Convex buildConvexQuadrilateral(const HFNode& node, const HeightField& model) { - const MatrixXf& heights = model.getHeights(); - const VecXf& x_grid = model.getXGrid(); - const VecXf& y_grid = model.getYGrid(); + const MatrixXs& heights = model.getHeights(); + const VecXs& x_grid = model.getXGrid(); + const VecXs& y_grid = model.getYGrid(); const CoalScalar min_height = model.getMinHeight(); const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; - const Eigen::Block cell = + const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); assert(cell.maxCoeff() > min_height && "max_height is lower than min_height"); // Check whether the geometry // is degenerated - std::shared_ptr> pts(new std::vector({ - Vec3f(x0, y0, min_height), - Vec3f(x0, y1, min_height), - Vec3f(x1, y1, min_height), - Vec3f(x1, y0, min_height), - Vec3f(x0, y0, cell(0, 0)), - Vec3f(x0, y1, cell(1, 0)), - Vec3f(x1, y1, cell(1, 1)), - Vec3f(x1, y0, cell(0, 1)), + std::shared_ptr> pts(new std::vector({ + Vec3s(x0, y0, min_height), + Vec3s(x0, y1, min_height), + Vec3s(x1, y1, min_height), + Vec3s(x1, y0, min_height), + Vec3s(x0, y0, cell(0, 0)), + Vec3s(x0, y1, cell(1, 0)), + Vec3s(x1, y1, cell(1, 1)), + Vec3s(x1, y0, cell(0, 1)), })); std::shared_ptr> polygons( @@ -122,16 +122,16 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, Convex& convex1, int& convex1_active_faces, Convex& convex2, int& convex2_active_faces) { - const MatrixXf& heights = model.getHeights(); - const VecXf& x_grid = model.getXGrid(); - const VecXf& y_grid = model.getYGrid(); + const MatrixXs& heights = model.getHeights(); + const VecXs& x_grid = model.getXGrid(); + const VecXs& y_grid = model.getYGrid(); const CoalScalar min_height = model.getMinHeight(); const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; const CoalScalar max_height = node.max_height; - const Eigen::Block cell = + const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); const int contact_active_faces = node.contact_active_faces; @@ -174,13 +174,13 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, COAL_UNUSED_VARIABLE(max_height); { - std::shared_ptr> pts(new std::vector({ - Vec3f(x0, y0, min_height), // A - Vec3f(x0, y1, min_height), // B - Vec3f(x1, y0, min_height), // C - Vec3f(x0, y0, cell(0, 0)), // D - Vec3f(x0, y1, cell(1, 0)), // E - Vec3f(x1, y0, cell(0, 1)), // F + std::shared_ptr> pts(new std::vector({ + Vec3s(x0, y0, min_height), // A + Vec3s(x0, y1, min_height), // B + Vec3s(x1, y0, min_height), // C + Vec3s(x0, y0, cell(0, 0)), // D + Vec3s(x0, y1, cell(1, 0)), // E + Vec3s(x1, y0, cell(0, 1)), // F })); std::shared_ptr> triangles( @@ -202,13 +202,13 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, } { - std::shared_ptr> pts(new std::vector({ - Vec3f(x0, y1, min_height), // A - Vec3f(x1, y1, min_height), // B - Vec3f(x1, y0, min_height), // C - Vec3f(x0, y1, cell(1, 0)), // D - Vec3f(x1, y1, cell(1, 1)), // E - Vec3f(x1, y0, cell(0, 1)), // F + std::shared_ptr> pts(new std::vector({ + Vec3s(x0, y1, min_height), // A + Vec3s(x1, y1, min_height), // B + Vec3s(x1, y0, min_height), // C + Vec3s(x0, y1, cell(1, 0)), // D + Vec3s(x1, y1, cell(1, 1)), // E + Vec3s(x1, y0, cell(0, 1)), // F })); std::shared_ptr> triangles( @@ -230,23 +230,23 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, } } -inline Vec3f projectTriangle(const Vec3f& pointA, const Vec3f& pointB, - const Vec3f& pointC, const Vec3f& point) { +inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB, + const Vec3s& pointC, const Vec3s& point) { const Project::ProjectResult result = Project::projectTriangle(pointA, pointB, pointC, point); - Vec3f res = result.parameterization[0] * pointA + + Vec3s res = result.parameterization[0] * pointA + result.parameterization[1] * pointB + result.parameterization[2] * pointC; return res; } -inline Vec3f projectTetrahedra(const Vec3f& pointA, const Vec3f& pointB, - const Vec3f& pointC, const Vec3f& pointD, - const Vec3f& point) { +inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB, + const Vec3s& pointC, const Vec3s& pointD, + const Vec3s& point) { const Project::ProjectResult result = Project::projectTetrahedra(pointA, pointB, pointC, pointD, point); - Vec3f res = result.parameterization[0] * pointA + + Vec3s res = result.parameterization[0] * pointA + result.parameterization[1] * pointB + result.parameterization[2] * pointC + result.parameterization[3] * pointD; @@ -254,46 +254,46 @@ inline Vec3f projectTetrahedra(const Vec3f& pointA, const Vec3f& pointB, return res; } -inline Vec3f computeTriangleNormal(const Triangle& triangle, - const std::vector& points) { - const Vec3f pointA = points[triangle[0]]; - const Vec3f pointB = points[triangle[1]]; - const Vec3f pointC = points[triangle[2]]; +inline Vec3s computeTriangleNormal(const Triangle& triangle, + const std::vector& points) { + const Vec3s pointA = points[triangle[0]]; + const Vec3s pointB = points[triangle[1]]; + const Vec3s pointC = points[triangle[2]]; - const Vec3f normal = (pointB - pointA).cross(pointC - pointA).normalized(); + const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized(); assert(!normal.array().isNaN().any() && "normal is ill-defined"); return normal; } -inline Vec3f projectPointOnTriangle(const Vec3f& contact_point, +inline Vec3s projectPointOnTriangle(const Vec3s& contact_point, const Triangle& triangle, - const std::vector& points) { - const Vec3f pointA = points[triangle[0]]; - const Vec3f pointB = points[triangle[1]]; - const Vec3f pointC = points[triangle[2]]; + const std::vector& points) { + const Vec3s pointA = points[triangle[0]]; + const Vec3s pointB = points[triangle[1]]; + const Vec3s pointC = points[triangle[2]]; - const Vec3f contact_point_projected = + const Vec3s contact_point_projected = projectTriangle(pointA, pointB, pointC, contact_point); return contact_point_projected; } inline CoalScalar distanceContactPointToTriangle( - const Vec3f& contact_point, const Triangle& triangle, - const std::vector& points) { - const Vec3f contact_point_projected = + const Vec3s& contact_point, const Triangle& triangle, + const std::vector& points) { + const Vec3s contact_point_projected = projectPointOnTriangle(contact_point, triangle, points); return (contact_point_projected - contact_point).norm(); } inline CoalScalar distanceContactPointToFace(const size_t face_id, - const Vec3f& contact_point, + const Vec3s& contact_point, const Convex& convex, size_t& closest_face_id) { assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]"); - const std::vector& points = *(convex.points); + const std::vector& points = *(convex.points); if (face_id <= 1) { const Triangle& triangle = (*(convex.polygons))[face_id]; closest_face_id = face_id; @@ -321,10 +321,10 @@ template bool binCorrection(const Convex& convex, const int convex_active_faces, const Shape& shape, const Transform3f& shape_pose, CoalScalar& distance, - Vec3f& contact_1, Vec3f& contact_2, Vec3f& normal, - Vec3f& face_normal, const bool is_collision) { + Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal, + Vec3s& face_normal, const bool is_collision) { const CoalScalar prec = 1e-12; - const std::vector& points = *(convex.points); + const std::vector& points = *(convex.points); bool hfield_witness_is_on_bin_side = true; @@ -366,16 +366,16 @@ bool binCorrection(const Convex& convex, if (!face_triangle.isValid()) COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error); - const Vec3f face_pointA = points[face_triangle[0]]; + const Vec3s face_pointA = points[face_triangle[0]]; face_normal = computeTriangleNormal(face_triangle, points); int hint = 0; // Since we compute the support manually, we need to take into account the // sphere swept radius of the shape. // TODO: take into account the swept-sphere radius of the bin. - const Vec3f _support = getSupport( + const Vec3s _support = getSupport( &shape, -shape_pose.rotation().transpose() * face_normal, hint); - const Vec3f support = + const Vec3s support = shape_pose.rotation() * _support + shape_pose.translation(); // Project support into the inclined bin having triangle @@ -384,7 +384,7 @@ bool binCorrection(const Convex& convex, const CoalScalar distance_support_projection_plane = projection_plane.signedDistance(support); - const Vec3f projected_support = + const Vec3s projected_support = support - distance_support_projection_plane * face_normal; // We need now to project the projected in the triangle shape @@ -405,8 +405,8 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, const Convex& convex2, const int convex2_active_faces, const Transform3f& tf1, const Shape& shape, const Transform3f& tf2, - CoalScalar& distance, Vec3f& c1, Vec3f& c2, Vec3f& normal, - Vec3f& normal_top, bool& hfield_witness_is_on_bin_side) { + CoalScalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal, + Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) { enum { RTIsIdentity = Options & RelativeTransformationIsIdentity }; const Transform3f Id; @@ -416,8 +416,8 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, // The only thing we need to make sure is that in case of collision, the // penetration information is computed (as we do bins comparison). const bool compute_penetration = true; - Vec3f contact1_1, contact1_2, contact2_1, contact2_2; - Vec3f normal1, normal1_top, normal2, normal2_top; + Vec3s contact1_1, contact1_2, contact2_1, contact2_2; + Vec3s normal1, normal1_top, normal2, normal2_top; CoalScalar distance1, distance2; if (RTIsIdentity) { @@ -608,8 +608,8 @@ class HeightFieldShapeCollisionTraversalNode } CoalScalar distance; - // Vec3f contact_point, normal; - Vec3f c1, c2, normal, normal_face; + // Vec3s contact_point, normal; + Vec3s c1, c2, normal, normal_face; bool hfield_witness_is_on_bin_side; bool collision = details::shapeDistance( @@ -632,8 +632,8 @@ class HeightFieldShapeCollisionTraversalNode } else sqrDistLowerBound = distToCollision * distToCollision; - // const Vec3f c1 = contact_point - distance * 0.5 * normal; - // const Vec3f c2 = contact_point + distance * 0.5 * normal; + // const Vec3s c1 = contact_point - distance * 0.5 * normal; + // const Vec3s c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, distToCollision, c1, c2, normal); @@ -715,7 +715,7 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { const ConvexQuadrilateral convex = details::buildConvexQuadrilateral(node, *this->model1); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &convex, this->tf1, this->model2, this->tf2, this->nsolver, diff --git a/include/coal/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h index 29cefe594..f9b49722e 100644 --- a/include/coal/internal/traversal_node_octree.h +++ b/include/coal/internal/traversal_node_octree.h @@ -259,7 +259,7 @@ class COAL_DLLAPI OcTreeSolver { box.computeLocalAABB(); } - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &box, box_tf, &s, tf2, this->solver, this->drequest->enable_signed_distance, p1, p2, normal); @@ -390,7 +390,7 @@ class COAL_DLLAPI OcTreeSolver { (*(tree2->vertices))[tri_id[1]], (*(tree2->vertices))[tri_id[2]]); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &box, box_tf, &tri, tf2, this->solver, @@ -515,7 +515,7 @@ class COAL_DLLAPI OcTreeSolver { // collision. const bool compute_penetration = this->crequest->enable_contact || (this->crequest->security_margin < 0); - Vec3f c1, c2, normal; + Vec3s c1, c2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2, normal); @@ -618,7 +618,7 @@ class COAL_DLLAPI OcTreeSolver { convex2.computeLocalAABB(); } - Vec3f c1, c2, normal, normal_top; + Vec3s c1, c2, normal, normal_top; CoalScalar distance; bool hfield_witness_is_on_bin_side; @@ -643,8 +643,8 @@ class COAL_DLLAPI OcTreeSolver { } else sqrDistLowerBound = distToCollision * distToCollision; - // const Vec3f c1 = contact_point - distance * 0.5 * normal; - // const Vec3f c2 = contact_point + distance * 0.5 * normal; + // const Vec3s c1 = contact_point - distance * 0.5 * normal; + // const Vec3s c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf( *crequest, *cresult, distToCollision, c1, c2, -normal); @@ -738,7 +738,7 @@ class COAL_DLLAPI OcTreeSolver { convex2.computeLocalAABB(); } - Vec3f c1, c2, normal, normal_top; + Vec3s c1, c2, normal, normal_top; CoalScalar distance; bool hfield_witness_is_on_bin_side; @@ -763,8 +763,8 @@ class COAL_DLLAPI OcTreeSolver { } else sqrDistLowerBound = distToCollision * distToCollision; - // const Vec3f c1 = contact_point - distance * 0.5 * normal; - // const Vec3f c2 = contact_point + distance * 0.5 * normal; + // const Vec3s c1 = contact_point - distance * 0.5 * normal; + // const Vec3s c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf( *crequest, *cresult, distToCollision, c1, c2, normal); @@ -820,7 +820,7 @@ class COAL_DLLAPI OcTreeSolver { box2.computeLocalAABB(); } - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; const CoalScalar distance = internal::ShapeShapeDistance( &box1, box1_tf, &box2, box2_tf, this->solver, this->drequest->enable_signed_distance, p1, p2, normal); @@ -948,7 +948,7 @@ class COAL_DLLAPI OcTreeSolver { // collision. const bool compute_penetration = (this->crequest->enable_contact || (this->crequest->security_margin < 0)); - Vec3f c1, c2, normal; + Vec3s c1, c2, normal; CoalScalar distance = internal::ShapeShapeDistance( &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1, c2, normal); diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h index 6a84580ad..6f60609e8 100644 --- a/include/coal/internal/traversal_node_setup.h +++ b/include/coal/internal/traversal_node_setup.h @@ -340,11 +340,11 @@ bool initialize(MeshShapeCollisionTraversalNode& node, if (!tf1.isIdentity() && model1.vertices.get()) // TODO(jcarpent): vectorized version { - std::vector vertices_transformed(model1.num_vertices); - const std::vector& model1_vertices_ = *(model1.vertices); + std::vector vertices_transformed(model1.num_vertices); + const std::vector& model1_vertices_ = *(model1.vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); vertices_transformed[i] = new_v; } @@ -479,11 +479,11 @@ bool initialize( std::invalid_argument) if (!tf1.isIdentity() && model1.vertices.get()) { - std::vector vertices_transformed1(model1.num_vertices); - const std::vector& model1_vertices_ = *(model1.vertices); + std::vector vertices_transformed1(model1.num_vertices); + const std::vector& model1_vertices_ = *(model1.vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } @@ -495,11 +495,11 @@ bool initialize( } if (!tf2.isIdentity() && model2.vertices.get()) { - std::vector vertices_transformed2(model2.num_vertices); - const std::vector& model2_vertices_ = *(model2.vertices); + std::vector vertices_transformed2(model2.num_vertices); + const std::vector& model2_vertices_ = *(model2.vertices); for (unsigned int i = 0; i < model2.num_vertices; ++i) { - const Vec3f& p = model2_vertices_[i]; - Vec3f new_v = tf2.transform(p); + const Vec3s& p = model2_vertices_[i]; + Vec3s new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } @@ -600,11 +600,11 @@ bool initialize( std::invalid_argument) if (!tf1.isIdentity() && model1.vertices.get()) { - std::vector vertices_transformed1(model1.num_vertices); - const std::vector& model1_vertices_ = *(model1.vertices); + std::vector vertices_transformed1(model1.num_vertices); + const std::vector& model1_vertices_ = *(model1.vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } @@ -616,11 +616,11 @@ bool initialize( } if (!tf2.isIdentity() && model2.vertices.get()) { - std::vector vertices_transformed2(model2.num_vertices); - const std::vector& model2_vertices_ = *(model2.vertices); + std::vector vertices_transformed2(model2.num_vertices); + const std::vector& model2_vertices_ = *(model2.vertices); for (unsigned int i = 0; i < model2.num_vertices; ++i) { - const Vec3f& p = model2_vertices_[i]; - Vec3f new_v = tf2.transform(p); + const Vec3s& p = model2_vertices_[i]; + Vec3s new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } @@ -706,11 +706,11 @@ bool initialize(MeshShapeDistanceTraversalNode& node, std::invalid_argument) if (!tf1.isIdentity() && model1.vertices.get()) { - const std::vector& model1_vertices_ = *(model1.vertices); - std::vector vertices_transformed1(model1.num_vertices); + const std::vector& model1_vertices_ = *(model1.vertices); + std::vector vertices_transformed1(model1.num_vertices); for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1_vertices_[i]; - Vec3f new_v = tf1.transform(p); + const Vec3s& p = model1_vertices_[i]; + Vec3s new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h index 3bfcd0c37..ff96bac1f 100644 --- a/include/coal/math/transform.h +++ b/include/coal/math/transform.h @@ -54,10 +54,10 @@ static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) { /// @brief Simple transform class used locally by InterpMotion class COAL_DLLAPI Transform3f { /// @brief Matrix cache - Matrix3f R; + Matrix3s R; /// @brief Translation vector - Vec3f T; + Vec3s T; public: /// @brief Default transform is no movement @@ -79,13 +79,13 @@ class COAL_DLLAPI Transform3f { : R(q_.toRotationMatrix()), T(T_) {} /// @brief Construct transform from rotation - Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {} + Transform3f(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {} /// @brief Construct transform from rotation - Transform3f(const Quatf& q_) : R(q_), T(Vec3f::Zero()) {} + Transform3f(const Quatf& q_) : R(q_), T(Vec3s::Zero()) {} /// @brief Construct transform from translation - Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {} + Transform3f(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {} /// @brief Construct transform from other transform Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {} @@ -98,22 +98,22 @@ class COAL_DLLAPI Transform3f { } /// @brief get translation - inline const Vec3f& getTranslation() const { return T; } + inline const Vec3s& getTranslation() const { return T; } /// @brief get translation - inline const Vec3f& translation() const { return T; } + inline const Vec3s& translation() const { return T; } /// @brief get translation - inline Vec3f& translation() { return T; } + inline Vec3s& translation() { return T; } /// @brief get rotation - inline const Matrix3f& getRotation() const { return R; } + inline const Matrix3s& getRotation() const { return R; } /// @brief get rotation - inline const Matrix3f& rotation() const { return R; } + inline const Matrix3s& rotation() const { return R; } /// @brief get rotation - inline Matrix3f& rotation() { return R; } + inline Matrix3s& rotation() { return R; } /// @brief get quaternion inline Quatf getQuatRotation() const { return Quatf(R); } @@ -127,7 +127,7 @@ class COAL_DLLAPI Transform3f { } /// @brief set transform from rotation and translation - inline void setTransform(const Quatf& q_, const Vec3f& T_) { + inline void setTransform(const Quatf& q_, const Vec3s& T_) { R = q_.toRotationMatrix(); T = T_; } @@ -149,13 +149,13 @@ class COAL_DLLAPI Transform3f { /// @brief transform a given vector by the transform template - inline Vec3f transform(const Eigen::MatrixBase& v) const { + inline Vec3s transform(const Eigen::MatrixBase& v) const { return R * v + T; } /// @brief transform a given vector by the inverse of the transform template - inline Vec3f inverseTransform(const Eigen::MatrixBase& v) const { + inline Vec3s inverseTransform(const Eigen::MatrixBase& v) const { return R.transpose() * (v - T); } @@ -257,8 +257,8 @@ inline Transform3f& Transform3f::setRandom() { /// @brief Construct othonormal basis from vector. /// The z-axis is the normalized input vector. -inline Matrix3f constructOrthonormalBasisFromVector(const Vec3f& vec) { - Matrix3f basis = Matrix3f::Zero(); +inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) { + Matrix3s basis = Matrix3s::Zero(); basis.col(2) = vec.normalized(); basis.col(1) = -vec.unitOrthogonal(); basis.col(0) = basis.col(1).cross(vec); diff --git a/include/coal/mesh_loader/assimp.h b/include/coal/mesh_loader/assimp.h index 2be9cfc9c..0133f4569 100644 --- a/include/coal/mesh_loader/assimp.h +++ b/include/coal/mesh_loader/assimp.h @@ -53,7 +53,7 @@ namespace coal { namespace internal { struct COAL_DLLAPI TriangleAndVertices { - std::vector vertices_; + std::vector vertices_; std::vector triangles_; }; @@ -75,7 +75,7 @@ struct COAL_DLLAPI Loader { * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -COAL_DLLAPI void buildMesh(const coal::Vec3f& scale, const aiScene* scene, +COAL_DLLAPI void buildMesh(const coal::Vec3s& scale, const aiScene* scene, unsigned vertices_offset, TriangleAndVertices& tv); /** @@ -87,7 +87,7 @@ COAL_DLLAPI void buildMesh(const coal::Vec3f& scale, const aiScene* scene, */ template inline void meshFromAssimpScene( - const coal::Vec3f& scale, const aiScene* scene, + const coal::Vec3s& scale, const aiScene* scene, const shared_ptr >& mesh) { TriangleAndVertices tv; @@ -114,7 +114,7 @@ inline void meshFromAssimpScene( */ template inline void loadPolyhedronFromResource( - const std::string& resource_path, const coal::Vec3f& scale, + const std::string& resource_path, const coal::Vec3s& scale, const shared_ptr >& polyhedron) { internal::Loader scene; scene.load(resource_path); diff --git a/include/coal/mesh_loader/loader.h b/include/coal/mesh_loader/loader.h index c24f48d00..18b2255b8 100644 --- a/include/coal/mesh_loader/loader.h +++ b/include/coal/mesh_loader/loader.h @@ -54,7 +54,7 @@ class COAL_DLLAPI MeshLoader { virtual ~MeshLoader() {} virtual BVHModelPtr_t load(const std::string& filename, - const Vec3f& scale = Vec3f::Ones()); + const Vec3s& scale = Vec3s::Ones()); /// Create an OcTree from a file in binary octomap format. /// \todo add OctreePtr_t @@ -76,13 +76,13 @@ class COAL_DLLAPI CachedMeshLoader : public MeshLoader { CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {} - virtual BVHModelPtr_t load(const std::string& filename, const Vec3f& scale); + virtual BVHModelPtr_t load(const std::string& filename, const Vec3s& scale); struct COAL_DLLAPI Key { std::string filename; - Vec3f scale; + Vec3s scale; - Key(const std::string& f, const Vec3f& s) : filename(f), scale(s) {} + Key(const std::string& f, const Vec3s& s) : filename(f), scale(s) {} bool operator<(const CachedMeshLoader::Key& b) const; }; diff --git a/include/coal/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h index ae92c05e7..ea8ed8f03 100644 --- a/include/coal/narrowphase/gjk.h +++ b/include/coal/narrowphase/gjk.h @@ -53,10 +53,10 @@ namespace details { struct COAL_DLLAPI GJK { struct COAL_DLLAPI SimplexV { /// @brief support vector for shape 0 and 1. - Vec3f w0, w1; + Vec3s w0, w1; /// @brief support vector (i.e., the furthest point on the shape along the /// support direction) - Vec3f w; + Vec3s w; }; typedef unsigned char vertex_id_t; @@ -108,7 +108,7 @@ struct COAL_DLLAPI GJK { GJKConvergenceCriterionType convergence_criterion_type; MinkowskiDiff const* shape; - Vec3f ray; + Vec3s ray; support_func_guess_t support_hint; /// @brief The distance between the two shapes, computed by GJK. /// If the distance is below GJK's threshold, the shapes are in collision in @@ -154,12 +154,12 @@ struct COAL_DLLAPI GJK { /// @brief GJK algorithm, given the initial value guess Status evaluate( - const MinkowskiDiff& shape, const Vec3f& guess, + const MinkowskiDiff& shape, const Vec3s& guess, const support_func_guess_t& supportHint = support_func_guess_t::Zero()); /// @brief apply the support function along a direction, the result is return /// in sv - inline void getSupport(const Vec3f& d, SimplexV& sv, + inline void getSupport(const Vec3s& d, SimplexV& sv, support_func_guess_t& hint) const { shape->support(d, sv.w0, sv.w1, hint); sv.w = sv.w0 - sv.w1; @@ -181,11 +181,11 @@ struct COAL_DLLAPI GJK { /// @param[out] w1 is the witness point on shape1. /// @param[out] normal is the normal of the separating plane found by /// GJK. It points from shape0 to shape1. - void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const; + void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const; /// @brief get the guess from current simplex - Vec3f getGuessFromSimplex() const; + Vec3s getGuessFromSimplex() const; /// @brief Distance threshold for early break. /// GJK stops when it proved the distance is more than this threshold. @@ -197,7 +197,7 @@ struct COAL_DLLAPI GJK { /// @brief Convergence check used to stop GJK when shapes are not in /// collision. - bool checkConvergence(const Vec3f& w, const CoalScalar& rl, CoalScalar& alpha, + bool checkConvergence(const Vec3s& w, const CoalScalar& rl, CoalScalar& alpha, const CoalScalar& omega) const; /// @brief Get the max number of iterations of GJK. @@ -225,7 +225,7 @@ struct COAL_DLLAPI GJK { inline void removeVertex(Simplex& simplex); /// @brief append one vertex to the simplex - inline void appendVertex(Simplex& simplex, const Vec3f& v, + inline void appendVertex(Simplex& simplex, const Vec3s& v, support_func_guess_t& hint); /// @brief Project origin (0) onto line a-b @@ -258,7 +258,7 @@ struct COAL_DLLAPI GJK { struct COAL_DLLAPI EPA { typedef GJK::SimplexV SimplexVertex; struct COAL_DLLAPI SimplexFace { - Vec3f n; + Vec3s n; CoalScalar d; bool ignore; // If the origin does not project inside the face, we // ignore this face. @@ -272,7 +272,7 @@ struct COAL_DLLAPI EPA { // (with 0 <= i <= 2). size_t pass; - SimplexFace() : n(Vec3f::Zero()), ignore(false) {}; + SimplexFace() : n(Vec3s::Zero()), ignore(false) {}; }; /// @brief The simplex list of EPA is a linked list of faces. @@ -342,7 +342,7 @@ struct COAL_DLLAPI EPA { public: Status status; GJK::Simplex result; - Vec3f normal; + Vec3s normal; support_func_guess_t support_hint; CoalScalar depth; SimplexFace* closest_face; @@ -409,7 +409,7 @@ struct COAL_DLLAPI EPA { /// \return a Status which can be demangled using (status & Valid) or /// (status & Failed). The other values provide a more detailled /// status - Status evaluate(GJK& gjk, const Vec3f& guess); + Status evaluate(GJK& gjk, const Vec3s& guess); /// Get the witness points on each object, and the corresponding normal. /// @param[in] shape is the Minkowski difference of the two shapes. @@ -418,8 +418,8 @@ struct COAL_DLLAPI EPA { /// @param[in] normal is the normal found by EPA. It points from shape0 to /// shape1. The normal is used to correct the witness points on the shapes if /// the shapes have a non-zero swept-sphere radius. - void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const; + void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const; private: /// @brief Allocates memory for the EPA algorithm. diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h index c0af94243..3449301ab 100644 --- a/include/coal/narrowphase/minkowski_difference.h +++ b/include/coal/narrowphase/minkowski_difference.h @@ -62,11 +62,11 @@ struct COAL_DLLAPI MinkowskiDiff { /// @brief rotation from shape1 to shape0 /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. - Matrix3f oR1; + Matrix3s oR1; /// @brief translation from shape1 to shape0 /// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$. - Vec3f ot1; + Vec3s ot1; /// @brief The radii of the sphere swepted around each shape of the Minkowski /// difference. The 2 values correspond to the swept-sphere radius of shape 0 @@ -79,8 +79,8 @@ struct COAL_DLLAPI MinkowskiDiff { bool normalize_support_direction; typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff, - const Vec3f& dir, Vec3f& support0, - Vec3f& support1, + const Vec3s& dir, Vec3s& support0, + Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]); GetSupportFunction getSupportFunc; @@ -139,7 +139,7 @@ struct COAL_DLLAPI MinkowskiDiff { /// @tparam `SupportOptions` see `set(const ShapeBase*, const /// ShapeBase*)` for more details. template - inline Vec3f support0(const Vec3f& dir, int& hint) const { + inline Vec3s support0(const Vec3s& dir, int& hint) const { return getSupport<_SupportOptions>(shapes[0], dir, hint); } @@ -156,7 +156,7 @@ struct COAL_DLLAPI MinkowskiDiff { /// @tparam `SupportOptions` see `set(const ShapeBase*, const /// ShapeBase*)` for more details. template - inline Vec3f support1(const Vec3f& dir, int& hint) const { + inline Vec3s support1(const Vec3s& dir, int& hint) const { // clang-format off return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1; // clang-format on @@ -171,7 +171,7 @@ struct COAL_DLLAPI MinkowskiDiff { /// frame of shape0. /// @param[in/out] hint used to initialize the search when shape is a /// ConvexBase object. - inline void support(const Vec3f& dir, Vec3f& supp0, Vec3f& supp1, + inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1, support_func_guess_t& hint) const { assert(getSupportFunc != NULL); getSupportFunc(*this, dir, supp0, supp1, hint, diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h index 261917ccf..03046b45e 100644 --- a/include/coal/narrowphase/narrowphase.h +++ b/include/coal/narrowphase/narrowphase.h @@ -76,7 +76,7 @@ struct COAL_DLLAPI GJKSolver { bool enable_cached_guess; /// @brief smart guess - mutable Vec3f cached_guess; + mutable Vec3s cached_guess; /// @brief smart guess for the support function mutable support_func_guess_t support_func_cached_guess; @@ -126,7 +126,7 @@ struct COAL_DLLAPI GJKSolver { gjk_tolerance(GJK_DEFAULT_TOLERANCE), gjk_initial_guess(GJKInitialGuess::DefaultGuess), enable_cached_guess(false), // Use gjk_initial_guess instead - cached_guess(Vec3f(1, 0, 0)), + cached_guess(Vec3s(1, 0, 0)), support_func_cached_guess(support_func_guess_t::Zero()), distance_upper_bound((std::numeric_limits::max)()), gjk_variant(GJKVariant::DefaultGJK), @@ -148,7 +148,7 @@ struct COAL_DLLAPI GJKSolver { explicit GJKSolver(const DistanceRequest& request) : gjk(request.gjk_max_iterations, request.gjk_tolerance), epa(0, request.epa_tolerance) { - this->cached_guess = Vec3f(1, 0, 0); + this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess = support_func_guess_t::Zero(); set(request); @@ -200,7 +200,7 @@ struct COAL_DLLAPI GJKSolver { explicit GJKSolver(const CollisionRequest& request) : gjk(request.gjk_max_iterations, request.gjk_tolerance), epa(0, request.epa_tolerance) { - this->cached_guess = Vec3f(1, 0, 0); + this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess = support_func_guess_t::Zero(); set(request); @@ -307,8 +307,8 @@ struct COAL_DLLAPI GJKSolver { template CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - const bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { + const bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { constexpr bool relative_transformation_already_computed = false; CoalScalar distance; this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, @@ -322,8 +322,8 @@ struct COAL_DLLAPI GJKSolver { template CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1, const TriangleP& s2, const Transform3f& tf2, - const bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { + const bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { const Transform3f tf_1M2(tf1.inverseTimes(tf2)); TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b), tf_1M2.transform(s2.c)); @@ -339,8 +339,8 @@ struct COAL_DLLAPI GJKSolver { template CoalScalar shapeDistance(const TriangleP& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - const bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { + const bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { CoalScalar distance = this->shapeDistance( s2, tf2, s1, tf1, compute_penetration, p2, p1, normal); normal = -normal; @@ -351,9 +351,9 @@ struct COAL_DLLAPI GJKSolver { /// @brief initialize GJK. /// This method assumes `minkowski_difference` has been set. template - void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3f& guess, + void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess, support_func_guess_t& support_hint, - const Vec3f& default_guess = Vec3f(1, 0, 0)) const { + const Vec3s& default_guess = Vec3s(1, 0, 0)) const { // There is no reason not to warm-start the support function, so we always // do it. support_hint = this->support_func_cached_guess; @@ -422,7 +422,7 @@ struct COAL_DLLAPI GJKSolver { void runGJKAndEPA( const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, const bool compute_penetration, - CoalScalar& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal, + CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal, const bool relative_transformation_already_computed = false) const { // Reset internal state of GJK algorithm if (relative_transformation_already_computed) @@ -437,7 +437,7 @@ struct COAL_DLLAPI GJKSolver { this->epa.status = details::EPA::Status::DidNotRun; // Get initial guess for GJK: default, cached or bounding volume guess - Vec3f guess; + Vec3s guess; support_func_guess_t support_hint; getGJKInitialGuess(*(this->minkowski_difference.shapes[0]), *(this->minkowski_difference.shapes[1]), guess, @@ -449,11 +449,11 @@ struct COAL_DLLAPI GJKSolver { case details::GJK::DidNotRun: COAL_ASSERT(false, "GJK did not run. It should have!", std::logic_error); - this->cached_guess = Vec3f(1, 0, 0); + this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess.setZero(); distance = -(std::numeric_limits::max)(); p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); break; case details::GJK::Failed: // @@ -588,8 +588,8 @@ struct COAL_DLLAPI GJKSolver { void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3f& tf1, CoalScalar& distance, - Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { + Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { COAL_UNUSED_VARIABLE(tf1); // Cache gjk result for potential future call to this GJKSolver. this->cached_guess = this->gjk.ray; @@ -597,7 +597,7 @@ struct COAL_DLLAPI GJKSolver { distance = this->gjk.distance; p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); // If we absolutely want to return some witness points, we could use // the following code (or simply merge the early stopped case with the // valid case below): @@ -608,8 +608,8 @@ struct COAL_DLLAPI GJKSolver { } void GJKExtractWitnessPointsAndNormal(const Transform3f& tf1, - CoalScalar& distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) const { + CoalScalar& distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) const { // Apart from early stopping, there are two cases where GJK says there is no // collision: // 1. GJK proved the distance is above its tolerance (default 1e-6). @@ -628,7 +628,7 @@ struct COAL_DLLAPI GJKSolver { // TODO: On degenerated case, the closest points may be non-unique. // (i.e. an object face normal is colinear to `gjk.ray`) gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal); - Vec3f p = tf1.transform(0.5 * (p1 + p2)); + Vec3s p = tf1.transform(0.5 * (p1 + p2)); normal = tf1.getRotation() * normal; p1.noalias() = p - 0.5 * distance * normal; p2.noalias() = p + 0.5 * distance * normal; @@ -636,8 +636,8 @@ struct COAL_DLLAPI GJKSolver { void GJKCollisionExtractWitnessPointsAndNormal(const Transform3f& tf1, CoalScalar& distance, - Vec3f& p1, Vec3f& p2, - Vec3f& normal) const { + Vec3s& p1, Vec3s& p2, + Vec3s& normal) const { COAL_UNUSED_VARIABLE(tf1); COAL_ASSERT(this->gjk.distance <= this->gjk.getTolerance() + this->m_dummy_precision, @@ -651,12 +651,12 @@ struct COAL_DLLAPI GJKSolver { distance = this->gjk.distance; p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); } void EPAExtractWitnessPointsAndNormal(const Transform3f& tf1, - CoalScalar& distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) const { + CoalScalar& distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) const { // Cache EPA result for potential future call to this GJKSolver. // This caching allows to warm-start the next GJK call. this->cached_guess = -(this->epa.depth * this->epa.normal); @@ -703,22 +703,22 @@ struct COAL_DLLAPI GJKSolver { // We compute the middle points of the current $p_1$ and $p_2$ and we use // the normal and the distance given by EPA to compute the new $p_1$ and // $p_2$. - Vec3f p = tf1.transform(0.5 * (p1 + p2)); + Vec3s p = tf1.transform(0.5 * (p1 + p2)); normal = tf1.getRotation() * normal; p1.noalias() = p - 0.5 * distance * normal; p2.noalias() = p + 0.5 * distance * normal; } void EPAFailedExtractWitnessPointsAndNormal(const Transform3f& tf1, - CoalScalar& distance, Vec3f& p1, - Vec3f& p2, Vec3f& normal) const { - this->cached_guess = Vec3f(1, 0, 0); + CoalScalar& distance, Vec3s& p1, + Vec3s& p2, Vec3s& normal) const { + this->cached_guess = Vec3s(1, 0, 0); this->support_func_cached_guess.setZero(); COAL_UNUSED_VARIABLE(tf1); distance = -(std::numeric_limits::max)(); p1 = p2 = normal = - Vec3f::Constant(std::numeric_limits::quiet_NaN()); + Vec3s::Constant(std::numeric_limits::quiet_NaN()); } }; diff --git a/include/coal/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h index 8dac64d41..df79a1353 100644 --- a/include/coal/narrowphase/support_functions.h +++ b/include/coal/narrowphase/support_functions.h @@ -74,7 +74,7 @@ enum SupportOptions { /// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const /// ShapeBase*)` for more details. template -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint); +Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint); /// @brief Stores temporary data for the computation of support points. struct COAL_DLLAPI ShapeSupportData { @@ -83,7 +83,7 @@ struct COAL_DLLAPI ShapeSupportData { // @brief Tracks the last support direction used on this shape; used to // warm-start the ConvexBase support function. - Vec3f last_dir = Vec3f::Zero(); + Vec3s last_dir = Vec3s::Zero(); // @brief Temporary set used to compute the convex-hull of a support set. // Only used for ConvexBase and Box. @@ -92,46 +92,46 @@ struct COAL_DLLAPI ShapeSupportData { /// @brief Triangle support function. template -void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, - Vec3f& support, int& /*unused*/, +void getShapeSupport(const TriangleP* triangle, const Vec3s& dir, + Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Box support function. template -void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Sphere support function. template -void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Ellipsoid support function. template -void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - Vec3f& support, int& /*unused*/, +void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir, + Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Capsule support function. template -void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Cone support function. template -void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief Cylinder support function. template -void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/); /// @brief ConvexBase support function. /// @note See @ref LargeConvex and SmallConvex to see how to optimize /// ConvexBase's support computation. template -void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& /*unused*/); /// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of @@ -145,13 +145,13 @@ struct SmallConvex : ShapeBase {}; /// @brief Support function for large ConvexBase (>32 vertices). template -void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, ShapeSupportData& data); +void getShapeSupport(const SmallConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& data); /// @brief Support function for small ConvexBase (<32 vertices). template -void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, ShapeSupportData& support_data); +void getShapeSupport(const LargeConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& support_data); // ============================================================================ // ========================== SUPPORT SET FUNCTIONS =========================== @@ -203,12 +203,12 @@ void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, /// @note This function automatically deals with the `direction` of the /// SupportSet. template -void getSupportSet(const ShapeBase* shape, const Vec3f& dir, +void getSupportSet(const ShapeBase* shape, const Vec3s& dir, SupportSet& support_set, int& hint, size_t num_sampled_supports = 6, CoalScalar tol = 1e-3) { support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir); - const Vec3f& support_dir = support_set.getNormal(); - const Vec3f support = getSupport<_SupportOptions>(shape, support_dir, hint); + const Vec3s& support_dir = support_set.getNormal(); + const Vec3s support = getSupport<_SupportOptions>(shape, support_dir, hint); getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports, tol); } diff --git a/include/coal/octree.h b/include/coal/octree.h index 7dee68df0..52c507953 100644 --- a/include/coal/octree.h +++ b/include/coal/octree.h @@ -103,8 +103,8 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { /// @brief compute the AABB for the octree in its local coordinate system void computeLocalAABB() { - typedef Eigen::Matrix Vec3float; - Vec3float max_extent, min_extent; + typedef Eigen::Matrix Vec3sloat; + Vec3sloat max_extent, min_extent; octomap::OcTree::iterator it = tree->begin((unsigned char)tree->getTreeDepth()); @@ -115,10 +115,10 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { { const octomap::point3d& coord = it.getCoordinate(); // getCoordinate returns a copy - max_extent = min_extent = Eigen::Map(&coord.x()); + max_extent = min_extent = Eigen::Map(&coord.x()); for (++it; it != end; ++it) { const octomap::point3d& coord = it.getCoordinate(); - const Vec3float pos = Eigen::Map(&coord.x()); + const Vec3sloat pos = Eigen::Map(&coord.x()); max_extent = max_extent.array().max(pos.array()); min_extent = min_extent.array().min(pos.array()); } @@ -140,7 +140,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { CoalScalar delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; // std::cout << "octree size " << delta << std::endl; - return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)); + return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta)); } /// @brief Returns the depth of the octree @@ -175,8 +175,8 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { /// @brief transform the octree into a bunch of boxes; uncertainty information /// is kept in the boxes. However, we only keep the occupied boxes (i.e., the /// boxes whose occupied probability is higher enough). - std::vector toBoxes() const { - std::vector boxes; + std::vector toBoxes() const { + std::vector boxes; boxes.reserve(tree->size() / 2); for (octomap::OcTree::iterator it = tree->begin((unsigned char)tree->getTreeDepth()), @@ -191,7 +191,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { CoalScalar c = (*it).getOccupancy(); CoalScalar t = tree->getOccupancyThres(); - Vec6f box; + Vec6s box; box << x, y, z, size, c, t; boxes.push_back(box); } @@ -201,7 +201,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { /// \brief Returns a byte description of *this std::vector tobytes() const { - typedef Eigen::Matrix Vec3float; + typedef Eigen::Matrix Vec3sloat; const size_t total_size = (tree->size() * sizeof(CoalScalar) * 3) / 2; std::vector bytes; bytes.reserve(total_size); @@ -210,8 +210,8 @@ class COAL_DLLAPI OcTree : public CollisionGeometry { it = tree->begin((unsigned char)tree->getTreeDepth()), end = tree->end(); it != end; ++it) { - const Vec3f box_pos = - Eigen::Map(&it.getCoordinate().x()).cast(); + const Vec3s box_pos = + Eigen::Map(&it.getCoordinate().x()).cast(); if (isNodeOccupied(&*it)) std::copy(box_pos.data(), box_pos.data() + sizeof(CoalScalar) * 3, std::back_inserter(bytes)); diff --git a/include/coal/serialization/collision_data.h b/include/coal/serialization/collision_data.h index 2cc10f449..0c1466dc2 100644 --- a/include/coal/serialization/collision_data.h +++ b/include/coal/serialization/collision_data.h @@ -27,7 +27,7 @@ void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) { ar >> make_nvp("b1", contact.b1); ar >> make_nvp("b2", contact.b2); ar >> make_nvp("normal", contact.normal); - std::array nearest_points; + std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); contact.nearest_points[0] = nearest_points[0]; contact.nearest_points[1] = nearest_points[1]; @@ -113,7 +113,7 @@ void load(Archive& ar, coal::CollisionResult& collision_result, for (size_t k = 0; k < contacts.size(); ++k) collision_result.addContact(contacts[k]); ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound); - std::array nearest_points; + std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); collision_result.nearest_points[0] = nearest_points[0]; collision_result.nearest_points[1] = nearest_points[1]; @@ -155,7 +155,7 @@ void load(Archive& ar, coal::DistanceResult& distance_result, ar >> make_nvp("base", boost::serialization::base_object( distance_result)); ar >> make_nvp("min_distance", distance_result.min_distance); - std::array nearest_points; + std::array nearest_points; ar >> make_nvp("nearest_points", nearest_points); distance_result.nearest_points[0] = nearest_points[0]; distance_result.nearest_points[1] = nearest_points[1]; diff --git a/include/coal/serialization/convex.h b/include/coal/serialization/convex.h index 52999cee0..5c89a918d 100644 --- a/include/coal/serialization/convex.h +++ b/include/coal/serialization/convex.h @@ -50,7 +50,7 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base, convex_base.points.reset(); if (convex_base.num_points > 0) convex_base.points.reset( - new std::vector(convex_base.num_points)); + new std::vector(convex_base.num_points)); } if (num_normals_and_offsets_previous != @@ -59,7 +59,7 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base, convex_base.offsets.reset(); if (convex_base.num_normals_and_offsets > 0) { convex_base.normals.reset( - new std::vector(convex_base.num_normals_and_offsets)); + new std::vector(convex_base.num_normals_and_offsets)); convex_base.offsets.reset( new std::vector(convex_base.num_normals_and_offsets)); } diff --git a/include/coal/serialization/kIOS.h b/include/coal/serialization/kIOS.h index 14c6457fc..18d349db2 100644 --- a/include/coal/serialization/kIOS.h +++ b/include/coal/serialization/kIOS.h @@ -23,7 +23,7 @@ void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) { // Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres ar& make_nvp("num_spheres", bv.num_spheres); - std::array centers{}; + std::array centers{}; std::array radii; for (std::size_t i = 0; i < coal::kIOS::max_num_spheres; ++i) { centers[i] = bv.spheres[i].o; @@ -39,7 +39,7 @@ template void load(Archive& ar, coal::kIOS& bv, const unsigned int /*version*/) { ar >> make_nvp("num_spheres", bv.num_spheres); - std::array centers; + std::array centers; std::array radii; ar >> make_nvp("centers", make_array(centers.data(), centers.size())); ar >> make_nvp("radii", make_array(radii.data(), radii.size())); diff --git a/include/coal/shape/convex.h b/include/coal/shape/convex.h index 916fdfda1..a17108a6e 100644 --- a/include/coal/shape/convex.h +++ b/include/coal/shape/convex.h @@ -60,7 +60,7 @@ class Convex : public ConvexBase { /// \param polygons_ \copydoc Convex::polygons /// \param num_polygons_ the number of polygons. /// \note num_polygons_ is not the allocated size of polygons_. - Convex(std::shared_ptr> points_, unsigned int num_points_, + Convex(std::shared_ptr> points_, unsigned int num_points_, std::shared_ptr> polygons_, unsigned int num_polygons_); @@ -71,9 +71,9 @@ class Convex : public ConvexBase { ~Convex(); /// based on http://number-none.com/blow/inertia/bb_inertia.doc - Matrix3f computeMomentofInertia() const; + Matrix3s computeMomentofInertia() const; - Vec3f computeCOM() const; + Vec3s computeCOM() const; CoalScalar computeVolume() const; @@ -88,7 +88,7 @@ class Convex : public ConvexBase { /// \param num_polygons the number of polygons. /// \note num_polygons is not the allocated size of polygons. /// - void set(std::shared_ptr> points, unsigned int num_points, + void set(std::shared_ptr> points, unsigned int num_points, std::shared_ptr> polygons, unsigned int num_polygons); diff --git a/include/coal/shape/details/convex.hxx b/include/coal/shape/details/convex.hxx index 566af0230..0c0ccf58c 100644 --- a/include/coal/shape/details/convex.hxx +++ b/include/coal/shape/details/convex.hxx @@ -44,7 +44,7 @@ namespace coal { template -Convex::Convex(std::shared_ptr> points_, +Convex::Convex(std::shared_ptr> points_, unsigned int num_points_, std::shared_ptr> polygons_, unsigned int num_polygons_) @@ -67,7 +67,7 @@ template Convex::~Convex() {} template -void Convex::set(std::shared_ptr> points_, +void Convex::set(std::shared_ptr> points_, unsigned int num_points_, std::shared_ptr> polygons_, unsigned int num_polygons_) { @@ -86,13 +86,13 @@ Convex* Convex::clone() const { } template -Matrix3f Convex::computeMomentofInertia() const { +Matrix3s Convex::computeMomentofInertia() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::index_type index_type; - Matrix3f C = Matrix3f::Zero(); + Matrix3s C = Matrix3s::Zero(); - Matrix3f C_canonical; + Matrix3s C_canonical; C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0; @@ -102,7 +102,7 @@ Matrix3f Convex::computeMomentofInertia() const { << std::endl; return C; } - const std::vector& points_ = *points; + const std::vector& points_ = *points; if (!(polygons.get())) { std::cerr << "Error in `Convex::computeMomentofInertia`! Convex has no polygons." @@ -114,43 +114,43 @@ Matrix3f Convex::computeMomentofInertia() const { const PolygonT& polygon = polygons_[i]; // compute the center of the polygon - Vec3f plane_center(0, 0, 0); + Vec3s plane_center(0, 0, 0); for (size_type j = 0; j < polygon.size(); ++j) plane_center += points_[polygon[(index_type)j]]; plane_center /= polygon.size(); // compute the volume of tetrahedron making by neighboring two points, the // plane center and the reference point (zero) of the convex shape - const Vec3f& v3 = plane_center; + const Vec3s& v3 = plane_center; for (size_type j = 0; j < polygon.size(); ++j) { index_type e_first = polygon[static_cast(j)]; index_type e_second = polygon[static_cast((j + 1) % polygon.size())]; - const Vec3f& v1 = points_[e_first]; - const Vec3f& v2 = points_[e_second]; - Matrix3f A; + const Vec3s& v1 = points_[e_first]; + const Vec3s& v2 = points_[e_second]; + Matrix3s A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3); } } - return C.trace() * Matrix3f::Identity() - C; + return C.trace() * Matrix3s::Identity() - C; } template -Vec3f Convex::computeCOM() const { +Vec3s Convex::computeCOM() const { typedef typename PolygonT::size_type size_type; typedef typename PolygonT::index_type index_type; - Vec3f com(0, 0, 0); + Vec3s com(0, 0, 0); CoalScalar vol = 0; if (!(points.get())) { std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices." << std::endl; return com; } - const std::vector& points_ = *points; + const std::vector& points_ = *points; if (!(polygons.get())) { std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons." << std::endl; @@ -160,20 +160,20 @@ Vec3f Convex::computeCOM() const { for (unsigned int i = 0; i < num_polygons; ++i) { const PolygonT& polygon = polygons_[i]; // compute the center of the polygon - Vec3f plane_center(0, 0, 0); + Vec3s plane_center(0, 0, 0); for (size_type j = 0; j < polygon.size(); ++j) plane_center += points_[polygon[(index_type)j]]; plane_center /= polygon.size(); // compute the volume of tetrahedron making by neighboring two points, the // plane center and the reference point (zero) of the convex shape - const Vec3f& v3 = plane_center; + const Vec3s& v3 = plane_center; for (size_type j = 0; j < polygon.size(); ++j) { index_type e_first = polygon[static_cast(j)]; index_type e_second = polygon[static_cast((j + 1) % polygon.size())]; - const Vec3f& v1 = points_[e_first]; - const Vec3f& v2 = points_[e_second]; + const Vec3s& v1 = points_[e_first]; + const Vec3s& v2 = points_[e_second]; CoalScalar d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol; @@ -194,7 +194,7 @@ CoalScalar Convex::computeVolume() const { << std::endl; return vol; } - const std::vector& points_ = *points; + const std::vector& points_ = *points; if (!(polygons.get())) { std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons." << std::endl; @@ -205,20 +205,20 @@ CoalScalar Convex::computeVolume() const { const PolygonT& polygon = polygons_[i]; // compute the center of the polygon - Vec3f plane_center(0, 0, 0); + Vec3s plane_center(0, 0, 0); for (size_type j = 0; j < polygon.size(); ++j) plane_center += points_[polygon[(index_type)j]]; plane_center /= polygon.size(); // compute the volume of tetrahedron making by neighboring two points, the // plane center and the reference point (zero point) of the convex shape - const Vec3f& v3 = plane_center; + const Vec3s& v3 = plane_center; for (size_type j = 0; j < polygon.size(); ++j) { index_type e_first = polygon[static_cast(j)]; index_type e_second = polygon[static_cast((j + 1) % polygon.size())]; - const Vec3f& v1 = points_[e_first]; - const Vec3f& v2 = points_[e_second]; + const Vec3s& v1 = points_[e_first]; + const Vec3s& v2 = points_[e_second]; CoalScalar d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; } diff --git a/include/coal/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h index 48516cd79..f9a3aca8b 100644 --- a/include/coal/shape/geometric_shape_to_BVH_model.h +++ b/include/coal/shape/geometric_shape_to_BVH_model.h @@ -51,16 +51,16 @@ void generateBVHModel(BVHModel& model, const Box& shape, CoalScalar a = shape.halfSide[0]; CoalScalar b = shape.halfSide[1]; CoalScalar c = shape.halfSide[2]; - std::vector points(8); + std::vector points(8); std::vector tri_indices(12); - points[0] = Vec3f(a, -b, c); - points[1] = Vec3f(a, b, c); - points[2] = Vec3f(-a, b, c); - points[3] = Vec3f(-a, -b, c); - points[4] = Vec3f(a, -b, -c); - points[5] = Vec3f(a, b, -c); - points[6] = Vec3f(-a, b, -c); - points[7] = Vec3f(-a, -b, -c); + points[0] = Vec3s(a, -b, c); + points[1] = Vec3s(a, b, c); + points[2] = Vec3s(-a, b, c); + points[3] = Vec3s(-a, -b, c); + points[4] = Vec3s(a, -b, -c); + points[5] = Vec3s(a, b, -c); + points[6] = Vec3s(-a, b, -c); + points[7] = Vec3s(-a, -b, -c); tri_indices[0].set(0, 4, 1); tri_indices[1].set(1, 4, 5); @@ -91,7 +91,7 @@ template void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring) { - std::vector points; + std::vector points; std::vector tri_indices; CoalScalar r = shape.radius; @@ -107,13 +107,13 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, for (unsigned int i = 0; i < ring; ++i) { CoalScalar theta_ = theta + thetad * (i + 1); for (unsigned int j = 0; j < seg; ++j) { - points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), + points.push_back(Vec3s(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_))); } } - points.push_back(Vec3f(0, 0, r)); - points.push_back(Vec3f(0, 0, -r)); + points.push_back(Vec3s(0, 0, r)); + points.push_back(Vec3s(0, 0, -r)); for (unsigned int i = 0; i < ring - 1; ++i) { for (unsigned int j = 0; j < seg; ++j) { @@ -172,7 +172,7 @@ template void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num) { - std::vector points; + std::vector points; std::vector tri_indices; CoalScalar r = shape.radius; @@ -186,21 +186,21 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, for (unsigned int i = 0; i < tot; ++i) points.push_back( - Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h)); + Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), h)); for (unsigned int i = 0; i < h_num - 1; ++i) { for (unsigned int j = 0; j < tot; ++j) { - points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), + points.push_back(Vec3s(r * cos(phi + phid * j), r * sin(phi + phid * j), h - (i + 1) * hd)); } } for (unsigned int i = 0; i < tot; ++i) points.push_back( - Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); + Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); - points.push_back(Vec3f(0, 0, h)); - points.push_back(Vec3f(0, 0, -h)); + points.push_back(Vec3s(0, 0, h)); + points.push_back(Vec3s(0, 0, -h)); for (unsigned int i = 0; i < tot; ++i) { Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); @@ -264,7 +264,7 @@ template void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num) { - std::vector points; + std::vector points; std::vector tri_indices; CoalScalar r = shape.radius; @@ -282,16 +282,16 @@ void generateBVHModel(BVHModel& model, const Cone& shape, CoalScalar rh = r * (0.5 - h_i / h / 2); for (unsigned int j = 0; j < tot; ++j) { points.push_back( - Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); + Vec3s(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); } } for (unsigned int i = 0; i < tot; ++i) points.push_back( - Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); + Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h)); - points.push_back(Vec3f(0, 0, h)); - points.push_back(Vec3f(0, 0, -h)); + points.push_back(Vec3s(0, 0, h)); + points.push_back(Vec3s(0, 0, -h)); for (unsigned int i = 0; i < tot; ++i) { Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h index d21d5c214..e88a737e9 100644 --- a/include/coal/shape/geometric_shapes.h +++ b/include/coal/shape/geometric_shapes.h @@ -111,7 +111,7 @@ class COAL_DLLAPI TriangleP : public ShapeBase { public: TriangleP() {}; - TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) + TriangleP(const Vec3s& a_, const Vec3s& b_, const Vec3s& c_) : ShapeBase(), a(a_), b(b_), c(c_) {} TriangleP(const TriangleP& other) @@ -128,13 +128,13 @@ class COAL_DLLAPI TriangleP : public ShapeBase { // std::pair inflated(const CoalScalar value) const // { // if (value == 0) return std::make_pair(new TriangleP(*this), - // Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize(); + // Transform3f()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize(); // BC.normalize(); // CA.normalize(); // - // Vec3f new_a(a + value * Vec3f(-AB + CA).normalized()); - // Vec3f new_b(b + value * Vec3f(-BC + AB).normalized()); - // Vec3f new_c(c + value * Vec3f(-CA + BC).normalized()); + // Vec3s new_a(a + value * Vec3s(-AB + CA).normalized()); + // Vec3s new_b(b + value * Vec3s(-BC + AB).normalized()); + // Vec3s new_c(c + value * Vec3s(-CA + BC).normalized()); // // return std::make_pair(new TriangleP(new_a, new_b, new_c), // Transform3f()); @@ -146,7 +146,7 @@ class COAL_DLLAPI TriangleP : public ShapeBase { // implement // } - Vec3f a, b, c; + Vec3s a, b, c; private: virtual bool isEqual(const CollisionGeometry& _other) const { @@ -168,7 +168,7 @@ class COAL_DLLAPI Box : public ShapeBase { Box(CoalScalar x, CoalScalar y, CoalScalar z) : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {} - Box(const Vec3f& side_) : ShapeBase(), halfSide(side_ / 2) {} + Box(const Vec3s& side_) : ShapeBase(), halfSide(side_ / 2) {} Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {} @@ -186,7 +186,7 @@ class COAL_DLLAPI Box : public ShapeBase { Box() {} /// @brief box side half-length - Vec3f halfSide; + Vec3s halfSide; /// @brief Compute AABB void computeLocalAABB(); @@ -196,10 +196,10 @@ class COAL_DLLAPI Box : public ShapeBase { CoalScalar computeVolume() const { return 8 * halfSide.prod(); } - Matrix3f computeMomentofInertia() const { + Matrix3s computeMomentofInertia() const { CoalScalar V = computeVolume(); - Vec3f s(halfSide.cwiseAbs2() * V); - return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); + Vec3s s(halfSide.cwiseAbs2() * V); + return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); } CoalScalar minInflationValue() const { return -halfSide.minCoeff(); } @@ -218,7 +218,7 @@ class COAL_DLLAPI Box : public ShapeBase { << "is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))), + return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))), Transform3f()); } @@ -258,9 +258,9 @@ class COAL_DLLAPI Sphere : public ShapeBase { /// @brief Get node type: a sphere NODE_TYPE getNodeType() const { return GEOM_SPHERE; } - Matrix3f computeMomentofInertia() const { + Matrix3s computeMomentofInertia() const { CoalScalar I = 0.4 * radius * radius * computeVolume(); - return I * Matrix3f::Identity(); + return I * Matrix3s::Identity(); } CoalScalar computeVolume() const { @@ -310,7 +310,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { Ellipsoid(CoalScalar rx, CoalScalar ry, CoalScalar rz) : ShapeBase(), radii(rx, ry, rz) {} - explicit Ellipsoid(const Vec3f& radii) : radii(radii) {} + explicit Ellipsoid(const Vec3s& radii) : radii(radii) {} Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} @@ -319,7 +319,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { /// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2 /// + z^2/rz^2 = 1) - Vec3f radii; + Vec3s radii; /// @brief Compute AABB void computeLocalAABB(); @@ -327,12 +327,12 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { /// @brief Get node type: an ellipsoid NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; } - Matrix3f computeMomentofInertia() const { + Matrix3s computeMomentofInertia() const { CoalScalar V = computeVolume(); CoalScalar a2 = V * radii[0] * radii[0]; CoalScalar b2 = V * radii[1] * radii[1]; CoalScalar c2 = V * radii[2] * radii[2]; - return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0, + return (Matrix3s() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0, 0.2 * (a2 + b2)) .finished(); } @@ -358,7 +358,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)), + return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)), Transform3f()); } @@ -412,7 +412,7 @@ class COAL_DLLAPI Capsule : public ShapeBase { ((halfLength * 2) + radius * 4 / 3.0); } - Matrix3f computeMomentofInertia() const { + Matrix3s computeMomentofInertia() const { CoalScalar v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi(); CoalScalar v_sph = radius * radius * radius * @@ -424,7 +424,7 @@ class COAL_DLLAPI Capsule : public ShapeBase { v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength); CoalScalar iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; - return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } CoalScalar minInflationValue() const { return -radius; } @@ -496,16 +496,16 @@ class COAL_DLLAPI Cone : public ShapeBase { (halfLength * 2) / 3; } - Matrix3f computeMomentofInertia() const { + Matrix3s computeMomentofInertia() const { CoalScalar V = computeVolume(); CoalScalar ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20); CoalScalar iz = 0.3 * V * radius * radius; - return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } - Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); } + Vec3s computeCOM() const { return Vec3s(0, 0, -0.5 * halfLength); } CoalScalar minInflationValue() const { return -(std::min)(radius, halfLength); @@ -538,7 +538,7 @@ class COAL_DLLAPI Cone : public ShapeBase { const CoalScalar new_radius = new_lz / tan_alpha; return std::make_pair(Cone(new_radius, new_lz), - Transform3f(Vec3f(0., 0., new_cz))); + Transform3f(Vec3s(0., 0., new_cz))); } private: @@ -597,11 +597,11 @@ class COAL_DLLAPI Cylinder : public ShapeBase { (halfLength * 2); } - Matrix3f computeMomentofInertia() const { + Matrix3s computeMomentofInertia() const { CoalScalar V = computeVolume(); CoalScalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3); CoalScalar iz = V * radius * radius / 2; - return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); + return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } CoalScalar minInflationValue() const { @@ -656,13 +656,13 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// Qhull. /// \note Coal must have been compiled with option \c COAL_HAS_QHULL set /// to \c ON. - static ConvexBase* convexHull(std::shared_ptr>& points, + static ConvexBase* convexHull(std::shared_ptr>& points, unsigned int num_points, bool keepTriangles, const char* qhullCommand = NULL); // TODO(louis): put this method in private sometime in the future. COAL_DEPRECATED static ConvexBase* convexHull( - const Vec3f* points, unsigned int num_points, bool keepTriangles, + const Vec3s* points, unsigned int num_points, bool keepTriangles, const char* qhullCommand = NULL); virtual ~ConvexBase(); @@ -716,11 +716,11 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { static constexpr size_t num_vertices_large_convex_threshold = 32; /// @brief An array of the points of the polygon. - std::shared_ptr> points; + std::shared_ptr> points; unsigned int num_points; /// @brief An array of the normals of the polygon. - std::shared_ptr> normals; + std::shared_ptr> normals; /// @brief An array of the offsets to the normals of the polygon. /// Note: there are as many offsets as normals. std::shared_ptr> offsets; @@ -733,7 +733,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// @brief center of the convex polytope, this is used for collision: center /// is guaranteed in the internal of the polytope (as it is convex) - Vec3f center; + Vec3s center; /// @brief The support warm start polytope contains certain points of `this` /// which are support points in specific directions of space. @@ -742,7 +742,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { struct SupportWarmStartPolytope { /// @brief Array of support points to warm start the support function /// computation. - std::vector points; + std::vector points; /// @brief Indices of the support points warm starts. /// These are the indices of the real convex, not the indices of points in @@ -763,7 +763,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { : ShapeBase(), num_points(0), num_normals_and_offsets(0), - center(Vec3f::Zero()) {} + center(Vec3s::Zero()) {} /// @brief Initialize the points of the convex shape /// This also initializes the ConvexBase::center. @@ -771,7 +771,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// \param ownStorage weither the ConvexBase owns the data. /// \param points_ list of 3D points /// /// \param num_points_ number of 3D points - void initialize(std::shared_ptr> points_, + void initialize(std::shared_ptr> points_, unsigned int num_points_); /// @brief Set the points of the convex shape. @@ -779,7 +779,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { /// \param ownStorage weither the ConvexBase owns the data. /// \param points_ list of 3D points /// /// \param num_points_ number of 3D points - void set(std::shared_ptr> points_, + void set(std::shared_ptr> points_, unsigned int num_points_); /// @brief Copy constructor @@ -814,8 +814,8 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { (points.get() && !(other.points.get()))) return false; if (points.get() && other.points.get()) { - const std::vector& points_ = *points; - const std::vector& other_points_ = *(other.points); + const std::vector& points_ = *points; + const std::vector& other_points_ = *(other.points); for (unsigned int i = 0; i < num_points; ++i) { if (points_[i] != (other_points_)[i]) return false; } @@ -836,8 +836,8 @@ class COAL_DLLAPI ConvexBase : public ShapeBase { (normals.get() && !(other.normals.get()))) return false; if (normals.get() && other.normals.get()) { - const std::vector& normals_ = *normals; - const std::vector& other_normals_ = *(other.normals); + const std::vector& normals_ = *normals; + const std::vector& other_normals_ = *(other.normals); for (unsigned int i = 0; i < num_normals_and_offsets; ++i) { if (normals_[i] != other_normals_[i]) return false; } @@ -892,7 +892,7 @@ class Convex; class COAL_DLLAPI Halfspace : public ShapeBase { public: /// @brief Construct a half space with normal direction and offset - Halfspace(const Vec3f& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { + Halfspace(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } @@ -917,11 +917,11 @@ class COAL_DLLAPI Halfspace : public ShapeBase { /// @brief Clone *this into a new Halfspace virtual Halfspace* clone() const { return new Halfspace(*this); }; - CoalScalar signedDistance(const Vec3f& p) const { + CoalScalar signedDistance(const Vec3s& p) const { return n.dot(p) - (d + this->getSweptSphereRadius()); } - CoalScalar distance(const Vec3f& p) const { + CoalScalar distance(const Vec3s& p) const { return std::abs(this->signedDistance(p)); } @@ -953,7 +953,7 @@ class COAL_DLLAPI Halfspace : public ShapeBase { } /// @brief Plane normal - Vec3f n; + Vec3s n; /// @brief Plane offset CoalScalar d; @@ -983,7 +983,7 @@ class COAL_DLLAPI Halfspace : public ShapeBase { class COAL_DLLAPI Plane : public ShapeBase { public: /// @brief Construct a plane with normal direction and offset - Plane(const Vec3f& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { + Plane(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } @@ -1007,7 +1007,7 @@ class COAL_DLLAPI Plane : public ShapeBase { /// @brief Clone *this into a new Plane virtual Plane* clone() const { return new Plane(*this); }; - CoalScalar signedDistance(const Vec3f& p) const { + CoalScalar signedDistance(const Vec3s& p) const { const CoalScalar dist = n.dot(p) - d; CoalScalar signed_dist = std::abs(n.dot(p) - d) - this->getSweptSphereRadius(); @@ -1020,7 +1020,7 @@ class COAL_DLLAPI Plane : public ShapeBase { return signed_dist; } - CoalScalar distance(const Vec3f& p) const { + CoalScalar distance(const Vec3s& p) const { return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius()); } @@ -1031,7 +1031,7 @@ class COAL_DLLAPI Plane : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_PLANE; } /// @brief Plane normal - Vec3f n; + Vec3s n; /// @brief Plane offset CoalScalar d; diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h index 83187b4f1..305008870 100644 --- a/include/coal/shape/geometric_shapes_utility.h +++ b/include/coal/shape/geometric_shapes_utility.h @@ -49,21 +49,21 @@ namespace coal { namespace details { /// @brief get the vertices of some convex shape which can bound the given shape /// in a specific configuration -COAL_DLLAPI std::vector getBoundVertices(const Box& box, +COAL_DLLAPI std::vector getBoundVertices(const Box& box, const Transform3f& tf); -COAL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, +COAL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf); -COAL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, +COAL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf); -COAL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, +COAL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, const Transform3f& tf); -COAL_DLLAPI std::vector getBoundVertices(const Cone& cone, +COAL_DLLAPI std::vector getBoundVertices(const Cone& cone, const Transform3f& tf); -COAL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, +COAL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& tf); -COAL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, +COAL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, const Transform3f& tf); -COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, +COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, const Transform3f& tf); } // namespace details /// @endcond @@ -75,7 +75,7 @@ inline void computeBV(const S& s, const Transform3f& tf, BV& bv) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - std::vector convex_bound_vertices = details::getBoundVertices(s, tf); + std::vector convex_bound_vertices = details::getBoundVertices(s, tf); fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(), bv); } diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc index 583e611c6..28e427a27 100644 --- a/python/broadphase/broadphase.cc +++ b/python/broadphase/broadphase.cc @@ -135,7 +135,7 @@ void exposeBroadPhase() { typedef SpatialHashingCollisionManager Derived; bp::class_>( "SpatialHashingCollisionManager", bp::no_init) - .def(dv::init>()); } } diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 2adacda28..63d160bbe 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -75,7 +75,7 @@ namespace bp = boost::python; using boost::noncopyable; -typedef std::vector Vec3fs; +typedef std::vector Vec3ss; typedef std::vector Triangles; struct BVHModelBaseWrapper { @@ -83,7 +83,7 @@ struct BVHModelBaseWrapper { typedef Eigen::Map MapRowMatrixX3; typedef Eigen::Ref RefRowMatrixX3; - static Vec3f& vertex(BVHModelBase& bvh, unsigned int i) { + static Vec3s& vertex(BVHModelBase& bvh, unsigned int i) { if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range"); return (*(bvh.vertices))[i]; } @@ -130,7 +130,7 @@ void exposeHeightField(const std::string& bvname) { type_name.c_str(), doxygen::class_doc(), no_init) .def(dv::init>()) .def(dv::init, const HeightField&>()) - .def(dv::init, CoalScalar, CoalScalar, const MatrixXf&, + .def(dv::init, CoalScalar, CoalScalar, const MatrixXs&, bp::optional>()) .DEF_CLASS_FUNC(Geometry, getXDim) @@ -168,7 +168,7 @@ struct ConvexBaseWrapper { typedef Eigen::Map MapVecOfDoubles; typedef Eigen::Ref RefVecOfDoubles; - static Vec3f& point(const ConvexBase& convex, unsigned int i) { + static Vec3s& point(const ConvexBase& convex, unsigned int i) { if (i >= convex.num_points) throw std::out_of_range("index is out of range"); return (*(convex.points))[i]; @@ -178,7 +178,7 @@ struct ConvexBaseWrapper { return MapRowMatrixX3((*(convex.points))[0].data(), convex.num_points, 3); } - static Vec3f& normal(const ConvexBase& convex, unsigned int i) { + static Vec3s& normal(const ConvexBase& convex, unsigned int i) { if (i >= convex.num_normals_and_offsets) throw std::out_of_range("index is out of range"); return (*(convex.normals))[i]; @@ -210,7 +210,7 @@ struct ConvexBaseWrapper { return n; } - static ConvexBase* convexHull(const Vec3fs& points, bool keepTri, + static ConvexBase* convexHull(const Vec3ss& points, bool keepTri, const char* qhullCommand) { return ConvexBase::convexHull(points.data(), (unsigned int)points.size(), keepTri, qhullCommand); @@ -227,11 +227,11 @@ struct ConvexWrapper { return (*convex.polygons)[i]; } - static shared_ptr constructor(const Vec3fs& _points, + static shared_ptr constructor(const Vec3ss& _points, const Triangles& _tris) { - std::shared_ptr> points( - new std::vector(_points.size())); - std::vector& points_ = *points; + std::shared_ptr> points( + new std::vector(_points.size())); + std::vector& points_ = *points; for (std::size_t i = 0; i < _points.size(); ++i) points_[i] = _points[i]; std::shared_ptr> tris( @@ -279,7 +279,7 @@ void exposeShapes() { .def(dv::init()) .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Box, halfSide) .def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone), return_value_policy()) @@ -372,7 +372,7 @@ void exposeShapes() { class_, shared_ptr>( "Halfspace", doxygen::class_doc(), no_init) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .def( dv::init()) @@ -387,7 +387,7 @@ void exposeShapes() { class_, shared_ptr>( "Plane", doxygen::class_doc(), no_init) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .def(dv::init()) .def(dv::init()) @@ -413,7 +413,7 @@ void exposeShapes() { "Ellipsoid", doxygen::class_doc(), no_init) .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, @@ -425,7 +425,7 @@ void exposeShapes() { class_, shared_ptr>( "TriangleP", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .DEF_RW_CLASS_ATTRIB(TriangleP, a) .DEF_RW_CLASS_ATTRIB(TriangleP, b) @@ -438,7 +438,7 @@ void exposeShapes() { } boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) { - Vec3f P, Q; + Vec3s P, Q; CoalScalar distance = self.distance(other, &P, &Q); return boost::python::make_tuple(distance, P, Q); } @@ -502,17 +502,17 @@ void exposeCollisionGeometries() { no_init) .def(init<>(bp::arg("self"), "Default constructor")) .def(init(bp::args("self", "other"), "Copy constructor")) - .def(init(bp::args("self", "v"), + .def(init(bp::args("self", "v"), "Creating an AABB at position v with zero size.")) - .def(init(bp::args("self", "a", "b"), + .def(init(bp::args("self", "a", "b"), "Creating an AABB with two endpoints a and b.")) - .def(init( + .def(init( bp::args("self", "core", "delta"), "Creating an AABB centered as core and is of half-dimension delta.")) - .def(init(bp::args("self", "a", "b", "c"), + .def(init(bp::args("self", "a", "b", "c"), "Creating an AABB contains three points.")) - .def("contain", (bool(AABB::*)(const Vec3f&) const) & AABB::contain, + .def("contain", (bool(AABB::*)(const Vec3s&) const) & AABB::contain, bp::args("self", "p"), "Check whether the AABB contains a point p.") .def("contain", (bool(AABB::*)(const AABB&) const) & AABB::contain, bp::args("self", "other"), @@ -536,18 +536,18 @@ void exposeCollisionGeometries() { .add_property( "min_", bp::make_function( - +[](AABB& self) -> Vec3f& { return self.min_; }, + +[](AABB& self) -> Vec3s& { return self.min_; }, bp::return_internal_reference<>()), bp::make_function( - +[](AABB& self, const Vec3f& min_) -> void { self.min_ = min_; }), + +[](AABB& self, const Vec3s& min_) -> void { self.min_ = min_; }), "The min point in the AABB.") .add_property( "max_", bp::make_function( - +[](AABB& self) -> Vec3f& { return self.max_; }, + +[](AABB& self) -> Vec3s& { return self.max_; }, bp::return_internal_reference<>()), bp::make_function( - +[](AABB& self, const Vec3f& max_) -> void { self.max_ = max_; }), + +[](AABB& self, const Vec3s& max_) -> void { self.max_ = max_; }), "The max point in the AABB.") .def(bp::self == bp::self) @@ -555,7 +555,7 @@ void exposeCollisionGeometries() { .def(bp::self + bp::self) .def(bp::self += bp::self) - .def(bp::self += Vec3f()) + .def(bp::self += Vec3s()) .def("size", &AABB::volume, bp::arg("self"), "Size of the AABB.") .def("center", &AABB::center, bp::arg("self"), "Center of the AABB.") @@ -578,19 +578,19 @@ void exposeCollisionGeometries() { // doxygen::member_func_args(static_cast(&AABB::expand)), bp::return_internal_reference<>()) - .def("expand", static_cast(&AABB::expand), + .def("expand", static_cast(&AABB::expand), // doxygen::member_func_doc(static_cast(&AABB::expand)), + // Vec3s &)>(&AABB::expand)), // doxygen::member_func_args(static_cast(&AABB::expand)), + // (AABB::*)(const Vec3s &)>(&AABB::expand)), bp::return_internal_reference<>()) .def_pickle(PickleObject()) .def(SerializableVisitor()); - def("translate", (AABB(*)(const AABB&, const Vec3f&)) & translate, + def("translate", (AABB(*)(const AABB&, const Vec3s&)) & translate, bp::args("aabb", "t"), "Translate the center of AABB by t"); - def("rotate", (AABB(*)(const AABB&, const Matrix3f&)) & rotate, + def("rotate", (AABB(*)(const AABB&, const Matrix3s&)) & rotate, bp::args("aabb", "R"), "Rotate the AABB object by R"); if (!eigenpy::register_symbolic_link_to_registered_type< @@ -671,9 +671,9 @@ void exposeCollisionGeometries() { .def(dv::member_func("addTriangle", &BVHModelBase::addTriangle)) .def(dv::member_func("addTriangles", &BVHModelBase::addTriangles)) .def(dv::member_func("addSubModel", + const Vec3ss&, const Triangles&)>("addSubModel", &BVHModelBase::addSubModel)) - .def(dv::member_func( + .def(dv::member_func( "addSubModel", &BVHModelBase::addSubModel)) .def(dv::member_func("endModel", &BVHModelBase::endModel)) // Expose function to replace a BVH @@ -701,7 +701,7 @@ void exposeCollisionObject() { .def(dv::init>()) .def(dv::init>()) + const Matrix3s&, const Vec3s&, bp::optional>()) .DEF_CLASS_FUNC(CollisionObject, getObjectType) .DEF_CLASS_FUNC(CollisionObject, getNodeType) diff --git a/python/collision.cc b/python/collision.cc index dac1c2dbc..12e596a10 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -65,10 +65,10 @@ const CollisionGeometry* geto(const Contact& c) { } struct ContactWrapper { - static Vec3f getNearestPoint1(const Contact& contact) { + static Vec3s getNearestPoint1(const Contact& contact) { return contact.nearest_points[0]; } - static Vec3f getNearestPoint2(const Contact& contact) { + static Vec3s getNearestPoint2(const Contact& contact) { return contact.nearest_points[1]; } }; @@ -182,8 +182,8 @@ void exposeCollisionAPI() { .def(dv::init()) .def(dv::init()) + const CollisionGeometry*, int, int, const Vec3s&, + const Vec3s&, CoalScalar>()) .add_property( "o1", make_function(&geto<1>, diff --git a/python/distance.cc b/python/distance.cc index 0e0032d9e..d83df0143 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -59,10 +59,10 @@ using namespace coal::python; namespace dv = doxygen::visitor; struct DistanceResultWrapper { - static Vec3f getNearestPoint1(const DistanceResult& res) { + static Vec3s getNearestPoint1(const DistanceResult& res) { return res.nearest_points[0]; } - static Vec3f getNearestPoint2(const DistanceResult& res) { + static Vec3s getNearestPoint2(const DistanceResult& res) { return res.nearest_points[1]; } }; diff --git a/python/gjk.cc b/python/gjk.cc index 9f705fb91..a098f2b15 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -52,7 +52,7 @@ using coal::details::MinkowskiDiff; using coal::details::SupportOptions; struct MinkowskiDiffWrapper { - static void support0(MinkowskiDiff& self, const Vec3f& dir, int& hint, + static void support0(MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support0(dir, hint); @@ -61,7 +61,7 @@ struct MinkowskiDiffWrapper { } } - static void support1(MinkowskiDiff& self, const Vec3f& dir, int& hint, + static void support1(MinkowskiDiff& self, const Vec3s& dir, int& hint, bool compute_swept_sphere_support = false) { if (compute_swept_sphere_support) { self.support1(dir, hint); diff --git a/python/math.cc b/python/math.cc index 85c365d46..119d29e04 100644 --- a/python/math.cc +++ b/python/math.cc @@ -74,17 +74,17 @@ void exposeMaths() { if (!eigenpy::register_symbolic_link_to_registered_type()) eigenpy::exposeAngleAxis(); - eigenpy::enableEigenPySpecific(); - eigenpy::enableEigenPySpecific(); + eigenpy::enableEigenPySpecific(); + eigenpy::enableEigenPySpecific(); class_("Transform3f", doxygen::class_doc(), no_init) .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) .def(dv::init()) - .def(dv::init()) + .def(dv::init()) .def(dv::init()) .def(dv::member_func("getQuatRotation", &Transform3f::getQuatRotation)) @@ -99,19 +99,19 @@ void exposeMaths() { doxygen::member_func_doc(&Transform3f::getTranslation)) .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) - .def("setTranslation", &Transform3f::setTranslation) - .def("setRotation", &Transform3f::setRotation) + .def("setTranslation", &Transform3f::setTranslation) + .def("setRotation", &Transform3f::setRotation) .def(dv::member_func("setTransform", - &Transform3f::setTransform)) + &Transform3f::setTransform)) .def(dv::member_func( "setTransform", - static_cast( + static_cast( &Transform3f::setTransform))) .def(dv::member_func("setIdentity", &Transform3f::setIdentity)) .def(dv::member_func("Identity", &Transform3f::Identity)) .staticmethod("Identity") - .def(dv::member_func("transform", &Transform3f::transform)) + .def(dv::member_func("transform", &Transform3f::transform)) .def("inverseInPlace", &Transform3f::inverseInPlace, return_internal_reference<>(), doxygen::member_func_doc(&Transform3f::inverseInPlace)) @@ -137,9 +137,9 @@ void exposeMaths() { .def(self == self); if (!eigenpy::register_symbolic_link_to_registered_type< - std::vector >()) { - class_ >("StdVec_Vec3f") - .def(vector_indexing_suite >()); + std::vector >()) { + class_ >("StdVec_Vec3s") + .def(vector_indexing_suite >()); } if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { diff --git a/python/octree.cc b/python/octree.cc index a569bba51..a33ac1665 100644 --- a/python/octree.cc +++ b/python/octree.cc @@ -53,7 +53,7 @@ void exposeOctree() { .def("tobytes", tobytes, doxygen::member_func_doc(&OcTree::tobytes)); doxygen::def("makeOctree", &makeOctree); - eigenpy::enableEigenPySpecific(); - eigenpy::StdVectorPythonVisitor, true>::expose( + eigenpy::enableEigenPySpecific(); + eigenpy::StdVectorPythonVisitor, true>::expose( "StdVec_Vec6"); } diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index de3d582d0..4e28a2548 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -44,8 +44,8 @@ namespace coal { AABB::AABB() - : min_(Vec3f::Constant((std::numeric_limits::max)())), - max_(Vec3f::Constant(-(std::numeric_limits::max)())) {} + : min_(Vec3s::Constant((std::numeric_limits::max)())), + max_(Vec3s::Constant(-(std::numeric_limits::max)())) {} bool AABB::overlap(const AABB& other, const CollisionRequest& request, CoalScalar& sqrDistLowerBound) const { @@ -53,7 +53,7 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request, request.break_distance * request.break_distance; sqrDistLowerBound = - (min_ - other.max_ - Vec3f::Constant(request.security_margin)) + (min_ - other.max_ - Vec3s::Constant(request.security_margin)) .array() .max(CoalScalar(0)) .matrix() @@ -61,7 +61,7 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request, if (sqrDistLowerBound > break_distance_squared) return false; sqrDistLowerBound = - (other.min_ - max_ - Vec3f::Constant(request.security_margin)) + (other.min_ - max_ - Vec3s::Constant(request.security_margin)) .array() .max(CoalScalar(0)) .matrix() @@ -71,7 +71,7 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request, return true; } -CoalScalar AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { +CoalScalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const { CoalScalar result = 0; for (Eigen::DenseIndex i = 0; i < 3; ++i) { const CoalScalar& amin = min_[i]; @@ -131,13 +131,13 @@ CoalScalar AABB::distance(const AABB& other) const { return std::sqrt(result); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2) { AABB bb1(translate(rotate(b1, R0), T0)); return bb1.overlap(b2); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1, const AABB& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { AABB bb1(translate(rotate(b1, R0), T0)); @@ -149,11 +149,11 @@ bool AABB::overlap(const Plane& p) const { // points in the directions normal and -normal. // If both points lie on different sides of the plane, there is an overlap // between the AABB and the plane. Otherwise, there is no overlap. - const Vec3f halfside = (this->max_ - this->min_) / 2; - const Vec3f center = (this->max_ + this->min_) / 2; + const Vec3s halfside = (this->max_ - this->min_) / 2; + const Vec3s center = (this->max_ + this->min_) / 2; - const Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center; - const Vec3f support2 = + const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center; + const Vec3s support2 = ((-p.n).array() > 0).select(halfside, -halfside) + center; const CoalScalar dist1 = p.n.dot(support1) - p.d; @@ -185,9 +185,9 @@ bool AABB::overlap(const Halfspace& hs) const { // If the support is below the plane defined by the halfspace, there is an // overlap between the AABB and the halfspace. Otherwise, there is no // overlap. - Vec3f halfside = (this->max_ - this->min_) / 2; - Vec3f center = (this->max_ + this->min_) / 2; - Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; + Vec3s halfside = (this->max_ - this->min_) / 2; + Vec3s center = (this->max_ + this->min_) / 2; + Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center; return (hs.signedDistance(support) < 0); } diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 621b059c2..6ae95bcf7 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -47,32 +47,32 @@ namespace coal { /// @brief Compute the 8 vertices of a OBB -inline void computeVertices(const OBB& b, Vec3f vertices[8]) { - Matrix3f extAxes(b.axes * b.extent.asDiagonal()); - vertices[0].noalias() = b.To + extAxes * Vec3f(-1, -1, -1); - vertices[1].noalias() = b.To + extAxes * Vec3f(1, -1, -1); - vertices[2].noalias() = b.To + extAxes * Vec3f(1, 1, -1); - vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1, -1); - vertices[4].noalias() = b.To + extAxes * Vec3f(-1, -1, 1); - vertices[5].noalias() = b.To + extAxes * Vec3f(1, -1, 1); - vertices[6].noalias() = b.To + extAxes * Vec3f(1, 1, 1); - vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1); +inline void computeVertices(const OBB& b, Vec3s vertices[8]) { + Matrix3s extAxes(b.axes * b.extent.asDiagonal()); + vertices[0].noalias() = b.To + extAxes * Vec3s(-1, -1, -1); + vertices[1].noalias() = b.To + extAxes * Vec3s(1, -1, -1); + vertices[2].noalias() = b.To + extAxes * Vec3s(1, 1, -1); + vertices[3].noalias() = b.To + extAxes * Vec3s(-1, 1, -1); + vertices[4].noalias() = b.To + extAxes * Vec3s(-1, -1, 1); + vertices[5].noalias() = b.To + extAxes * Vec3s(1, -1, 1); + vertices[6].noalias() = b.To + extAxes * Vec3s(1, 1, 1); + vertices[7].noalias() = b.To + extAxes * Vec3s(-1, 1, 1); } /// @brief OBB merge method when the centers of two smaller OBB are far away inline OBB merge_largedist(const OBB& b1, const OBB& b2) { OBB b; - Vec3f vertex[16]; + Vec3s vertex[16]; computeVertices(b1, vertex); computeVertices(b2, vertex + 8); - Matrix3f M; - Vec3f E[3]; + Matrix3s M; + Vec3s E[3]; CoalScalar s[3] = {0, 0, 0}; // obb axes b.axes.col(0).noalias() = (b1.To - b2.To).normalized(); - Vec3f vertex_proj[16]; + Vec3s vertex_proj[16]; for (int i = 0; i < 16; ++i) vertex_proj[i].noalias() = vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0)); @@ -102,7 +102,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) { b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid]; // set obb centers and extensions - Vec3f center, extent; + Vec3s center, extent; getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent); b.To.noalias() = center; @@ -121,10 +121,10 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { Quatf q((q0.coeffs() + q1.coeffs()).normalized()); b.axes = q.toRotationMatrix(); - Vec3f vertex[8], diff; + Vec3s vertex[8], diff; CoalScalar real_max = (std::numeric_limits::max)(); - Vec3f pmin(real_max, real_max, real_max); - Vec3f pmax(-real_max, -real_max, -real_max); + Vec3s pmin(real_max, real_max, real_max); + Vec3s pmax(-real_max, -real_max, -real_max); computeVertices(b1, vertex); for (int i = 0; i < 8; ++i) { @@ -158,12 +158,12 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { return b; } -bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b) { +bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b) { CoalScalar t, s; const CoalScalar reps = 1e-6; - Matrix3f Bf(B.array().abs() + reps); + Matrix3s Bf(B.array().abs() + reps); // Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint @@ -286,17 +286,17 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } namespace internal { -inline CoalScalar obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf) { +inline CoalScalar obbDisjoint_check_A_axis(const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf) { // |T| - |B| * b - a - Vec3f AABB_corner(T.cwiseAbs() - a); + Vec3s AABB_corner(T.cwiseAbs() - a); AABB_corner.noalias() -= Bf * b; return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); } -inline CoalScalar obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf) { +inline CoalScalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const Matrix3s& Bf) { // Bf = |B| // | B^T T| - Bf^T * a - b CoalScalar s, t = 0; @@ -311,9 +311,9 @@ inline CoalScalar obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, template struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { - static inline bool run(int ia, int ja, int ka, const Matrix3f& B, - const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, const CoalScalar& breakDistance2, + static inline bool run(int ia, int ja, int ka, const Matrix3s& B, + const Vec3s& T, const Vec3s& a, const Vec3s& b, + const Matrix3s& Bf, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); if (sinus2 < 1e-6) return false; @@ -341,8 +341,8 @@ struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { // // This function tests whether bounding boxes should be broken down. // -bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, - const Vec3f& a_, const Vec3f& b_, +bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T, + const Vec3s& a_, const Vec3s& b_, const CollisionRequest& request, CoalScalar& squaredLowerBoundDistance) { assert(request.security_margin > @@ -354,11 +354,11 @@ bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, const CoalScalar breakDistance2 = request.break_distance * request.break_distance; - Matrix3f Bf(B.cwiseAbs()); - const Vec3f a((a_ + Vec3f::Constant(request.security_margin / 2)) + Matrix3s Bf(B.cwiseAbs()); + const Vec3s a((a_ + Vec3s::Constant(request.security_margin / 2)) .array() .max(CoalScalar(0))); - const Vec3f b((b_ + Vec3f::Constant(request.security_margin / 2)) + const Vec3s b((b_ + Vec3s::Constant(request.security_margin / 2)) .array() .max(CoalScalar(0))); @@ -396,8 +396,8 @@ bool OBB::overlap(const OBB& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part - Vec3f T(axes.transpose() * (other.To - To)); - Matrix3f R(axes.transpose() * other.axes); + Vec3s T(axes.transpose() * (other.To - To)); + Matrix3s R(axes.transpose() * other.axes); return !obbDisjoint(R, T, extent, other.extent); } @@ -407,23 +407,23 @@ bool OBB::overlap(const OBB& other, const CollisionRequest& request, /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part - // Vec3f t = other.To - To; // T2 - T1 - // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) - // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), + // Vec3s t = other.To - To; // T2 - T1 + // Vec3s T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) + // Matrix3s R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), // axis[0].dot(other.axis[2]), // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), // axis[1].dot(other.axis[2]), // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), // axis[2].dot(other.axis[2])); - Vec3f T(axes.transpose() * (other.To - To)); - Matrix3f R(axes.transpose() * other.axes); + Vec3s T(axes.transpose() * (other.To - To)); + Matrix3s R(axes.transpose() * other.axes); return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request, sqrDistLowerBound); } -bool OBB::contain(const Vec3f& p) const { - Vec3f local_p(p - To); +bool OBB::contain(const Vec3s& p) const { + Vec3s local_p(p - To); CoalScalar proj = local_p.dot(axes.col(0)); if ((proj > extent[0]) || (proj < -extent[0])) return false; @@ -436,7 +436,7 @@ bool OBB::contain(const Vec3f& p) const { return true; } -OBB& OBB::operator+=(const Vec3f& p) { +OBB& OBB::operator+=(const Vec3s& p) { OBB bvp; bvp.To = p; bvp.axes.noalias() = axes; @@ -447,7 +447,7 @@ OBB& OBB::operator+=(const Vec3f& p) { } OBB OBB::operator+(const OBB& other) const { - Vec3f center_diff = To - other.To; + Vec3s center_diff = To - other.To; CoalScalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); CoalScalar max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); @@ -458,32 +458,32 @@ OBB OBB::operator+(const OBB& other) const { } } -CoalScalar OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, - Vec3f* /*Q*/) const { +CoalScalar OBB::distance(const OBB& /*other*/, Vec3s* /*P*/, + Vec3s* /*Q*/) const { std::cerr << "OBB distance not implemented!" << std::endl; return 0.0; } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2) { - Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); + Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); return !obbDisjoint(R, T, b1.extent, b2.extent); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { - Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); + Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request, sqrDistLowerBound); } -OBB translate(const OBB& bv, const Vec3f& t) { +OBB translate(const OBB& bv, const Vec3s& t) { OBB res(bv); res.To += t; return res; diff --git a/src/BV/OBB.h b/src/BV/OBB.h index 907272a50..a7d3a3817 100644 --- a/src/BV/OBB.h +++ b/src/BV/OBB.h @@ -38,13 +38,13 @@ namespace coal { -bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CollisionRequest& request, CoalScalar& squaredLowerBoundDistance); -bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b); +bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b); } // namespace coal #endif // COAL_SRC_OBB_H diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp index 18a0c83f7..1842e60f0 100644 --- a/src/BV/OBBRSS.cpp +++ b/src/BV/OBBRSS.cpp @@ -39,7 +39,7 @@ namespace coal { -OBBRSS translate(const OBBRSS& bv, const Vec3f& t) { +OBBRSS translate(const OBBRSS& bv, const Vec3s& t) { OBBRSS res(bv); res.obb.To += t; res.rss.Tr += t; diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index 15c3043ac..97d5d1adb 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -117,9 +117,9 @@ bool inVoronoi(CoalScalar a, CoalScalar b, CoalScalar Anorm_dot_B, /// @brief Distance between two oriented rectangles; P and Q (optional return /// values) are the closest points in the rectangles, both are in the local /// frame of the first rectangle. -CoalScalar rectDistance(const Matrix3f& Rab, Vec3f const& Tab, +CoalScalar rectDistance(const Matrix3s& Rab, Vec3s const& Tab, const CoalScalar a[2], const CoalScalar b[2], - Vec3f* P = NULL, Vec3f* Q = NULL) { + Vec3s* P = NULL, Vec3s* Q = NULL) { CoalScalar A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; A0_dot_B0 = Rab(0, 0); @@ -139,9 +139,9 @@ CoalScalar rectDistance(const Matrix3f& Rab, Vec3f const& Tab, bA0_dot_B1 = b[1] * A0_dot_B1; bA1_dot_B1 = b[1] * A1_dot_B1; - Vec3f Tba(Rab.transpose() * Tab); + Vec3s Tba(Rab.transpose() * Tab); - Vec3f S; + Vec3s S; CoalScalar t, u; // determine if any edge pair contains the closest points @@ -687,8 +687,8 @@ CoalScalar rectDistance(const Matrix3f& Rab, Vec3f const& Tab, } if (sep2 >= sep1 && sep2 >= 0) { - Vec3f Q_(Tab[0], Tab[1], Tab[2]); - Vec3f P_; + Vec3s Q_(Tab[0], Tab[1], Tab[2]); + Vec3s P_; if (Tba[2] < 0) { P_[0] = Rab(0, 2) * sep2 + Tab[0]; P_[1] = Rab(1, 2) * sep2 + Tab[1]; @@ -717,40 +717,40 @@ bool RSS::overlap(const RSS& other) const { /// First compute the rotation part, then translation part /// Then compute R1'(T2 - T1) - Vec3f T(axes.transpose() * (other.Tr - Tr)); + Vec3s T(axes.transpose() * (other.Tr - Tr)); /// Now compute R1'R2 - Matrix3f R(axes.transpose() * other.axes); + Matrix3s R(axes.transpose() * other.axes); CoalScalar dist = rectDistance(R, T, length, other.length); return (dist <= (radius + other.radius)); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 - Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); + Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); CoalScalar dist = rectDistance(R, T, b1.length, b2.length); return (dist <= (b1.radius + b2.radius)); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] // R = b2^T RO^T b1 - Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); - Vec3f T(b1.axes.transpose() * Ttemp); - Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); + Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); + Vec3s T(b1.axes.transpose() * Ttemp); + Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes); CoalScalar dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius - request.security_margin; @@ -759,14 +759,14 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, return false; } -bool RSS::contain(const Vec3f& p) const { - Vec3f local_p = p - Tr; - // FIXME: Vec3f proj (axes.transpose() * local_p); +bool RSS::contain(const Vec3s& p) const { + Vec3s local_p = p - Tr; + // FIXME: Vec3s proj (axes.transpose() * local_p); CoalScalar proj0 = local_p.dot(axes.col(0)); CoalScalar proj1 = local_p.dot(axes.col(1)); CoalScalar proj2 = local_p.dot(axes.col(2)); CoalScalar abs_proj2 = fabs(proj2); - Vec3f proj(proj0, proj1, proj2); + Vec3s proj(proj0, proj1, proj2); /// projection is within the rectangle if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && @@ -775,28 +775,28 @@ bool RSS::contain(const Vec3f& p) const { } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { CoalScalar y = (proj1 > 0) ? length[1] : 0; - Vec3f v(proj0, y, 0); + Vec3s v(proj0, y, 0); return ((proj - v).squaredNorm() < radius * radius); } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { CoalScalar x = (proj0 > 0) ? length[0] : 0; - Vec3f v(x, proj1, 0); + Vec3s v(x, proj1, 0); return ((proj - v).squaredNorm() < radius * radius); } else { CoalScalar x = (proj0 > 0) ? length[0] : 0; CoalScalar y = (proj1 > 0) ? length[1] : 0; - Vec3f v(x, y, 0); + Vec3s v(x, y, 0); return ((proj - v).squaredNorm() < radius * radius); } } -RSS& RSS::operator+=(const Vec3f& p) { - Vec3f local_p = p - Tr; +RSS& RSS::operator+=(const Vec3s& p) { + Vec3s local_p = p - Tr; CoalScalar proj0 = local_p.dot(axes.col(0)); CoalScalar proj1 = local_p.dot(axes.col(1)); CoalScalar proj2 = local_p.dot(axes.col(2)); CoalScalar abs_proj2 = fabs(proj2); - Vec3f proj(proj0, proj1, proj2); + Vec3s proj(proj0, proj1, proj2); // projection is within the rectangle if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && @@ -814,7 +814,7 @@ RSS& RSS::operator+=(const Vec3f& p) { } else if ((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1]))) { CoalScalar y = (proj1 > 0) ? length[1] : 0; - Vec3f v(proj0, y, 0); + Vec3s v(proj0, y, 0); CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing @@ -838,7 +838,7 @@ RSS& RSS::operator+=(const Vec3f& p) { } else if ((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0]))) { CoalScalar x = (proj0 > 0) ? length[0] : 0; - Vec3f v(x, proj1, 0); + Vec3s v(x, proj1, 0); CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing @@ -862,7 +862,7 @@ RSS& RSS::operator+=(const Vec3f& p) { } else { CoalScalar x = (proj0 > 0) ? length[0] : 0; CoalScalar y = (proj1 > 0) ? length[1] : 0; - Vec3f v(x, y, 0); + Vec3s v(x, y, 0); CoalScalar new_r_sqr = (proj - v).squaredNorm(); if (new_r_sqr < radius * radius) ; // do nothing @@ -907,13 +907,13 @@ RSS& RSS::operator+=(const Vec3f& p) { RSS RSS::operator+(const RSS& other) const { RSS bv; - Vec3f v[16]; - Vec3f d0_pos(other.axes.col(0) * (other.length[0] + other.radius)); - Vec3f d1_pos(other.axes.col(1) * (other.length[1] + other.radius)); - Vec3f d0_neg(other.axes.col(0) * (-other.radius)); - Vec3f d1_neg(other.axes.col(1) * (-other.radius)); - Vec3f d2_pos(other.axes.col(2) * other.radius); - Vec3f d2_neg(other.axes.col(2) * (-other.radius)); + Vec3s v[16]; + Vec3s d0_pos(other.axes.col(0) * (other.length[0] + other.radius)); + Vec3s d1_pos(other.axes.col(1) * (other.length[1] + other.radius)); + Vec3s d0_neg(other.axes.col(0) * (-other.radius)); + Vec3s d1_neg(other.axes.col(1) * (-other.radius)); + Vec3s d2_pos(other.axes.col(2) * other.radius); + Vec3s d2_neg(other.axes.col(2) * (-other.radius)); v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos; v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg; @@ -940,8 +940,8 @@ RSS RSS::operator+(const RSS& other) const { v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos; v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors CoalScalar s[3] = {0, 0, 0}; getCovariance(v, NULL, NULL, NULL, 16, M); @@ -979,31 +979,31 @@ RSS RSS::operator+(const RSS& other) const { return bv; } -CoalScalar RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const { +CoalScalar RSS::distance(const RSS& other, Vec3s* P, Vec3s* Q) const { // compute what transform [R,T] that takes us from cs1 to cs2. // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] // First compute the rotation part, then translation part - Matrix3f R(axes.transpose() * other.axes); - Vec3f T(axes.transpose() * (other.Tr - Tr)); + Matrix3s R(axes.transpose() * other.axes); + Vec3s T(axes.transpose() * (other.Tr - Tr)); CoalScalar dist = rectDistance(R, T, length, other.length, P, Q); dist -= (radius + other.radius); return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist; } -CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, - const RSS& b2, Vec3f* P, Vec3f* Q) { - Matrix3f R(b1.axes.transpose() * R0 * b2.axes); - Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr); +CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, + const RSS& b2, Vec3s* P, Vec3s* Q) { + Matrix3s R(b1.axes.transpose() * R0 * b2.axes); + Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr); - Vec3f T(b1.axes.transpose() * Ttemp); + Vec3s T(b1.axes.transpose() * Ttemp); CoalScalar dist = rectDistance(R, T, b1.length, b2.length, P, Q); dist -= (b1.radius + b2.radius); return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist; } -RSS translate(const RSS& bv, const Vec3f& t) { +RSS translate(const RSS& bv, const Vec3s& t) { RSS res(bv); res.Tr += t; return res; diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp index cc581a84d..d59ccc6b9 100644 --- a/src/BV/kDOP.cpp +++ b/src/BV/kDOP.cpp @@ -63,11 +63,11 @@ inline void minmax(CoalScalar p, CoalScalar& minv, CoalScalar& maxv) { /// @brief Compute the distances to planes with normals from KDOP vectors except /// those of AABB face planes template -void getDistances(const Vec3f& /*p*/, CoalScalar* /*d*/) {} +void getDistances(const Vec3s& /*p*/, CoalScalar* /*d*/) {} /// @brief Specification of getDistances template <> -inline void getDistances<5>(const Vec3f& p, CoalScalar* d) { +inline void getDistances<5>(const Vec3s& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -76,7 +76,7 @@ inline void getDistances<5>(const Vec3f& p, CoalScalar* d) { } template <> -inline void getDistances<6>(const Vec3f& p, CoalScalar* d) { +inline void getDistances<6>(const Vec3s& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -86,7 +86,7 @@ inline void getDistances<6>(const Vec3f& p, CoalScalar* d) { } template <> -inline void getDistances<9>(const Vec3f& p, CoalScalar* d) { +inline void getDistances<9>(const Vec3s& p, CoalScalar* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; @@ -106,7 +106,7 @@ KDOP::KDOP() { } template -KDOP::KDOP(const Vec3f& v) { +KDOP::KDOP(const Vec3s& v) { for (short i = 0; i < 3; ++i) { dist_[i] = dist_[N / 2 + i] = v[i]; } @@ -119,7 +119,7 @@ KDOP::KDOP(const Vec3f& v) { } template -KDOP::KDOP(const Vec3f& a, const Vec3f& b) { +KDOP::KDOP(const Vec3s& a, const Vec3s& b) { for (short i = 0; i < 3; ++i) { minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); } @@ -168,7 +168,7 @@ bool KDOP::overlap(const KDOP& other, const CollisionRequest& request, } template -bool KDOP::inside(const Vec3f& p) const { +bool KDOP::inside(const Vec3s& p) const { if ((p.array() < dist_.template head<3>()).any()) return false; if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false; @@ -183,7 +183,7 @@ bool KDOP::inside(const Vec3f& p) const { } template -KDOP& KDOP::operator+=(const Vec3f& p) { +KDOP& KDOP::operator+=(const Vec3s& p) { for (short i = 0; i < 3; ++i) { minmax(p[i], dist_[i], dist_[N / 2 + i]); } @@ -213,14 +213,14 @@ KDOP KDOP::operator+(const KDOP& other) const { } template -CoalScalar KDOP::distance(const KDOP& /*other*/, Vec3f* /*P*/, - Vec3f* /*Q*/) const { +CoalScalar KDOP::distance(const KDOP& /*other*/, Vec3s* /*P*/, + Vec3s* /*Q*/) const { std::cerr << "KDOP distance not implemented!" << std::endl; return 0.0; } template -KDOP translate(const KDOP& bv, const Vec3f& t) { +KDOP translate(const KDOP& bv, const Vec3s& t) { KDOP res(bv); for (short i = 0; i < 3; ++i) { res.dist(i) += t[i]; @@ -241,8 +241,8 @@ template class KDOP<16>; template class KDOP<18>; template class KDOP<24>; -template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&); -template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&); -template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&); +template KDOP<16> translate<16>(const KDOP<16>&, const Vec3s&); +template KDOP<18> translate<18>(const KDOP<18>&, const Vec3s&); +template KDOP<24> translate<24>(const KDOP<24>&, const Vec3s&); } // namespace coal diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp index 10e9d7fd1..802496e08 100644 --- a/src/BV/kIOS.cpp +++ b/src/BV/kIOS.cpp @@ -73,7 +73,7 @@ bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, return obb.overlap(other.obb, request, sqrDistLowerBound); } -bool kIOS::contain(const Vec3f& p) const { +bool kIOS::contain(const Vec3s& p) const { for (unsigned int i = 0; i < num_spheres; ++i) { CoalScalar r = spheres[i].r; if ((spheres[i].o - p).squaredNorm() > r * r) return false; @@ -82,7 +82,7 @@ bool kIOS::contain(const Vec3f& p) const { return true; } -kIOS& kIOS::operator+=(const Vec3f& p) { +kIOS& kIOS::operator+=(const Vec3s& p) { for (unsigned int i = 0; i < num_spheres; ++i) { CoalScalar r = spheres[i].r; CoalScalar new_r_sqr = (p - spheres[i].o).squaredNorm(); @@ -119,7 +119,7 @@ CoalScalar kIOS::volume() const { return obb.volume(); } CoalScalar kIOS::size() const { return volume(); } -CoalScalar kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { +CoalScalar kIOS::distance(const kIOS& other, Vec3s* P, Vec3s* Q) const { CoalScalar d_max = 0; long id_a = -1, id_b = -1; for (unsigned int i = 0; i < num_spheres; ++i) { @@ -138,7 +138,7 @@ CoalScalar kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { if (P && Q) { if (id_a != -1 && id_b != -1) { - const Vec3f v = spheres[id_a].o - spheres[id_b].o; + const Vec3s v = spheres[id_a].o - spheres[id_b].o; CoalScalar len_v = v.norm(); *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); @@ -148,7 +148,7 @@ CoalScalar kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { return d_max; } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { @@ -162,7 +162,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, return b1.overlap(b2_temp); } -bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, +bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, const kIOS& b2, const CollisionRequest& request, CoalScalar& sqrDistLowerBound) { kIOS b2_temp = b2; @@ -177,8 +177,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, return b1.overlap(b2_temp, request, sqrDistLowerBound); } -CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, - const kIOS& b2, Vec3f* P, Vec3f* Q) { +CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, + const kIOS& b2, Vec3s* P, Vec3s* Q) { kIOS b2_temp = b2; for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; @@ -187,7 +187,7 @@ CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, return b1.distance(b2_temp, P, Q); } -kIOS translate(const kIOS& bv, const Vec3f& t) { +kIOS translate(const kIOS& bv, const Vec3s& t) { kIOS res(bv); for (size_t i = 0; i < res.num_spheres; ++i) { res.spheres[i].o += t; diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp index 807549e69..9cfdcab99 100644 --- a/src/BVH/BVH_model.cpp +++ b/src/BVH/BVH_model.cpp @@ -66,7 +66,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other) num_tris_allocated(other.num_tris), num_vertices_allocated(other.num_vertices) { if (other.vertices.get() && other.vertices->size() > 0) { - vertices.reset(new std::vector(*(other.vertices))); + vertices.reset(new std::vector(*(other.vertices))); } else vertices.reset(); @@ -76,7 +76,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other) tri_indices.reset(); if (other.prev_vertices.get() && other.prev_vertices->size() > 0) { - prev_vertices.reset(new std::vector(*(other.prev_vertices))); + prev_vertices.reset(new std::vector(*(other.prev_vertices))); } else prev_vertices.reset(); } @@ -105,8 +105,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const { (vertices.get() && !(other.vertices.get()))) return false; if (vertices.get() && other.vertices.get()) { - const std::vector& vertices_ = *(vertices); - const std::vector& other_vertices_ = *(other.vertices); + const std::vector& vertices_ = *(vertices); + const std::vector& other_vertices_ = *(other.vertices); for (size_t k = 0; k < static_cast(num_vertices); ++k) if (vertices_[k] != other_vertices_[k]) return false; } @@ -115,8 +115,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const { (prev_vertices.get() && !(other.prev_vertices.get()))) return false; if (prev_vertices.get() && other.prev_vertices.get()) { - const std::vector& prev_vertices_ = *(prev_vertices); - const std::vector& other_prev_vertices_ = *(other.prev_vertices); + const std::vector& prev_vertices_ = *(prev_vertices); + const std::vector& other_prev_vertices_ = *(other.prev_vertices); for (size_t k = 0; k < static_cast(num_vertices); ++k) { if (prev_vertices_[k] != other_prev_vertices_[k]) return false; } @@ -140,10 +140,10 @@ void BVHModelBase::buildConvexRepresentation(bool share_memory) { } if (!convex) { - std::shared_ptr> points = vertices; + std::shared_ptr> points = vertices; std::shared_ptr> polygons = tri_indices; if (!share_memory) { - points.reset(new std::vector(*(vertices))); + points.reset(new std::vector(*(vertices))); polygons.reset(new std::vector(*(tri_indices))); } convex.reset( @@ -206,7 +206,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_, tri_indices.reset(); if (num_vertices_allocated > 0) { - vertices.reset(new std::vector(num_vertices_allocated)); + vertices.reset(new std::vector(num_vertices_allocated)); if (!(vertices.get())) { std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" @@ -232,7 +232,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_, return BVH_OK; } -int BVHModelBase::addVertex(const Vec3f& p) { +int BVHModelBase::addVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() " "was ignored. Must do a beginModel() to clear the model for " @@ -242,8 +242,8 @@ int BVHModelBase::addVertex(const Vec3f& p) { } if (num_vertices >= num_vertices_allocated) { - std::shared_ptr> temp( - new std::vector(num_vertices_allocated * 2)); + std::shared_ptr> temp( + new std::vector(num_vertices_allocated * 2)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" @@ -304,7 +304,7 @@ int BVHModelBase::addTriangles(const Matrixx3i& triangles) { return BVH_OK; } -int BVHModelBase::addVertices(const Matrixx3f& points) { +int BVHModelBase::addVertices(const MatrixX3s& points) { if (build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call addVertex() in a wrong order. " "addVertices() was ignored. Must do a beginModel() to clear " @@ -315,8 +315,8 @@ int BVHModelBase::addVertices(const Matrixx3f& points) { if (num_vertices + points.rows() > num_vertices_allocated) { num_vertices_allocated = num_vertices + (unsigned int)points.rows(); - std::shared_ptr> temp( - new std::vector(num_vertices_allocated)); + std::shared_ptr> temp( + new std::vector(num_vertices_allocated)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" @@ -330,15 +330,15 @@ int BVHModelBase::addVertices(const Matrixx3f& points) { vertices = temp; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (Eigen::DenseIndex id = 0; id < points.rows(); ++id) vertices_[num_vertices++] = points.row(id).transpose(); return BVH_OK; } -int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, - const Vec3f& p3) { +int BVHModelBase::addTriangle(const Vec3s& p1, const Vec3s& p2, + const Vec3s& p3) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Call addTriangle() in a wrong order. " "addTriangle() was ignored. Must do a beginModel() to clear " @@ -348,8 +348,8 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, } if (num_vertices + 2 >= num_vertices_allocated) { - std::shared_ptr> temp( - new std::vector(num_vertices_allocated * 2 + 2)); + std::shared_ptr> temp( + new std::vector(num_vertices_allocated * 2 + 2)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " "addTriangle() call!" @@ -398,7 +398,7 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2, return BVH_OK; } -int BVHModelBase::addSubModel(const std::vector& ps) { +int BVHModelBase::addSubModel(const std::vector& ps) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. " "addSubModel() was ignored. Must do a beginModel() to clear " @@ -410,7 +410,7 @@ int BVHModelBase::addSubModel(const std::vector& ps) { const unsigned int num_vertices_to_add = (unsigned int)ps.size(); if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { - std::shared_ptr> temp(new std::vector( + std::shared_ptr> temp(new std::vector( num_vertices_allocated * 2 + num_vertices_to_add - 1)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " @@ -427,7 +427,7 @@ int BVHModelBase::addSubModel(const std::vector& ps) { num_vertices_allocated * 2 + num_vertices_to_add - 1; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices_[num_vertices] = ps[i]; num_vertices++; @@ -436,7 +436,7 @@ int BVHModelBase::addSubModel(const std::vector& ps) { return BVH_OK; } -int BVHModelBase::addSubModel(const std::vector& ps, +int BVHModelBase::addSubModel(const std::vector& ps, const std::vector& ts) { if (build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. " @@ -449,7 +449,7 @@ int BVHModelBase::addSubModel(const std::vector& ps, const unsigned int num_vertices_to_add = (unsigned int)ps.size(); if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { - std::shared_ptr> temp(new std::vector( + std::shared_ptr> temp(new std::vector( num_vertices_allocated * 2 + num_vertices_to_add - 1)); if (!(temp.get())) { std::cerr << "BVH Error! Out of memory for vertices array on " @@ -468,7 +468,7 @@ int BVHModelBase::addSubModel(const std::vector& ps, const unsigned int offset = num_vertices; - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) { vertices_[num_vertices] = ps[i]; num_vertices++; @@ -543,8 +543,8 @@ int BVHModelBase::endModel() { if (num_vertices_allocated > num_vertices) { if (num_vertices > 0) { - std::shared_ptr> new_vertices( - new std::vector(num_vertices)); + std::shared_ptr> new_vertices( + new std::vector(num_vertices)); if (!(new_vertices.get())) { std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" @@ -591,7 +591,7 @@ int BVHModelBase::beginReplaceModel() { return BVH_OK; } -int BVHModelBase::replaceVertex(const Vec3f& p) { +int BVHModelBase::replaceVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. " "replaceVertex() was ignored. Must do a beginReplaceModel() " @@ -606,8 +606,8 @@ int BVHModelBase::replaceVertex(const Vec3f& p) { return BVH_OK; } -int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, - const Vec3f& p3) { +int BVHModelBase::replaceTriangle(const Vec3s& p1, const Vec3s& p2, + const Vec3s& p3) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. " "replaceTriangle() was ignored. Must do a beginReplaceModel() " @@ -625,7 +625,7 @@ int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2, return BVH_OK; } -int BVHModelBase::replaceSubModel(const std::vector& ps) { +int BVHModelBase::replaceSubModel(const std::vector& ps) { if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. " "replaceSubModel() was ignored. Must do a beginReplaceModel() " @@ -634,7 +634,7 @@ int BVHModelBase::replaceSubModel(const std::vector& ps) { return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < ps.size(); ++i) { vertices_[num_vertex_updated] = ps[i]; num_vertex_updated++; @@ -680,12 +680,12 @@ int BVHModelBase::beginUpdateModel() { } if (prev_vertices.get()) { - std::shared_ptr> temp = prev_vertices; + std::shared_ptr> temp = prev_vertices; prev_vertices = vertices; vertices = temp; } else { prev_vertices = vertices; - vertices.reset(new std::vector(num_vertices)); + vertices.reset(new std::vector(num_vertices)); } num_vertex_updated = 0; @@ -695,7 +695,7 @@ int BVHModelBase::beginUpdateModel() { return BVH_OK; } -int BVHModelBase::updateVertex(const Vec3f& p) { +int BVHModelBase::updateVertex(const Vec3s& p) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() " @@ -710,8 +710,8 @@ int BVHModelBase::updateVertex(const Vec3f& p) { return BVH_OK; } -int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, - const Vec3f& p3) { +int BVHModelBase::updateTriangle(const Vec3s& p1, const Vec3s& p2, + const Vec3s& p3) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. " "updateTriangle() was ignored. Must do a beginUpdateModel() " @@ -729,7 +729,7 @@ int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2, return BVH_OK; } -int BVHModelBase::updateSubModel(const std::vector& ps) { +int BVHModelBase::updateSubModel(const std::vector& ps) { if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. " "updateSubModel() was ignored. Must do a beginUpdateModel() " @@ -738,7 +738,7 @@ int BVHModelBase::updateSubModel(const std::vector& ps) { return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } - std::vector& vertices_ = *vertices; + std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < ps.size(); ++i) { vertices_[num_vertex_updated] = ps[i]; num_vertex_updated++; @@ -780,7 +780,7 @@ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) { void BVHModelBase::computeLocalAABB() { AABB aabb_; - const std::vector& vertices_ = *vertices; + const std::vector& vertices_ = *vertices; for (unsigned int i = 0; i < num_vertices; ++i) { aabb_ += vertices_[i]; } @@ -840,7 +840,7 @@ template int BVHModel::memUsage(const bool msg) const { unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs; unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris; - unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3f) * num_vertices; + unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3s) * num_vertices; unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + (unsigned int)sizeof(BVHModel); @@ -857,7 +857,7 @@ int BVHModel::memUsage(const bool msg) const { template int BVHModel::buildTree() { // set BVFitter - Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL; + Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL; Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL; bv_fitter->set(vertices_, tri_indices_, getModelType()); // set SplitRule @@ -911,17 +911,17 @@ int BVHModel::recursiveBuildTree(int bv_id, unsigned int first_primitive, num_bvs += 2; unsigned int c1 = 0; - const std::vector& vertices_ = *vertices; + const std::vector& vertices_ = *vertices; const std::vector& tri_indices_ = *tri_indices; for (unsigned int i = 0; i < num_primitives; ++i) { - Vec3f p; + Vec3s p; if (type == BVH_MODEL_POINTCLOUD) p = vertices_[cur_primitive_indices[i]]; else if (type == BVH_MODEL_TRIANGLES) { const Triangle& t = tri_indices_[cur_primitive_indices[i]]; - const Vec3f& p1 = vertices_[t[0]]; - const Vec3f& p2 = vertices_[t[1]]; - const Vec3f& p3 = vertices_[t[2]]; + const Vec3s& p1 = vertices_[t[0]]; + const Vec3s& p2 = vertices_[t[1]]; + const Vec3s& p3 = vertices_[t[2]]; p = (p1 + p2 + p3) / 3.; } else { @@ -989,7 +989,7 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { BV bv; if (prev_vertices.get()) { - Vec3f v[2]; + Vec3s v[2]; v[0] = (*prev_vertices)[static_cast(primitive_id)]; v[1] = (*vertices)[static_cast(primitive_id)]; fit(v, 2, bv); @@ -1003,7 +1003,7 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { (*tri_indices)[static_cast(primitive_id)]; if (prev_vertices.get()) { - Vec3f v[6]; + Vec3s v[6]; for (Triangle::index_type i = 0; i < 3; ++i) { v[i] = (*prev_vertices)[triangle[i]]; v[i + 3] = (*vertices)[triangle[i]]; @@ -1015,7 +1015,7 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { // unsigned int* cur_primitive_indices = primitive_indices + // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices, // bvnode->num_primitives); - Vec3f v[3]; + Vec3s v[3]; for (int i = 0; i < 3; ++i) { v[i] = (*vertices)[triangle[(Triangle::index_type)i]]; } @@ -1044,8 +1044,8 @@ int BVHModel::recursiveRefitTree_bottomup(int bv_id) { template int BVHModel::refitTree_topdown() { - Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL; - Vec3f* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL; + Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL; + Vec3s* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL; Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL; bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType()); BVNode* bvs_ = bvs->data(); @@ -1062,8 +1062,8 @@ int BVHModel::refitTree_topdown() { } template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c) { +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; OBB& obb = bvs_[static_cast(bv_id)].bv; if (!bvs_[static_cast(bv_id)].isLeaf()) { @@ -1078,13 +1078,13 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, // obb.axes = parent_axes.transpose() * obb.axes; obb.axes.applyOnTheLeft(parent_axes.transpose()); - Vec3f t(obb.To - parent_c); + Vec3s t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; } template <> -void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, - const Vec3f& parent_c) { +void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes, + const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; RSS& rss = bvs_[static_cast(bv_id)].bv; if (!bvs_[static_cast(bv_id)].isLeaf()) { @@ -1099,14 +1099,14 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, // rss.axes = parent_axes.transpose() * rss.axes; rss.axes.applyOnTheLeft(parent_axes.transpose()); - Vec3f t(rss.Tr - parent_c); + Vec3s t(rss.Tr - parent_c); rss.Tr.noalias() = parent_axes.transpose() * t; } template <> void BVHModel::makeParentRelativeRecurse(int bv_id, - Matrix3f& parent_axes, - const Vec3f& parent_c) { + Matrix3s& parent_axes, + const Vec3s& parent_c) { bv_node_vector_t& bvs_ = *bvs; OBB& obb = bvs_[static_cast(bv_id)].bv.obb; RSS& rss = bvs_[static_cast(bv_id)].bv.rss; @@ -1122,7 +1122,7 @@ void BVHModel::makeParentRelativeRecurse(int bv_id, rss.axes.noalias() = parent_axes.transpose() * obb.axes; obb.axes = rss.axes; - Vec3f t(obb.To - parent_c); + Vec3s t(obb.To - parent_c); obb.To.noalias() = parent_axes.transpose() * t; rss.Tr = obb.To; } diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp index 9a9194310..5ce7c2ccd 100644 --- a/src/BVH/BVH_utility.cpp +++ b/src/BVH/BVH_utility.cpp @@ -47,7 +47,7 @@ template BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, const AABB& _aabb) { assert(model.getModelType() == BVH_MODEL_TRIANGLES); - const Matrix3f& q = pose.getRotation(); + const Matrix3s& q = pose.getRotation(); AABB aabb = translate(_aabb, -pose.getTranslation()); Transform3f box_pose; @@ -64,7 +64,7 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, std::vector keep_vertex(model.num_vertices, false); std::vector keep_tri(model.num_tris, false); unsigned int ntri = 0; - const std::vector& model_vertices_ = *(model.vertices); + const std::vector& model_vertices_ = *(model.vertices); const std::vector& model_tri_indices_ = *(model.tri_indices); for (unsigned int i = 0; i < model.num_tris; ++i) { const Triangle& t = model_tri_indices_[i]; @@ -111,7 +111,7 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices)); std::vector idxConversion(model.num_vertices); assert(new_model->num_vertices == 0); - std::vector& new_model_vertices_ = *(new_model->vertices); + std::vector& new_model_vertices_ = *(new_model->vertices); for (unsigned int i = 0; i < keep_vertex.size(); ++i) { if (keep_vertex[i]) { idxConversion[i] = new_model->num_vertices; @@ -179,18 +179,18 @@ BVHModel >* BVHExtract(const BVHModel >& model, return details::BVHExtract(model, pose, aabb); } -void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, - unsigned int n, Matrix3f& M) { - Vec3f S1(Vec3f::Zero()); - Vec3f S2[3] = {Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero()}; +void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, + unsigned int n, Matrix3s& M) { + Vec3s S1(Vec3s::Zero()); + Vec3s S2[3] = {Vec3s::Zero(), Vec3s::Zero(), Vec3s::Zero()}; if (ts) { for (unsigned int i = 0; i < n; ++i) { const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; - const Vec3f& p1 = ps[t[0]]; - const Vec3f& p2 = ps[t[1]]; - const Vec3f& p3 = ps[t[2]]; + const Vec3s& p1 = ps[t[0]]; + const Vec3s& p2 = ps[t[1]]; + const Vec3s& p3 = ps[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); @@ -203,9 +203,9 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); if (ps2) { - const Vec3f& p1 = ps2[t[0]]; - const Vec3f& p2 = ps2[t[1]]; - const Vec3f& p3 = ps2[t[2]]; + const Vec3s& p1 = ps2[t[0]]; + const Vec3s& p2 = ps2[t[1]]; + const Vec3s& p3 = ps2[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); @@ -221,7 +221,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, } } else { for (unsigned int i = 0; i < n; ++i) { - const Vec3f& p = (indices) ? ps[indices[i]] : ps[i]; + const Vec3s& p = (indices) ? ps[indices[i]] : ps[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); @@ -232,7 +232,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, if (ps2) // another frame { - const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i]; + const Vec3s& p = (indices) ? ps2[indices[i]] : ps2[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); @@ -260,9 +260,9 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, /** @brief Compute the RSS bounding volume parameters: radius, rectangle size * and the origin. The bounding volume axes are known. */ -void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, +void getRadiusAndOriginAndRectangleSize(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Matrix3f& axes, Vec3f& origin, + const Matrix3s& axes, Vec3s& origin, CoalScalar l[2], CoalScalar& r) { bool indirect_index = true; if (!indices) indirect_index = false; @@ -280,8 +280,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps[point_id]; - Vec3f v(p[0], p[1], p[2]); + const Vec3s& p = ps[point_id]; + Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); @@ -291,9 +291,9 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, if (ps2) { for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps2[point_id]; + const Vec3s& p = ps2[point_id]; // FIXME Is this right ????? - Vec3f v(p[0], p[1], p[2]); + Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(0).dot(v); P[P_id][2] = axes.col(1).dot(v); @@ -305,15 +305,15 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; - const Vec3f& p = ps[index]; - Vec3f v(p[0], p[1], p[2]); + const Vec3s& p = ps[index]; + Vec3s v(p[0], p[1], p[2]); P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); P_id++; if (ps2) { - const Vec3f& v = ps2[index]; + const Vec3s& v = ps2[index]; P[P_id][0] = axes.col(0).dot(v); P[P_id][1] = axes.col(1).dot(v); P[P_id][2] = axes.col(2).dot(v); @@ -472,7 +472,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - origin.noalias() = axes * Vec3f(minx, miny, cz); + origin.noalias() = axes * Vec3s(minx, miny, cz); l[0] = std::max(maxx - minx, 0); l[1] = std::max(maxy - miny, 0); @@ -483,23 +483,23 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, /** @brief Compute the bounding volume extent and center for a set or subset of * points. The bounding volume axes are known. */ -static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, +static inline void getExtentAndCenter_pointcloud(Vec3s* ps, Vec3s* ps2, unsigned int* indices, - unsigned int n, Matrix3f& axes, - Vec3f& center, Vec3f& extent) { + unsigned int n, Matrix3s& axes, + Vec3s& center, Vec3s& extent) { bool indirect_index = true; if (!indices) indirect_index = false; CoalScalar real_max = (std::numeric_limits::max)(); - Vec3f min_coord(real_max, real_max, real_max); - Vec3f max_coord(-real_max, -real_max, -real_max); + Vec3s min_coord(real_max, real_max, real_max); + Vec3s max_coord(-real_max, -real_max, -real_max); for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; - const Vec3f& p = ps[index]; - Vec3f proj(axes.transpose() * p); + const Vec3s& p = ps[index]; + Vec3s proj(axes.transpose() * p); for (int j = 0; j < 3; ++j) { if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; @@ -507,7 +507,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, } if (ps2) { - const Vec3f& v = ps2[index]; + const Vec3s& v = ps2[index]; proj.noalias() = axes.transpose() * v; for (int j = 0; j < 3; ++j) { @@ -525,17 +525,17 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, /** @brief Compute the bounding volume extent and center for a set or subset of * points. The bounding volume axes are known. */ -static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, +static inline void getExtentAndCenter_mesh(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, - unsigned int n, Matrix3f& axes, - Vec3f& center, Vec3f& extent) { + unsigned int n, Matrix3s& axes, + Vec3s& center, Vec3s& extent) { bool indirect_index = true; if (!indices) indirect_index = false; CoalScalar real_max = (std::numeric_limits::max)(); - Vec3f min_coord(real_max, real_max, real_max); - Vec3f max_coord(-real_max, -real_max, -real_max); + Vec3s min_coord(real_max, real_max, real_max); + Vec3s max_coord(-real_max, -real_max, -real_max); for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; @@ -543,8 +543,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps[point_id]; - Vec3f proj(axes.transpose() * p); + const Vec3s& p = ps[point_id]; + Vec3s proj(axes.transpose() * p); for (int k = 0; k < 3; ++k) { if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; @@ -555,8 +555,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, if (ps2) { for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps2[point_id]; - Vec3f proj(axes.transpose() * p); + const Vec3s& p = ps2[point_id]; + Vec3s proj(axes.transpose() * p); for (int k = 0; k < 3; ++k) { if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; @@ -566,29 +566,29 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, } } - Vec3f o((max_coord + min_coord) / 2); + Vec3s o((max_coord + min_coord) / 2); center.noalias() = axes * o; extent.noalias() = (max_coord - min_coord) / 2; } -void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, - unsigned int* indices, unsigned int n, Matrix3f& axes, - Vec3f& center, Vec3f& extent) { +void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts, + unsigned int* indices, unsigned int n, Matrix3s& axes, + Vec3s& center, Vec3s& extent) { if (ts) getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent); else getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent); } -void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, - Vec3f& center, CoalScalar& radius) { - Vec3f e1 = a - c; - Vec3f e2 = b - c; +void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c, + Vec3s& center, CoalScalar& radius) { + Vec3s e1 = a - c; + Vec3s e2 = b - c; CoalScalar e1_len2 = e1.squaredNorm(); CoalScalar e2_len2 = e2.squaredNorm(); - Vec3f e3 = e1.cross(e2); + Vec3s e3 = e1.cross(e2); CoalScalar e3_len2 = e3.squaredNorm(); radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; radius = std::sqrt(radius) * 0.5; @@ -596,11 +596,11 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; } -static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, +static inline CoalScalar maximumDistance_mesh(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Vec3f& query) { + const Vec3s& query) { bool indirect_index = true; if (!indices) indirect_index = false; @@ -611,7 +611,7 @@ static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps[point_id]; + const Vec3s& p = ps[point_id]; CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; @@ -620,7 +620,7 @@ static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, if (ps2) { for (Triangle::index_type j = 0; j < 3; ++j) { Triangle::index_type point_id = t[j]; - const Vec3f& p = ps2[point_id]; + const Vec3s& p = ps2[point_id]; CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; @@ -631,10 +631,10 @@ static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, return std::sqrt(maxD); } -static inline CoalScalar maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, +static inline CoalScalar maximumDistance_pointcloud(Vec3s* ps, Vec3s* ps2, unsigned int* indices, unsigned int n, - const Vec3f& query) { + const Vec3s& query) { bool indirect_index = true; if (!indices) indirect_index = false; @@ -642,12 +642,12 @@ static inline CoalScalar maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, for (unsigned int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; - const Vec3f& p = ps[index]; + const Vec3s& p = ps[index]; CoalScalar d = (p - query).squaredNorm(); if (d > maxD) maxD = d; if (ps2) { - const Vec3f& v = ps2[index]; + const Vec3s& v = ps2[index]; CoalScalar d = (v - query).squaredNorm(); if (d > maxD) maxD = d; } @@ -656,9 +656,9 @@ static inline CoalScalar maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, return std::sqrt(maxD); } -CoalScalar maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, +CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n, - const Vec3f& query) { + const Vec3s& query) { if (ts) return maximumDistance_mesh(ps, ps2, ts, indices, n, query); else diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp index deb74700a..02a8bc8c9 100644 --- a/src/BVH/BV_fitter.cpp +++ b/src/BVH/BV_fitter.cpp @@ -47,8 +47,8 @@ static const double kIOS_RATIO = 1.5; static const double invSinA = 2; static const double cosA = sqrt(3.0) / 2.0; -static inline void axisFromEigen(Vec3f eigenV[3], CoalScalar eigenS[3], - Matrix3f& axes) { +static inline void axisFromEigen(Vec3s eigenV[3], CoalScalar eigenS[3], + Matrix3s& axes) { int min, mid, max; if (eigenS[0] > eigenS[1]) { max = 0; @@ -77,16 +77,16 @@ static inline void axisFromEigen(Vec3f eigenV[3], CoalScalar eigenS[3], namespace OBB_fit_functions { -void fit1(Vec3f* ps, OBB& bv) { +void fit1(Vec3s* ps, OBB& bv) { bv.To.noalias() = ps[0]; bv.axes.setIdentity(); bv.extent.setZero(); } -void fit2(Vec3f* ps, OBB& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - Vec3f p1p2 = p1 - p2; +void fit2(Vec3s* ps, OBB& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + Vec3s p1p2 = p1 - p2; CoalScalar len_p1p2 = p1p2.norm(); p1p2.normalize(); @@ -97,11 +97,11 @@ void fit2(Vec3f* ps, OBB& bv) { bv.To.noalias() = (p1 + p2) / 2; } -void fit3(Vec3f* ps, OBB& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - const Vec3f& p3 = ps[2]; - Vec3f e[3]; +void fit3(Vec3s* ps, OBB& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + const Vec3s& p3 = ps[2]; + Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; @@ -121,16 +121,16 @@ void fit3(Vec3f* ps, OBB& bv) { getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent); } -void fit6(Vec3f* ps, OBB& bv) { +void fit6(Vec3s* ps, OBB& bv) { OBB bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } -void fitn(Vec3f* ps, unsigned int n, OBB& bv) { - Matrix3f M; - Vec3f E[3]; +void fitn(Vec3s* ps, unsigned int n, OBB& bv) { + Matrix3s M; + Vec3s E[3]; CoalScalar s[3] = {0, 0, 0}; // three eigen values getCovariance(ps, NULL, NULL, NULL, n, M); @@ -144,7 +144,7 @@ void fitn(Vec3f* ps, unsigned int n, OBB& bv) { } // namespace OBB_fit_functions namespace RSS_fit_functions { -void fit1(Vec3f* ps, RSS& bv) { +void fit1(Vec3s* ps, RSS& bv) { bv.Tr.noalias() = ps[0]; bv.axes.setIdentity(); bv.length[0] = 0; @@ -152,9 +152,9 @@ void fit1(Vec3f* ps, RSS& bv) { bv.radius = 0; } -void fit2(Vec3f* ps, RSS& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; +void fit2(Vec3s* ps, RSS& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; bv.axes.col(0).noalias() = p1 - p2; CoalScalar len_p1p2 = bv.axes.col(0).norm(); bv.axes.col(0) /= len_p1p2; @@ -167,11 +167,11 @@ void fit2(Vec3f* ps, RSS& bv) { bv.radius = 0; } -void fit3(Vec3f* ps, RSS& bv) { - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - const Vec3f& p3 = ps[2]; - Vec3f e[3]; +void fit3(Vec3s* ps, RSS& bv) { + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + const Vec3s& p3 = ps[2]; + Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; @@ -192,16 +192,16 @@ void fit3(Vec3f* ps, RSS& bv) { bv.length, bv.radius); } -void fit6(Vec3f* ps, RSS& bv) { +void fit6(Vec3s* ps, RSS& bv) { RSS bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } -void fitn(Vec3f* ps, unsigned int n, RSS& bv) { - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors +void fitn(Vec3s* ps, unsigned int n, RSS& bv) { + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors CoalScalar s[3] = {0, 0, 0}; getCovariance(ps, NULL, NULL, NULL, n, M); @@ -217,7 +217,7 @@ void fitn(Vec3f* ps, unsigned int n, RSS& bv) { namespace kIOS_fit_functions { -void fit1(Vec3f* ps, kIOS& bv) { +void fit1(Vec3s* ps, kIOS& bv) { bv.num_spheres = 1; bv.spheres[0].o.noalias() = ps[0]; bv.spheres[0].r = 0; @@ -227,16 +227,16 @@ void fit1(Vec3f* ps, kIOS& bv) { bv.obb.To.noalias() = ps[0]; } -void fit2(Vec3f* ps, kIOS& bv) { +void fit2(Vec3s* ps, kIOS& bv) { bv.num_spheres = 5; - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - Vec3f p1p2 = p1 - p2; + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + Vec3s p1p2 = p1 - p2; CoalScalar len_p1p2 = p1p2.norm(); p1p2.normalize(); - Matrix3f& axes = bv.obb.axes; + Matrix3s& axes = bv.obb.axes; axes.col(0).noalias() = p1p2; generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2)); @@ -251,7 +251,7 @@ void fit2(Vec3f* ps, kIOS& bv) { CoalScalar r1cosA = r1 * cosA; bv.spheres[1].r = r1; bv.spheres[2].r = r1; - Vec3f delta = axes.col(1) * r1cosA; + Vec3s delta = axes.col(1) * r1cosA; bv.spheres[1].o = bv.spheres[0].o - delta; bv.spheres[2].o = bv.spheres[0].o + delta; @@ -262,13 +262,13 @@ void fit2(Vec3f* ps, kIOS& bv) { bv.spheres[4].o = bv.spheres[0].o + delta; } -void fit3(Vec3f* ps, kIOS& bv) { +void fit3(Vec3s* ps, kIOS& bv) { bv.num_spheres = 3; - const Vec3f& p1 = ps[0]; - const Vec3f& p2 = ps[1]; - const Vec3f& p3 = ps[2]; - Vec3f e[3]; + const Vec3s& p1 = ps[0]; + const Vec3s& p2 = ps[1]; + const Vec3s& p3 = ps[2]; + Vec3s e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; @@ -290,14 +290,14 @@ void fit3(Vec3f* ps, kIOS& bv) { // compute radius and center CoalScalar r0; - Vec3f center; + Vec3s center; circumCircleComputation(p1, p2, p3, center, r0); bv.spheres[0].o = center; bv.spheres[0].r = r0; CoalScalar r1 = r0 * invSinA; - Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA); + Vec3s delta = bv.obb.axes.col(2) * (r1 * cosA); bv.spheres[1].r = r1; bv.spheres[1].o = center - delta; @@ -305,22 +305,22 @@ void fit3(Vec3f* ps, kIOS& bv) { bv.spheres[2].o = center + delta; } -void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { - Matrix3f M; - Vec3f E[3]; +void fitn(Vec3s* ps, unsigned int n, kIOS& bv) { + Matrix3s M; + Vec3s E[3]; CoalScalar s[3] = {0, 0, 0}; // three eigen values; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); - Matrix3f& axes = bv.obb.axes; + Matrix3s& axes = bv.obb.axes; axisFromEigen(E, s, axes); getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent); // get center and extension - const Vec3f& center = bv.obb.To; - const Vec3f& extent = bv.obb.extent; + const Vec3s& center = bv.obb.To; + const Vec3s& extent = bv.obb.extent; CoalScalar r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); // decide the k in kIOS @@ -337,7 +337,7 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { if (bv.num_spheres >= 3) { CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; - Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); + Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; @@ -353,7 +353,7 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { if (bv.num_spheres >= 5) { CoalScalar r10 = bv.spheres[1].r; - Vec3f delta = + Vec3s delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); @@ -375,22 +375,22 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { } // namespace kIOS_fit_functions namespace OBBRSS_fit_functions { -void fit1(Vec3f* ps, OBBRSS& bv) { +void fit1(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit1(ps, bv.obb); RSS_fit_functions::fit1(ps, bv.rss); } -void fit2(Vec3f* ps, OBBRSS& bv) { +void fit2(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit2(ps, bv.obb); RSS_fit_functions::fit2(ps, bv.rss); } -void fit3(Vec3f* ps, OBBRSS& bv) { +void fit3(Vec3s* ps, OBBRSS& bv) { OBB_fit_functions::fit3(ps, bv.obb); RSS_fit_functions::fit3(ps, bv.rss); } -void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) { +void fitn(Vec3s* ps, unsigned int n, OBBRSS& bv) { OBB_fit_functions::fitn(ps, n, bv.obb); RSS_fit_functions::fitn(ps, n, bv.rss); } @@ -398,7 +398,7 @@ void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) { } // namespace OBBRSS_fit_functions template <> -void fit(Vec3f* ps, unsigned int n, OBB& bv) { +void fit(Vec3s* ps, unsigned int n, OBB& bv) { switch (n) { case 1: OBB_fit_functions::fit1(ps, bv); @@ -418,7 +418,7 @@ void fit(Vec3f* ps, unsigned int n, OBB& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, RSS& bv) { +void fit(Vec3s* ps, unsigned int n, RSS& bv) { switch (n) { case 1: RSS_fit_functions::fit1(ps, bv); @@ -435,7 +435,7 @@ void fit(Vec3f* ps, unsigned int n, RSS& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, kIOS& bv) { +void fit(Vec3s* ps, unsigned int n, kIOS& bv) { switch (n) { case 1: kIOS_fit_functions::fit1(ps, bv); @@ -452,7 +452,7 @@ void fit(Vec3f* ps, unsigned int n, kIOS& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) { +void fit(Vec3s* ps, unsigned int n, OBBRSS& bv) { switch (n) { case 1: OBBRSS_fit_functions::fit1(ps, bv); @@ -469,7 +469,7 @@ void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) { } template <> -void fit(Vec3f* ps, unsigned int n, AABB& bv) { +void fit(Vec3s* ps, unsigned int n, AABB& bv) { if (n <= 0) return; bv = AABB(ps[0]); for (unsigned int i = 1; i < n; ++i) { @@ -481,8 +481,8 @@ OBB BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBB bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors CoalScalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, @@ -501,8 +501,8 @@ OBB BVFitter::fit(unsigned int* primitive_indices, OBBRSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { OBBRSS bv; - Matrix3f M; - Vec3f E[3]; + Matrix3s M; + Vec3s E[3]; CoalScalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, @@ -515,7 +515,7 @@ OBBRSS BVFitter::fit(unsigned int* primitive_indices, getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent); - Vec3f origin; + Vec3s origin; CoalScalar l[2]; CoalScalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, @@ -534,8 +534,8 @@ RSS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { RSS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors CoalScalar s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); @@ -544,7 +544,7 @@ RSS BVFitter::fit(unsigned int* primitive_indices, // set rss origin, rectangle size and radius - Vec3f origin; + Vec3s origin; CoalScalar l[2]; CoalScalar r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, @@ -563,23 +563,23 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, unsigned int num_primitives) { kIOS bv; - Matrix3f M; // row first matrix - Vec3f E[3]; // row first eigen-vectors + Matrix3s M; // row first matrix + Vec3s E[3]; // row first eigen-vectors CoalScalar s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); - Matrix3f& axes = bv.obb.axes; + Matrix3s& axes = bv.obb.axes; axisFromEigen(E, s, axes); // get centers and extensions getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axes, bv.obb.To, bv.obb.extent); - const Vec3f& center = bv.obb.To; - const Vec3f& extent = bv.obb.extent; + const Vec3s& center = bv.obb.To; + const Vec3s& extent = bv.obb.extent; CoalScalar r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center); @@ -597,7 +597,7 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, if (bv.num_spheres >= 3) { CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; - Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); + Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; @@ -617,7 +617,7 @@ kIOS BVFitter::fit(unsigned int* primitive_indices, if (bv.num_spheres >= 5) { CoalScalar r10 = bv.spheres[1].r; - Vec3f delta = + Vec3s delta = axes.col(1) * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp index 48c28433c..50d402429 100644 --- a/src/BVH/BV_splitter.cpp +++ b/src/BVH/BV_splitter.cpp @@ -40,21 +40,21 @@ namespace coal { template -void computeSplitVector(const BV& bv, Vec3f& split_vector) { +void computeSplitVector(const BV& bv, Vec3s& split_vector) { split_vector = bv.axes.col(0); } template <> -void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { +void computeSplitVector(const kIOS& bv, Vec3s& split_vector) { /* switch(bv.num_spheres) { case 1: - split_vector = Vec3f(1, 0, 0); + split_vector = Vec3s(1, 0, 0); break; case 3: { - Vec3f v[3]; + Vec3s v[3]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[0].normalize(); generateCoordinateSystem(v[0], v[1], v[2]); @@ -63,7 +63,7 @@ void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { break; case 5: { - Vec3f v[2]; + Vec3s v[2]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[1] = bv.spheres[3].o - bv.spheres[0].o; split_vector = v[0].cross(v[1]); @@ -78,30 +78,30 @@ void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { } template <> -void computeSplitVector(const OBBRSS& bv, Vec3f& split_vector) { +void computeSplitVector(const OBBRSS& bv, Vec3s& split_vector) { split_vector = bv.obb.axes.col(0); } template void computeSplitValue_bvcenter(const BV& bv, CoalScalar& split_value) { - Vec3f center = bv.center(); + Vec3s center = bv.center(); split_value = center[0]; } template -void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, +void computeSplitValue_mean(const BV&, Vec3s* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, - const Vec3f& split_vector, + const Vec3s& split_vector, CoalScalar& split_value) { if (type == BVH_MODEL_TRIANGLES) { - Vec3f c(Vec3f::Zero()); + Vec3s c(Vec3s::Zero()); for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; - const Vec3f& p1 = vertices[t[0]]; - const Vec3f& p2 = vertices[t[1]]; - const Vec3f& p3 = vertices[t[2]]; + const Vec3s& p1 = vertices[t[0]]; + const Vec3s& p2 = vertices[t[1]]; + const Vec3s& p3 = vertices[t[2]]; c += p1 + p2 + p3; } @@ -109,7 +109,7 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, } else if (type == BVH_MODEL_POINTCLOUD) { CoalScalar sum = 0; for (unsigned int i = 0; i < num_primitives; ++i) { - const Vec3f& p = vertices[primitive_indices[i]]; + const Vec3s& p = vertices[primitive_indices[i]]; sum += p.dot(split_vector); } @@ -118,28 +118,28 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles, } template -void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles, +void computeSplitValue_median(const BV&, Vec3s* vertices, Triangle* triangles, unsigned int* primitive_indices, unsigned int num_primitives, BVHModelType type, - const Vec3f& split_vector, + const Vec3s& split_vector, CoalScalar& split_value) { std::vector proj(num_primitives); if (type == BVH_MODEL_TRIANGLES) { for (unsigned int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; - const Vec3f& p1 = vertices[t[0]]; - const Vec3f& p2 = vertices[t[1]]; - const Vec3f& p3 = vertices[t[2]]; - Vec3f centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1], + const Vec3s& p1 = vertices[t[0]]; + const Vec3s& p2 = vertices[t[1]]; + const Vec3s& p3 = vertices[t[2]]; + Vec3s centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1], p1[2] + p2[2] + p3[2]); proj[i] = centroid3.dot(split_vector) / 3; } } else if (type == BVH_MODEL_POINTCLOUD) { for (unsigned int i = 0; i < num_primitives; ++i) { - const Vec3f& p = vertices[primitive_indices[i]]; - Vec3f v(p[0], p[1], p[2]); + const Vec3s& p = vertices[primitive_indices[i]]; + Vec3s v(p[0], p[1], p[2]); proj[i] = v.dot(split_vector); } } @@ -259,23 +259,23 @@ void BVSplitter::computeRule_median(const OBBRSS& bv, } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template <> -bool BVSplitter::apply(const Vec3f& q) const { - return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; +bool BVSplitter::apply(const Vec3s& q) const { + return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value; } template class BVSplitter; diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index ca1ba3249..a39953835 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -265,10 +265,10 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, CoalScalar& min_dist) const { static const unsigned int CUTOFF = 100; - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; - Vec3f dummy_vector = obj->getAABB().max_; + Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + Vec3s dummy_vector = obj->getAABB().max_; if (min_dist < (std::numeric_limits::max)()) - dummy_vector += Vec3f(min_dist, min_dist, min_dist); + dummy_vector += Vec3s(min_dist, min_dist, min_dist); typename std::vector::const_iterator pos_start1 = objs_x.begin(); @@ -335,7 +335,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, // to check the possible missed ones to the right of the objs array if (min_dist < old_min_distance) { dummy_vector = - obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist); + obj->getAABB().max_ + Vec3s(min_dist, min_dist, min_dist); status = 0; } else // need more loop { diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 9e0d64c76..b2b27f5fe 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -292,8 +292,8 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) { const AABB current_aabb = current->obj->getAABB(); - const Vec3f& new_min = current_aabb.min_; - const Vec3f& new_max = current_aabb.max_; + const Vec3s& new_min = current_aabb.min_; + const Vec3s& new_max = current_aabb.max_; for (int coord = 0; coord < 3; ++coord) { int direction; // -1 reverse, 0 nochange, 1 forward @@ -585,11 +585,11 @@ void SaPCollisionManager::collide(CollisionObject* obj, bool SaPCollisionManager::distance_(CollisionObject* obj, DistanceCallBackBase* callback, CoalScalar& min_dist) const { - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); if (min_dist < (std::numeric_limits::max)()) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } @@ -656,7 +656,7 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, break; else { if (min_dist < old_min_distance) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { @@ -779,7 +779,7 @@ bool SaPCollisionManager::empty() const { return AABB_arr.empty(); } size_t SaPCollisionManager::size() const { return AABB_arr.size(); } //============================================================================== -const Vec3f& SaPCollisionManager::EndPoint::getVal() const { +const Vec3s& SaPCollisionManager::EndPoint::getVal() const { if (minmax) return aabb->cached.max_; else @@ -787,7 +787,7 @@ const Vec3f& SaPCollisionManager::EndPoint::getVal() const { } //============================================================================== -Vec3f& SaPCollisionManager::EndPoint::getVal() { +Vec3s& SaPCollisionManager::EndPoint::getVal() { if (minmax) return aabb->cached.max_; else diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 8a4313151..5f65e1be9 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -372,10 +372,10 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, CoalScalar& min_dist) const { static const unsigned int CUTOFF = 100; - Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; + Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); if (min_dist < (std::numeric_limits::max)()) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } @@ -429,7 +429,7 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, break; else { if (min_dist < old_min_distance) { - Vec3f min_dist_delta(min_dist, min_dist, min_dist); + Vec3s min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { diff --git a/src/broadphase/detail/morton-inl.h b/src/broadphase/detail/morton-inl.h index 4f27aef91..b646f299e 100644 --- a/src/broadphase/detail/morton-inl.h +++ b/src/broadphase/detail/morton-inl.h @@ -62,7 +62,7 @@ morton_functor::morton_functor(const AABB& bbox) //============================================================================== template -uint32_t morton_functor::operator()(const Vec3f& point) const { +uint32_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u); @@ -82,7 +82,7 @@ morton_functor::morton_functor(const AABB& bbox) //============================================================================== template -uint64_t morton_functor::operator()(const Vec3f& point) const { +uint64_t morton_functor::operator()(const Vec3s& point) const { uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20); uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20); uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20); @@ -115,7 +115,7 @@ morton_functor>::morton_functor(const AABB& bbox) //============================================================================== template std::bitset morton_functor>::operator()( - const Vec3f& point) const { + const Vec3s& point) const { S x = (point[0] - base[0]) * inv[0]; S y = (point[1] - base[1]) * inv[1]; S z = (point[2] - base[2]) * inv[2]; diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp index f4dbc8061..8a4c3a185 100644 --- a/src/distance/box_halfspace.cpp +++ b/src/distance/box_halfspace.cpp @@ -51,7 +51,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -64,7 +64,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp index abce82fb7..cdcde09e2 100644 --- a/src/distance/box_plane.cpp +++ b/src/distance/box_plane.cpp @@ -52,7 +52,7 @@ CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { + Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -67,7 +67,7 @@ CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const bool, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { + Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp index aafda9c17..4c249b40c 100644 --- a/src/distance/box_sphere.cpp +++ b/src/distance/box_sphere.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::boxSphereDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -60,7 +60,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); const CoalScalar distance = diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp index b09a35222..ad5a2c051 100644 --- a/src/distance/capsule_capsule.cpp +++ b/src/distance/capsule_capsule.cpp @@ -59,8 +59,8 @@ CoalScalar clamp(const CoalScalar& num, const CoalScalar& denom) { } /// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd -void clamped_linear(Vec3f& a_sd, const Vec3f& a, const CoalScalar& s_n, - const CoalScalar& s_d, const Vec3f& d) { +void clamped_linear(Vec3s& a_sd, const Vec3s& a, const CoalScalar& s_n, + const CoalScalar& s_d, const Vec3s& d) { assert(s_d >= 0.); if (s_n <= 0.) a_sd = a; @@ -80,15 +80,15 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& wp1, Vec3f& wp2, Vec3f& normal) { + const bool, Vec3s& wp1, Vec3s& wp2, Vec3s& normal) { const Capsule* capsule1 = static_cast(o1); const Capsule* capsule2 = static_cast(o2); CoalScalar EPSILON = std::numeric_limits::epsilon() * 100; // We assume that capsules are centered at the origin. - const coal::Vec3f& c1 = tf1.getTranslation(); - const coal::Vec3f& c2 = tf2.getTranslation(); + const coal::Vec3s& c1 = tf1.getTranslation(); + const coal::Vec3s& c2 = tf2.getTranslation(); // We assume that capsules are oriented along z-axis. CoalScalar halfLength1 = capsule1->halfLength; CoalScalar halfLength2 = capsule2->halfLength; @@ -96,14 +96,14 @@ CoalScalar ShapeShapeDistance( CoalScalar radius2 = (capsule2->radius + capsule2->getSweptSphereRadius()); // direction of capsules // ||d1|| = 2 * halfLength1 - const coal::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2); - const coal::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2); + const coal::Vec3s d1 = 2 * halfLength1 * tf1.getRotation().col(2); + const coal::Vec3s d2 = 2 * halfLength2 * tf2.getRotation().col(2); // Starting point of the segments // p1 + d1 is the end point of the segment - const coal::Vec3f p1 = c1 - d1 / 2; - const coal::Vec3f p2 = c2 - d2 / 2; - const coal::Vec3f r = p1 - p2; + const coal::Vec3s p1 = c1 - d1 / 2; + const coal::Vec3s p2 = c2 - d2 / 2; + const coal::Vec3s r = p1 - p2; CoalScalar a = d1.dot(d1); CoalScalar b = d1.dot(d2); CoalScalar c = d1.dot(r); @@ -112,7 +112,7 @@ CoalScalar ShapeShapeDistance( // S1 is parametrized by the equation p1 + s * d1 // S2 is parametrized by the equation p2 + t * d2 - Vec3f w1, w2; + Vec3s w1, w2; if (a <= EPSILON) { w1 = p1; if (e <= EPSILON) diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp index 3d794ba23..16b42d259 100644 --- a/src/distance/capsule_halfspace.cpp +++ b/src/distance/capsule_halfspace.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp index 35c1b8ee3..2d587cfb7 100644 --- a/src/distance/capsule_plane.cpp +++ b/src/distance/capsule_plane.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp index 6a3430d11..10159a989 100644 --- a/src/distance/cone_halfspace.cpp +++ b/src/distance/cone_halfspace.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cone& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp index 350552fe0..9e905bc36 100644 --- a/src/distance/cone_plane.cpp +++ b/src/distance/cone_plane.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cone& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 55acbfa73..274632596 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -46,7 +46,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const ConvexBase& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -59,7 +59,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp index b0514489c..a4907880d 100644 --- a/src/distance/convex_plane.cpp +++ b/src/distance/convex_plane.cpp @@ -46,7 +46,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const ConvexBase& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -59,7 +59,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp index 3c06d8c8b..bdff1a2ed 100644 --- a/src/distance/cylinder_halfspace.cpp +++ b/src/distance/cylinder_halfspace.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp index 125d3d4e0..807d3bf99 100644 --- a/src/distance/cylinder_plane.cpp +++ b/src/distance/cylinder_plane.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp index 5dc3e8b73..9c2674dc6 100644 --- a/src/distance/ellipsoid_halfspace.cpp +++ b/src/distance/ellipsoid_halfspace.cpp @@ -48,7 +48,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Ellipsoid& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -61,7 +61,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp index 169479041..4cc31f3aa 100644 --- a/src/distance/ellipsoid_plane.cpp +++ b/src/distance/ellipsoid_plane.cpp @@ -47,7 +47,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Ellipsoid& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -60,7 +60,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp index 07648d704..e1fa824fd 100644 --- a/src/distance/halfspace_halfspace.cpp +++ b/src/distance/halfspace_halfspace.cpp @@ -47,7 +47,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); return details::halfspaceHalfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp index abea962c3..7fdf4bf98 100644 --- a/src/distance/halfspace_plane.cpp +++ b/src/distance/halfspace_plane.cpp @@ -47,7 +47,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); return details::halfspacePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -57,7 +57,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); CoalScalar distance = diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp index 98b23b76e..5598e92f1 100644 --- a/src/distance/plane_plane.cpp +++ b/src/distance/plane_plane.cpp @@ -47,7 +47,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); return details::planePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp index 2724700f9..b2b79d7aa 100644 --- a/src/distance/sphere_capsule.cpp +++ b/src/distance/sphere_capsule.cpp @@ -47,7 +47,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); return details::sphereCapsuleDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -57,7 +57,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); const CoalScalar distance = diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp index 932810809..6844c201f 100644 --- a/src/distance/sphere_cylinder.cpp +++ b/src/distance/sphere_cylinder.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); return details::sphereCylinderDistance(s1, tf1, s2, tf2, p1, p2, normal); @@ -60,7 +60,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); const CoalScalar distance = diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp index e0fdbc64a..632836570 100644 --- a/src/distance/sphere_halfspace.cpp +++ b/src/distance/sphere_halfspace.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp index c28df4d35..d423786eb 100644 --- a/src/distance/sphere_plane.cpp +++ b/src/distance/sphere_plane.cpp @@ -50,7 +50,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -63,7 +63,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index 365f8af5e..6546ab183 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -56,7 +56,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); return details::sphereSphereDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index f4220b25e..8f4b0892d 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -46,7 +46,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); const CoalScalar distance = @@ -59,7 +59,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp index 8e3ba7e29..8db569fb8 100644 --- a/src/distance/triangle_plane.cpp +++ b/src/distance/triangle_plane.cpp @@ -46,7 +46,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); const CoalScalar distance = @@ -59,7 +59,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp index 8e7e1ce72..278e54db3 100644 --- a/src/distance/triangle_sphere.cpp +++ b/src/distance/triangle_sphere.cpp @@ -46,7 +46,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); const CoalScalar distance = @@ -59,7 +59,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, - const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); return details::sphereTriangleDistance(s1, tf1, s2, tf2, p1, p2, normal); diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp index ae737e6dd..9f3166642 100644 --- a/src/distance/triangle_triangle.cpp +++ b/src/distance/triangle_triangle.cpp @@ -46,7 +46,7 @@ template <> CoalScalar ShapeShapeDistance( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* solver, const bool, Vec3f& p1, Vec3f& p2, Vec3f& normal) { + const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { // Transform the triangles in world frame const TriangleP& s1 = static_cast(*o1); const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b), @@ -64,7 +64,7 @@ CoalScalar ShapeShapeDistance( solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance); // Get GJK initial guess - Vec3f guess; + Vec3s guess; if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess || solver->enable_cached_guess) { guess = solver->cached_guess; diff --git a/src/intersect.cpp b/src/intersect.cpp index feca0843e..6440ab5da 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -45,9 +45,9 @@ namespace coal { -bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, - const Vec3f& v3, Vec3f* n, CoalScalar* t) { - Vec3f n_ = (v2 - v1).cross(v3 - v1); +bool Intersect::buildTrianglePlane(const Vec3s& v1, const Vec3s& v2, + const Vec3s& v3, Vec3s* n, CoalScalar* t) { + Vec3s n_ = (v2 - v1).cross(v3 - v1); CoalScalar norm2 = n_.squaredNorm(); if (norm2 > 0) { *n = n_ / sqrt(norm2); @@ -57,12 +57,12 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, return false; } -void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, - const Vec3f& B, Vec3f& VEC, Vec3f& X, - Vec3f& Y) { - Vec3f T; +void TriangleDistance::segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q, + const Vec3s& B, Vec3s& VEC, Vec3s& X, + Vec3s& Y) { + Vec3s T; CoalScalar A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; - Vec3f TMP; + Vec3s TMP; T = Q - P; A_dot_A = A.dot(A); @@ -153,13 +153,13 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, } } -CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - Vec3f& P, Vec3f& Q) { +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + Vec3s& P, Vec3s& Q) { // Compute vectors along the 6 sides - Vec3f Sv[3]; - Vec3f Tv[3]; - Vec3f VEC; + Vec3s Sv[3]; + Vec3s Tv[3]; + Vec3s VEC; Sv[0] = S[1] - S[0]; Sv[1] = S[2] - S[1]; @@ -177,7 +177,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], // Even if these tests fail, it may be helpful to know the closest // points found, and whether the triangles were shown disjoint - Vec3f V, Z, minP, minQ; + Vec3s V, Z, minP, minQ; CoalScalar mindd; int shown_disjoint = 0; @@ -232,7 +232,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], // First check for case 1 - Vec3f Sn; + Vec3s Sn; CoalScalar Snl; Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle @@ -243,7 +243,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], if (Snl > 1e-15) { // Get projection lengths of T points - Vec3f Tp; + Vec3s Tp; V = S[0] - T[0]; Tp[0] = V.dot(Sn); @@ -300,14 +300,14 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], } } - Vec3f Tn; + Vec3s Tn; CoalScalar Tnl; Tn = Tv[0].cross(Tv[1]); Tnl = Tn.dot(Tn); if (Tnl > 1e-15) { - Vec3f Sp; + Vec3s Sp; V = T[0] - S[0]; Sp[0] = V.dot(Tn); @@ -367,12 +367,12 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return 0; } -CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - Vec3f& P, Vec3f& Q) { - Vec3f S[3]; - Vec3f T[3]; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + Vec3s& P, Vec3s& Q) { + Vec3s S[3]; + Vec3s T[3]; S[0] = S1; S[1] = S2; S[2] = S3; @@ -383,10 +383,10 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, return sqrTriDistance(S, T, P, Q); } -CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) { - Vec3f T_transformed[3]; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Matrix3s& R, const Vec3s& Tl, + Vec3s& P, Vec3s& Q) { + Vec3s T_transformed[3]; T_transformed[0] = R * T[0] + Tl; T_transformed[1] = R * T[1] + Tl; T_transformed[2] = R * T[2] + Tl; @@ -394,10 +394,10 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return sqrTriDistance(S, T_transformed, P, Q); } -CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], - const Transform3f& tf, Vec3f& P, - Vec3f& Q) { - Vec3f T_transformed[3]; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], + const Transform3f& tf, Vec3s& P, + Vec3s& Q) { + Vec3s T_transformed[3]; T_transformed[0] = tf.transform(T[0]); T_transformed[1] = tf.transform(T[1]); T_transformed[2] = tf.transform(T[2]); @@ -405,35 +405,35 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3], return sqrTriDistance(S, T_transformed, P, Q); } -CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Matrix3f& R, const Vec3f& Tl, - Vec3f& P, Vec3f& Q) { - Vec3f T1_transformed = R * T1 + Tl; - Vec3f T2_transformed = R * T2 + Tl; - Vec3f T3_transformed = R * T3 + Tl; +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Matrix3s& R, const Vec3s& Tl, + Vec3s& P, Vec3s& Q) { + Vec3s T1_transformed = R * T1 + Tl; + Vec3s T2_transformed = R * T2 + Tl; + Vec3s T3_transformed = R * T3 + Tl; return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } -CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2, - const Vec3f& S3, const Vec3f& T1, - const Vec3f& T2, const Vec3f& T3, - const Transform3f& tf, Vec3f& P, - Vec3f& Q) { - Vec3f T1_transformed = tf.transform(T1); - Vec3f T2_transformed = tf.transform(T2); - Vec3f T3_transformed = tf.transform(T3); +CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, + const Vec3s& S3, const Vec3s& T1, + const Vec3s& T2, const Vec3s& T3, + const Transform3f& tf, Vec3s& P, + Vec3s& Q) { + Vec3s T1_transformed = tf.transform(T1); + Vec3s T2_transformed = tf.transform(T2); + Vec3s T3_transformed = tf.transform(T3); return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } -Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, - const Vec3f& p) { +Project::ProjectResult Project::projectLine(const Vec3s& a, const Vec3s& b, + const Vec3s& p) { ProjectResult res; - const Vec3f d = b - a; + const Vec3s d = b - a; const CoalScalar l = d.squaredNorm(); if (l > 0) { @@ -455,15 +455,15 @@ Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, return res; } -Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, - const Vec3f& c, - const Vec3f& p) { +Project::ProjectResult Project::projectTriangle(const Vec3s& a, const Vec3s& b, + const Vec3s& c, + const Vec3s& p) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c}; - const Vec3f dl[] = {a - b, b - c, c - a}; - const Vec3f& n = dl[0].cross(dl[1]); + const Vec3s* vt[] = {&a, &b, &c}; + const Vec3s dl[] = {a - b, b - c, c - a}; + const Vec3s& n = dl[0].cross(dl[1]); const CoalScalar l = n.squaredNorm(); if (l > 0) { @@ -492,7 +492,7 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, { CoalScalar d = (a - p).dot(n); CoalScalar s = sqrt(l); - Vec3f p_to_project = n * (d / l); + Vec3s p_to_project = n * (d / l); mindist = p_to_project.squaredNorm(); res.encode = 7; // m = 0x111 res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s; @@ -507,16 +507,16 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, return res; } -Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, - const Vec3f& b, - const Vec3f& c, - const Vec3f& d, - const Vec3f& p) { +Project::ProjectResult Project::projectTetrahedra(const Vec3s& a, + const Vec3s& b, + const Vec3s& c, + const Vec3s& d, + const Vec3s& p) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c, &d}; - const Vec3f dl[3] = {a - d, b - d, c - d}; + const Vec3s* vt[] = {&a, &b, &c, &d}; + const Vec3s dl[3] = {a - d, b - d, c - d}; CoalScalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0; if (ng && @@ -567,11 +567,11 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, return res; } -Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, - const Vec3f& b) { +Project::ProjectResult Project::projectLineOrigin(const Vec3s& a, + const Vec3s& b) { ProjectResult res; - const Vec3f d = b - a; + const Vec3s d = b - a; const CoalScalar l = d.squaredNorm(); if (l > 0) { @@ -593,15 +593,15 @@ Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, return res; } -Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, - const Vec3f& b, - const Vec3f& c) { +Project::ProjectResult Project::projectTriangleOrigin(const Vec3s& a, + const Vec3s& b, + const Vec3s& c) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c}; - const Vec3f dl[] = {a - b, b - c, c - a}; - const Vec3f& n = dl[0].cross(dl[1]); + const Vec3s* vt[] = {&a, &b, &c}; + const Vec3s dl[] = {a - b, b - c, c - a}; + const Vec3s& n = dl[0].cross(dl[1]); const CoalScalar l = n.squaredNorm(); if (l > 0) { @@ -630,7 +630,7 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, { CoalScalar d = a.dot(n); CoalScalar s = sqrt(l); - Vec3f o_to_project = n * (d / l); + Vec3s o_to_project = n * (d / l); mindist = o_to_project.squaredNorm(); res.encode = 7; // m = 0x111 res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; @@ -645,15 +645,15 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, return res; } -Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, - const Vec3f& b, - const Vec3f& c, - const Vec3f& d) { +Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3s& a, + const Vec3s& b, + const Vec3s& c, + const Vec3s& d) { ProjectResult res; static const size_t nexti[] = {1, 2, 0}; - const Vec3f* vt[] = {&a, &b, &c, &d}; - const Vec3f dl[3] = {a - d, b - d, c - d}; + const Vec3s* vt[] = {&a, &b, &c, &d}; + const Vec3s dl[3] = {a - d, b - d, c - d}; CoalScalar vl = triple(dl[0], dl[1], dl[2]); bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0; if (ng && diff --git a/src/math/transform.cpp b/src/math/transform.cpp index 5843b03ad..844c9c183 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -46,7 +46,7 @@ void relativeTransform(const Transform3f& tf1, const Transform3f& tf2, void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2, Transform3f& tf) { - Matrix3f R(tf2.getRotation() * tf1.getRotation().transpose()); + Matrix3s R(tf2.getRotation() * tf1.getRotation().transpose()); tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation()); } diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp index dc947ab0c..a0012d764 100644 --- a/src/mesh_loader/assimp.cpp +++ b/src/mesh_loader/assimp.cpp @@ -106,7 +106,7 @@ void Loader::load(const std::string& resource_path) { * @param[in] vertices_offset Current number of vertices in the model * @param tv Triangles and Vertices of the mesh submodels */ -unsigned recurseBuildMesh(const coal::Vec3f& scale, const aiScene* scene, +unsigned recurseBuildMesh(const coal::Vec3s& scale, const aiScene* scene, const aiNode* node, unsigned vertices_offset, TriangleAndVertices& tv) { if (!node) return 0; @@ -131,7 +131,7 @@ unsigned recurseBuildMesh(const coal::Vec3f& scale, const aiScene* scene, aiVector3D p = input_mesh->mVertices[j]; p *= transform; tv.vertices_.push_back( - coal::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2])); + coal::Vec3s(p.x * scale[0], p.y * scale[1], p.z * scale[2])); } // add the indices @@ -155,7 +155,7 @@ unsigned recurseBuildMesh(const coal::Vec3f& scale, const aiScene* scene, return nbVertices; } -void buildMesh(const coal::Vec3f& scale, const aiScene* scene, +void buildMesh(const coal::Vec3s& scale, const aiScene* scene, unsigned vertices_offset, TriangleAndVertices& tv) { recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv); } diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp index 5ebe872d2..9b38398ba 100644 --- a/src/mesh_loader/loader.cpp +++ b/src/mesh_loader/loader.cpp @@ -58,14 +58,14 @@ bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const { } template -BVHModelPtr_t _load(const std::string& filename, const Vec3f& scale) { +BVHModelPtr_t _load(const std::string& filename, const Vec3s& scale) { shared_ptr > polyhedron(new BVHModel); loadPolyhedronFromResource(filename, scale, polyhedron); return polyhedron; } BVHModelPtr_t MeshLoader::load(const std::string& filename, - const Vec3f& scale) { + const Vec3s& scale) { switch (bvType_) { case BV_AABB: return _load(filename, scale); @@ -100,7 +100,7 @@ CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) { } BVHModelPtr_t CachedMeshLoader::load(const std::string& filename, - const Vec3f& scale) { + const Vec3s& scale) { Key key(filename, scale); std::time_t mtime = 0; diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index 4cc1601e7..b6a8b4ae0 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -48,11 +48,11 @@ namespace details { // segment to to another point. The code is inspired by the explanation // given by Dan Sunday's page: // http://geomalgorithms.com/a02-_lines.html -static inline void lineSegmentPointClosestToPoint(const Vec3f& p, - const Vec3f& s1, - const Vec3f& s2, Vec3f& sp) { - Vec3f v = s2 - s1; - Vec3f w = p - s1; +static inline void lineSegmentPointClosestToPoint(const Vec3s& p, + const Vec3s& s1, + const Vec3s& s2, Vec3s& sp) { + Vec3s v = s2 - s1; + Vec3s w = p - s1; CoalScalar c1 = w.dot(v); CoalScalar c2 = v.dot(v); @@ -63,7 +63,7 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p, sp = s2; } else { CoalScalar b = c1 / c2; - Vec3f Pb = s1 + v * b; + Vec3s Pb = s1 + v * b; sp = Pb; } } @@ -75,13 +75,13 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p, inline CoalScalar sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { - Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength))); - Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength))); - Vec3f s_c = tf1.getTranslation(); + const Transform3f& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { + Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength))); + Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength))); + Vec3s s_c = tf1.getTranslation(); - Vec3f segment_point; + Vec3s segment_point; lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); normal = segment_point - s_c; @@ -108,31 +108,31 @@ inline CoalScalar sphereCapsuleDistance(const Sphere& s1, inline CoalScalar sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, const Cylinder& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { + const Transform3f& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { static const CoalScalar eps(sqrt(std::numeric_limits::epsilon())); CoalScalar r1(s1.radius); CoalScalar r2(s2.radius); CoalScalar lz2(s2.halfLength); // boundaries of the cylinder axis - Vec3f A(tf2.transform(Vec3f(0, 0, -lz2))); - Vec3f B(tf2.transform(Vec3f(0, 0, lz2))); + Vec3s A(tf2.transform(Vec3s(0, 0, -lz2))); + Vec3s B(tf2.transform(Vec3s(0, 0, lz2))); // Position of the center of the sphere - Vec3f S(tf1.getTranslation()); + Vec3s S(tf1.getTranslation()); // axis of the cylinder - Vec3f u(tf2.getRotation().col(2)); + Vec3s u(tf2.getRotation().col(2)); /// @todo a tiny performance improvement could be achieved using the abscissa /// with S as the origin assert((B - A - (s2.halfLength * 2) * u).norm() < eps); - Vec3f AS(S - A); + Vec3s AS(S - A); // abscissa of S on cylinder axis with A as the origin CoalScalar s(u.dot(AS)); - Vec3f P(A + s * u); - Vec3f PS(S - P); + Vec3s P(A + s * u); + Vec3s PS(S - P); CoalScalar dPS = PS.norm(); // Normal to cylinder axis such that plane (A, u, v) contains sphere // center - Vec3f v(0, 0, 0); + Vec3s v(0, 0, 0); CoalScalar dist; if (dPS > eps) { // S is not on cylinder axis @@ -148,7 +148,7 @@ inline CoalScalar sphereCylinderDistance(const Sphere& s1, } else { // closest point on cylinder is on cylinder circle basis p2 = A + r2 * v; - Vec3f Sp2(p2 - S); + Vec3s Sp2(p2 - S); CoalScalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; @@ -181,7 +181,7 @@ inline CoalScalar sphereCylinderDistance(const Sphere& s1, } else { // closest point on cylinder is on cylinder circle basis p2 = B + r2 * v; - Vec3f Sp2(p2 - S); + Vec3s Sp2(p2 - S); CoalScalar dSp2 = Sp2.norm(); if (dSp2 > eps) { normal = (1 / dSp2) * Sp2; @@ -216,15 +216,15 @@ inline CoalScalar sphereCylinderDistance(const Sphere& s1, /// @return the distance between the two spheres (negative if penetration). inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { - const coal::Vec3f& center1 = tf1.getTranslation(); - const coal::Vec3f& center2 = tf2.getTranslation(); + Vec3s& p1, Vec3s& p2, Vec3s& normal) { + const coal::Vec3s& center1 = tf1.getTranslation(); + const coal::Vec3s& center2 = tf2.getTranslation(); CoalScalar r1 = (s1.radius + s1.getSweptSphereRadius()); CoalScalar r2 = (s2.radius + s2.getSweptSphereRadius()); - Vec3f c1c2 = center2 - center1; + Vec3s c1c2 = center2 - center1; CoalScalar cdist = c1c2.norm(); - Vec3f unit(1, 0, 0); + Vec3s unit(1, 0, 0); if (cdist > Eigen::NumTraits::epsilon()) unit = c1c2 / cdist; CoalScalar dist = cdist - r1 - r2; normal = unit; @@ -234,10 +234,10 @@ inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, } /** @brief the minimum distance from a point to a line */ -inline CoalScalar segmentSqrDistance(const Vec3f& from, const Vec3f& to, - const Vec3f& p, Vec3f& nearest) { - Vec3f diff = p - from; - Vec3f v = to - from; +inline CoalScalar segmentSqrDistance(const Vec3s& from, const Vec3s& to, + const Vec3s& p, Vec3s& nearest) { + Vec3s diff = p - from; + Vec3s v = to - from; CoalScalar t = v.dot(diff); if (t > 0) { @@ -257,19 +257,19 @@ inline CoalScalar segmentSqrDistance(const Vec3f& from, const Vec3f& to, } /// @brief Whether a point's projection is in a triangle -inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, - const Vec3f& normal, const Vec3f& p) { - Vec3f edge1(p2 - p1); - Vec3f edge2(p3 - p2); - Vec3f edge3(p1 - p3); +inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3, + const Vec3s& normal, const Vec3s& p) { + Vec3s edge1(p2 - p1); + Vec3s edge2(p3 - p2); + Vec3s edge3(p1 - p3); - Vec3f p1_to_p(p - p1); - Vec3f p2_to_p(p - p2); - Vec3f p3_to_p(p - p3); + Vec3s p1_to_p(p - p1); + Vec3s p2_to_p(p - p2); + Vec3s p3_to_p(p - p3); - Vec3f edge1_normal(edge1.cross(normal)); - Vec3f edge2_normal(edge2.cross(normal)); - Vec3f edge3_normal(edge3.cross(normal)); + Vec3s edge1_normal(edge1.cross(normal)); + Vec3s edge2_normal(edge2.cross(normal)); + Vec3s edge3_normal(edge3.cross(normal)); CoalScalar r1, r2, r3; r1 = edge1_normal.dot(p1_to_p); @@ -288,15 +288,15 @@ inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, inline CoalScalar sphereTriangleDistance(const Sphere& s, const Transform3f& tf1, const TriangleP& tri, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { - const Vec3f& P1 = tf2.transform(tri.a); - const Vec3f& P2 = tf2.transform(tri.b); - const Vec3f& P3 = tf2.transform(tri.c); + const Transform3f& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { + const Vec3s& P1 = tf2.transform(tri.a); + const Vec3s& P2 = tf2.transform(tri.b); + const Vec3s& P3 = tf2.transform(tri.c); - Vec3f tri_normal = (P2 - P1).cross(P3 - P1); + Vec3s tri_normal = (P2 - P1).cross(P3 - P1); tri_normal.normalize(); - const Vec3f& center = tf1.getTranslation(); + const Vec3s& center = tf1.getTranslation(); // Note: comparing an object with a swept-sphere radius of r1 against another // object with a swept-sphere radius of r2 is equivalent to comparing the // first object with a swept-sphere radius of r1 + r2 against the second @@ -305,10 +305,10 @@ inline CoalScalar sphereTriangleDistance(const Sphere& s, s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius(); assert(radius >= 0); assert(s.radius >= 0); - Vec3f p1_to_center = center - P1; + Vec3s p1_to_center = center - P1; CoalScalar distance_from_plane = p1_to_center.dot(tri_normal); - Vec3f closest_point( - Vec3f::Constant(std::numeric_limits::quiet_NaN())); + Vec3s closest_point( + Vec3s::Constant(std::numeric_limits::quiet_NaN())); CoalScalar min_distance_sqr, distance_sqr; if (distance_from_plane < 0) { @@ -321,7 +321,7 @@ inline CoalScalar sphereTriangleDistance(const Sphere& s, min_distance_sqr = distance_from_plane * distance_from_plane; } else { // Compute distance to each edge and take minimal distance - Vec3f nearest_on_edge; + Vec3s nearest_on_edge; min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point); distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); @@ -349,7 +349,7 @@ inline CoalScalar sphereTriangleDistance(const Sphere& s, /// @return the distance between the two shapes (negative if penetration). inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1, const ShapeBase& s, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { + Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the halfspace normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). @@ -358,7 +358,7 @@ inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1, Halfspace new_h = transform(h, tf1); // Express halfspace normal in shape frame - Vec3f n_2(tf2.getRotation().transpose() * new_h.n); + Vec3s n_2(tf2.getRotation().transpose() * new_h.n); // Compute support of shape in direction of halfspace normal int hint = 0; @@ -383,7 +383,7 @@ inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1, /// @return the distance between the two shapes (negative if penetration). inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1, const ShapeBase& s, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { + Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the plane normal is // parallel to the shape's surface (every primitive except sphere and // ellipsoid). @@ -392,17 +392,17 @@ inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1, std::array new_h = transformToHalfspaces(plane, tf1); // Express halfspace normals in shape frame - Vec3f n_h1(tf2.getRotation().transpose() * new_h[0].n); - Vec3f n_h2(tf2.getRotation().transpose() * new_h[1].n); + Vec3s n_h1(tf2.getRotation().transpose() * new_h[0].n); + Vec3s n_h2(tf2.getRotation().transpose() * new_h[1].n); // Compute support of shape in direction of halfspace normal and its opposite int hint = 0; - Vec3f p2h1 = + Vec3s p2h1 = getSupport(&s, -n_h1, hint); p2h1 = tf2.transform(p2h1); hint = 0; - Vec3f p2h2 = + Vec3s p2h2 = getSupport(&s, -n_h2, hint); p2h2 = tf2.transform(p2h2); @@ -437,15 +437,15 @@ inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1, /// @return the distance between the two shapes (negative if penetration). inline CoalScalar boxSphereDistance(const Box& b, const Transform3f& tfb, const Sphere& s, const Transform3f& tfs, - Vec3f& pb, Vec3f& ps, Vec3f& normal) { - const Vec3f& os = tfs.getTranslation(); - const Vec3f& ob = tfb.getTranslation(); - const Matrix3f& Rb = tfb.getRotation(); + Vec3s& pb, Vec3s& ps, Vec3s& normal) { + const Vec3s& os = tfs.getTranslation(); + const Vec3s& ob = tfb.getTranslation(); + const Matrix3s& Rb = tfb.getRotation(); pb = ob; bool outside = false; - const Vec3f os_in_b_frame(Rb.transpose() * (os - ob)); + const Vec3s os_in_b_frame(Rb.transpose() * (os - ob)); int axis = -1; CoalScalar min_d = (std::numeric_limits::max)(); for (int i = 0; i < 3; ++i) { @@ -512,13 +512,13 @@ inline CoalScalar boxSphereDistance(const Box& b, const Transform3f& tfb, inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { + const Transform3f& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Halfspace new_s2 = transform(s2, tf2); CoalScalar distance; - Vec3f dir = (new_s1.n).cross(new_s2.n); + Vec3s dir = (new_s1.n).cross(new_s2.n); CoalScalar dir_sq_norm = dir.squaredNorm(); if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel @@ -588,13 +588,13 @@ inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1, inline CoalScalar halfspacePlaneDistance(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, - const Transform3f& tf2, Vec3f& p1, - Vec3f& p2, Vec3f& normal) { + const Transform3f& tf2, Vec3s& p1, + Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); CoalScalar distance; - Vec3f dir = (new_s1.n).cross(new_s2.n); + Vec3s dir = (new_s1.n).cross(new_s2.n); CoalScalar dir_sq_norm = dir.squaredNorm(); if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel @@ -649,12 +649,12 @@ inline CoalScalar halfspacePlaneDistance(const Halfspace& s1, /// line. inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, - Vec3f& p1, Vec3f& p2, Vec3f& normal) { + Vec3s& p1, Vec3s& p2, Vec3s& normal) { Plane new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); CoalScalar distance; - Vec3f dir = (new_s1.n).cross(new_s2.n); + Vec3s dir = (new_s1.n).cross(new_s2.n); CoalScalar dir_sq_norm = dir.squaredNorm(); if (dir_sq_norm < std::numeric_limits::epsilon()) // parallel @@ -700,11 +700,11 @@ inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3f& tf1, } /// See the prototype below -inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Vec3f& Q1, - const Vec3f& Q2, const Vec3f& Q3, - Vec3f& normal) { - Vec3f u((P2 - P1).cross(P3 - P1)); +inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2, + const Vec3s& P3, const Vec3s& Q1, + const Vec3s& Q2, const Vec3s& Q3, + Vec3s& normal) { + Vec3s u((P2 - P1).cross(P3 - P1)); normal = u.normalized(); CoalScalar depth1((P1 - Q1).dot(normal)); CoalScalar depth2((P1 - Q2).dot(normal)); @@ -719,17 +719,17 @@ inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2, // // Note that we compute here an upper bound of the penetration distance, // not the exact value. -inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2, - const Vec3f& P3, const Vec3f& Q1, - const Vec3f& Q2, const Vec3f& Q3, +inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2, + const Vec3s& P3, const Vec3s& Q1, + const Vec3s& Q2, const Vec3s& Q3, const Transform3f& tf1, - const Transform3f& tf2, Vec3f& normal) { - Vec3f globalP1(tf1.transform(P1)); - Vec3f globalP2(tf1.transform(P2)); - Vec3f globalP3(tf1.transform(P3)); - Vec3f globalQ1(tf2.transform(Q1)); - Vec3f globalQ2(tf2.transform(Q2)); - Vec3f globalQ3(tf2.transform(Q3)); + const Transform3f& tf2, Vec3s& normal) { + Vec3s globalP1(tf1.transform(P1)); + Vec3s globalP2(tf1.transform(P2)); + Vec3s globalP3(tf1.transform(P3)); + Vec3s globalQ1(tf2.transform(Q1)); + Vec3s globalQ2(tf2.transform(Q2)); + Vec3s globalQ3(tf2.transform(Q3)); return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2, globalQ3, normal); } diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index e8093de60..0e81fce23 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -67,7 +67,7 @@ void GJK::reset(size_t max_iterations_, CoalScalar tolerance_) { iterations_momentum_stop = 0; } -Vec3f GJK::getGuessFromSimplex() const { return ray; } +Vec3s GJK::getGuessFromSimplex() const { return ray; } namespace details { @@ -90,7 +90,7 @@ namespace details { // w0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0 // w1 = alpha * w[0].w1 + (1 - alpha) * w[1].w1 // clang-format on -void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { +void getClosestPoints(const GJK::Simplex& simplex, Vec3s& w0, Vec3s& w1) { GJK::SimplexV* const* vs = simplex.vertex; for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) { @@ -104,10 +104,10 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { w1 = vs[0]->w1; return; case 2: { - const Vec3f &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, + const Vec3s &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, b0 = vs[1]->w0, b1 = vs[1]->w1; CoalScalar la, lb; - Vec3f N(b - a); + Vec3s N(b - a); la = N.dot(-a); if (la <= 0) { assert(false); @@ -155,8 +155,8 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) { /// The normal should follow coal convention: it points from shape0 to /// shape1. template -void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0, - Vec3f& w1) { +void inflate(const MinkowskiDiff& shape, const Vec3s& normal, Vec3s& w0, + Vec3s& w1) { #ifndef NDEBUG const CoalScalar dummy_precision = Eigen::NumTraits::dummy_precision(); @@ -173,8 +173,8 @@ void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0, } // namespace details -void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const { +void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const { details::getClosestPoints(*simplex, w0, w1); if ((w1 - w0).norm() > Eigen::NumTraits::dummy_precision()) { normal = (w1 - w0).normalized(); @@ -184,7 +184,7 @@ void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, details::inflate(shape, normal, w0, w1); } -GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, +GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3s& guess, const support_func_guess_t& supportHint) { CoalScalar alpha = 0; iterations = 0; @@ -206,16 +206,16 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, CoalScalar rl = guess.norm(); if (rl < tolerance) { - ray = Vec3f(-1, 0, 0); + ray = Vec3s(-1, 0, 0); rl = 1; } else ray = guess; // Momentum GJKVariant current_gjk_variant = gjk_variant; - Vec3f w = ray; - Vec3f dir = ray; - Vec3f y; + Vec3s w = ray; + Vec3s dir = ray; + Vec3s y; CoalScalar momentum; bool normalize_support_direction = shape->normalize_support_direction; do { @@ -370,7 +370,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, return status; } -bool GJK::checkConvergence(const Vec3f& w, const CoalScalar& rl, +bool GJK::checkConvergence(const Vec3s& w, const CoalScalar& rl, CoalScalar& alpha, const CoalScalar& omega) const { // x^* is the optimal solution (projection of origin onto the Minkowski // difference). @@ -429,14 +429,14 @@ inline void GJK::removeVertex(Simplex& simplex) { free_v[nfree++] = simplex.vertex[--simplex.rank]; } -inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, +inline void GJK::appendVertex(Simplex& simplex, const Vec3s& v, support_func_guess_t& hint) { simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory getSupport(v, *simplex.vertex[simplex.rank++], hint); } bool GJK::encloseOrigin() { - Vec3f axis(Vec3f::Zero()); + Vec3s axis(Vec3s::Zero()); support_func_guess_t hint = support_func_guess_t::Zero(); switch (simplex->rank) { case 1: @@ -453,10 +453,10 @@ bool GJK::encloseOrigin() { } break; case 2: { - Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w; + Vec3s d = simplex->vertex[1]->w - simplex->vertex[0]->w; for (int i = 0; i < 3; ++i) { axis[i] = 1; - Vec3f p = d.cross(axis); + Vec3s p = d.cross(axis); if (!p.isZero()) { appendVertex(*simplex, p, hint); if (encloseOrigin()) return true; @@ -493,7 +493,7 @@ bool GJK::encloseOrigin() { } inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a, - const Vec3f& A, GJK::Simplex& next, Vec3f& ray) { + const Vec3s& A, GJK::Simplex& next, Vec3s& ray) { // A is the closest to the origin ray = A; next.vertex[0] = current.vertex[a]; @@ -501,9 +501,9 @@ inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a, } inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, - GJK::vertex_id_t b, const Vec3f& A, const Vec3f& B, - const Vec3f& AB, const CoalScalar& ABdotAO, - GJK::Simplex& next, Vec3f& ray) { + GJK::vertex_id_t b, const Vec3s& A, const Vec3s& B, + const Vec3s& AB, const CoalScalar& ABdotAO, + GJK::Simplex& next, Vec3s& ray) { // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B ray = AB.dot(B) * A + ABdotAO * B; @@ -517,8 +517,8 @@ inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a, inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a, GJK::vertex_id_t b, GJK::vertex_id_t c, - const Vec3f& ABC, const CoalScalar& ABCdotAO, - GJK::Simplex& next, Vec3f& ray) { + const Vec3s& ABC, const CoalScalar& ABCdotAO, + GJK::Simplex& next, Vec3s& ray) { next.rank = 3; next.vertex[2] = current.vertex[a]; @@ -544,10 +544,10 @@ inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a, bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 1, b = 0; // A is the last point we added. - const Vec3f& A = current.vertex[a]->w; - const Vec3f& B = current.vertex[b]->w; + const Vec3s& A = current.vertex[a]->w; + const Vec3s& B = current.vertex[b]->w; - const Vec3f AB = B - A; + const Vec3s AB = B - A; const CoalScalar d = AB.dot(-A); assert(d <= AB.squaredNorm()); @@ -572,10 +572,10 @@ bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { const vertex_id_t a = 2, b = 1, c = 0; // A is the last point we added. - const Vec3f &A = current.vertex[a]->w, B = current.vertex[b]->w, + const Vec3s &A = current.vertex[a]->w, B = current.vertex[b]->w, C = current.vertex[c]->w; - const Vec3f AB = B - A, AC = C - A, ABC = AB.cross(AC); + const Vec3s AB = B - A, AC = C - A, ABC = AB.cross(AC); CoalScalar edgeAC2o = ABC.cross(AC).dot(-A); if (edgeAC2o >= 0) { @@ -614,10 +614,10 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { // The code of this function was generated using doc/gjk.py const vertex_id_t a = 3, b = 2, c = 1, d = 0; - const Vec3f& A(current.vertex[a]->w); - const Vec3f& B(current.vertex[b]->w); - const Vec3f& C(current.vertex[c]->w); - const Vec3f& D(current.vertex[d]->w); + const Vec3s& A(current.vertex[a]->w); + const Vec3s& B(current.vertex[b]->w); + const Vec3s& C(current.vertex[c]->w); + const Vec3s& D(current.vertex[d]->w); const CoalScalar aa = A.squaredNorm(); const CoalScalar da = D.dot(A); const CoalScalar db = D.dot(B); @@ -637,8 +637,8 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { const CoalScalar ba_ca = ba - ca; const CoalScalar ca_da = ca - da; const CoalScalar da_ba = da - ba; - const Vec3f a_cross_b = A.cross(B); - const Vec3f a_cross_c = A.cross(C); + const Vec3s a_cross_b = A.cross(B); + const Vec3s a_cross_c = A.cross(C); const CoalScalar dummy_precision( 3 * std::sqrt(std::numeric_limits::epsilon())); @@ -1039,8 +1039,8 @@ void EPA::reset(size_t max_iterations_, CoalScalar tolerance_) { bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a, const SimplexVertex& b, CoalScalar& dist) { - Vec3f ab = b.w - a.w; - Vec3f n_ab = ab.cross(face->n); + Vec3s ab = b.w - a.w; + Vec3s n_ab = ab.cross(face->n); CoalScalar a_dot_nab = a.w.dot(n_ab); if (a_dot_nab < 0) // the origin is on the outside part of ab @@ -1154,7 +1154,7 @@ EPA::SimplexFace* EPA::findClosestFace() { return minf; } -EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { +EPA::Status EPA::evaluate(GJK& gjk, const Vec3s& guess) { GJK::Simplex& simplex = *gjk.getSimplex(); support_hint = gjk.support_hint; @@ -1309,7 +1309,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { if (nl > 0) normal /= nl; else - normal = Vec3f(1, 0, 0); + normal = Vec3s(1, 0, 0); depth = 0; result.rank = 1; result.vertex[0] = simplex.vertex[0]; @@ -1449,8 +1449,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, return false; } -void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0, - Vec3f& w1, Vec3f& normal) const { +void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0, + Vec3s& w1, Vec3s& normal) const { details::getClosestPoints(result, w0, w1); if ((w0 - w1).norm() > Eigen::NumTraits::dummy_precision()) { if (this->depth >= 0) { @@ -1477,13 +1477,13 @@ void ConvexBase::buildSupportWarmStart() { this->support_warm_starts.indices.reserve( ConvexBase::num_support_warm_starts); - Vec3f axiis(0, 0, 0); + Vec3s axiis(0, 0, 0); details::ShapeSupportData support_data; int support_hint = 0; for (int i = 0; i < 3; ++i) { axiis(i) = 1; { - Vec3f support; + Vec3s support; coal::details::getShapeSupport(this, axiis, support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); @@ -1492,7 +1492,7 @@ void ConvexBase::buildSupportWarmStart() { axiis(i) = -1; { - Vec3f support; + Vec3s support; coal::details::getShapeSupport(this, axiis, support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); @@ -1502,14 +1502,14 @@ void ConvexBase::buildSupportWarmStart() { axiis(i) = 0; } - std::array eis = {Vec3f(1, 1, 1), // - Vec3f(-1, 1, 1), // - Vec3f(-1, -1, 1), // - Vec3f(1, -1, 1)}; + std::array eis = {Vec3s(1, 1, 1), // + Vec3s(-1, 1, 1), // + Vec3s(-1, -1, 1), // + Vec3s(1, -1, 1)}; for (size_t ei_index = 0; ei_index < 4; ++ei_index) { { - Vec3f support; + Vec3s support; coal::details::getShapeSupport(this, eis[ei_index], support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); @@ -1517,7 +1517,7 @@ void ConvexBase::buildSupportWarmStart() { } { - Vec3f support; + Vec3s support; coal::details::getShapeSupport(this, -eis[ei_index], support, support_hint, support_data); this->support_warm_starts.points.emplace_back(support); diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index 73307e12f..baa3f4dc9 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -45,9 +45,9 @@ namespace details { // ============================================================================ template -void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, - const Vec3f& ot1, const Vec3f& dir, Vec3f& support0, - Vec3f& support1, support_func_guess_t& hint, +void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3s& oR1, + const Vec3s& ot1, const Vec3s& dir, Vec3s& support0, + Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]) { assert(dir.norm() > Eigen::NumTraits::epsilon()); getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]); @@ -64,8 +64,8 @@ void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1, // ============================================================================ template -void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir, - Vec3f& support0, Vec3f& support1, +void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir, + Vec3s& support0, Vec3s& support1, support_func_guess_t& hint, ShapeSupportData data[2]) { getSupportTpl( static_cast(md.shapes[0]), @@ -310,13 +310,13 @@ template void COAL_DLLAPI MinkowskiDiff::set(co // ============================================================================ // clang-format off -template Vec3f COAL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support0(const Vec3s&, int&) const; -template Vec3f COAL_DLLAPI MinkowskiDiff::support0(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support0(const Vec3s&, int&) const; -template Vec3f COAL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support1(const Vec3s&, int&) const; -template Vec3f COAL_DLLAPI MinkowskiDiff::support1(const Vec3f&, int&) const; +template Vec3s COAL_DLLAPI MinkowskiDiff::support1(const Vec3s&, int&) const; // clang-format on } // namespace details diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp index 9c7563f4b..64be0e0cc 100644 --- a/src/narrowphase/support_functions.cpp +++ b/src/narrowphase/support_functions.cpp @@ -48,8 +48,8 @@ namespace details { getShapeSupport<_SupportOptions>(static_cast(shape), dir, \ support, hint, support_data) template -Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) { - Vec3f support; +Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) { + Vec3s support; ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: @@ -89,29 +89,29 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) { // Explicit instantiation // clang-format off -template COAL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); +template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); -template COAL_DLLAPI Vec3f getSupport(const ShapeBase*, const Vec3f&, int&); +template COAL_DLLAPI Vec3s getSupport(const ShapeBase*, const Vec3s&, int&); // clang-format on // ============================================================================ #define getShapeSupportTplInstantiation(ShapeType) \ template COAL_DLLAPI void getShapeSupport( \ - const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ + const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \ ShapeSupportData& support_data); \ \ template COAL_DLLAPI void getShapeSupport( \ - const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \ + const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \ ShapeSupportData& support_data); // ============================================================================ template -void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, - Vec3f& support, int& /*unused*/, +void getShapeSupport(const TriangleP* triangle, const Vec3s& dir, + Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { - FCL_REAL dota = dir.dot(triangle->a); - FCL_REAL dotb = dir.dot(triangle->b); - FCL_REAL dotc = dir.dot(triangle->c); + CoalScalar dota = dir.dot(triangle->a); + CoalScalar dotb = dir.dot(triangle->b); + CoalScalar dotc = dir.dot(triangle->c); if (dota > dotb) { if (dotc > dota) { support = triangle->c; @@ -134,15 +134,15 @@ getShapeSupportTplInstantiation(TriangleP); // ============================================================================ template -inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support, +inline void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { // The inflate value is simply to make the specialized functions with box // have a preferred side for edge cases. - static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.; - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); - Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0); - Vec3f support2 = + static const CoalScalar inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.; + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); + Vec3s support1 = (dir.array() > dummy_precision).select(box->halfSide, 0); + Vec3s support2 = (dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0); support.noalias() = support1 + support2; @@ -154,8 +154,8 @@ getShapeSupportTplInstantiation(Box); // ============================================================================ template -inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, - Vec3f& support, int& /*unused*/, +inline void getShapeSupport(const Sphere* sphere, const Vec3s& dir, + Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { if (_SupportOptions == SupportOptions::WithSweptSphere) { support.noalias() = @@ -164,23 +164,23 @@ inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, support.setZero(); } - HPP_FCL_UNUSED_VARIABLE(sphere); - HPP_FCL_UNUSED_VARIABLE(dir); + COAL_UNUSED_VARIABLE(sphere); + COAL_UNUSED_VARIABLE(dir); } getShapeSupportTplInstantiation(Sphere); // ============================================================================ template -inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir, - Vec3f& support, int& /*unused*/, +inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir, + Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { - FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; - FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; - FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; + CoalScalar a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; + CoalScalar b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; + CoalScalar c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; - Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); + Vec3s v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); - FCL_REAL d = std::sqrt(v.dot(dir)); + CoalScalar d = std::sqrt(v.dot(dir)); support = v / d; @@ -192,11 +192,11 @@ getShapeSupportTplInstantiation(Ellipsoid); // ============================================================================ template -inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, - Vec3f& support, int& /*unused*/, +inline void getShapeSupport(const Capsule* capsule, const Vec3s& dir, + Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); support.setZero(); if (dir[2] > dummy_precision) { support[2] = capsule->halfLength; @@ -213,17 +213,17 @@ getShapeSupportTplInstantiation(Capsule); // ============================================================================ template -void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); // The cone radius is, for -h < z < h, (h - z) * r / (2*h) // The inflate value is simply to make the specialized functions with cone // have a preferred side for edge cases. - static const FCL_REAL inflate = 1 + 1e-10; - FCL_REAL h = cone->halfLength; - FCL_REAL r = cone->radius; + static const CoalScalar inflate = 1 + 1e-10; + CoalScalar h = cone->halfLength; + CoalScalar r = cone->radius; if (dir.head<2>().isZero(dummy_precision)) { support.head<2>().setZero(); @@ -233,22 +233,22 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support, support[2] = -inflate * h; } } else { - FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; - FCL_REAL len = zdist + dir[2] * dir[2]; + CoalScalar zdist = dir[0] * dir[0] + dir[1] * dir[1]; + CoalScalar len = zdist + dir[2] * dir[2]; zdist = std::sqrt(zdist); if (dir[2] <= 0) { - FCL_REAL rad = r / zdist; + CoalScalar rad = r / zdist; support.head<2>() = rad * dir.head<2>(); support[2] = -h; } else { len = std::sqrt(len); - FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h); + CoalScalar sin_a = r / std::sqrt(r * r + 4 * h * h); if (dir[2] > len * sin_a) support << 0, 0, h; else { - FCL_REAL rad = r / zdist; + CoalScalar rad = r / zdist; support.head<2>() = rad * dir.head<2>(); support[2] = -h; } @@ -263,16 +263,16 @@ getShapeSupportTplInstantiation(Cone); // ============================================================================ template -void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support, int& /*unused*/, ShapeSupportData& /*unused*/) { - static const FCL_REAL dummy_precision = - Eigen::NumTraits::dummy_precision(); + static const CoalScalar dummy_precision = + Eigen::NumTraits::dummy_precision(); // The inflate value is simply to make the specialized functions with cylinder // have a preferred side for edge cases. - static const FCL_REAL inflate = 1 + 1e-10; - FCL_REAL half_h = cylinder->halfLength; - FCL_REAL r = cylinder->radius; + static const CoalScalar inflate = 1 + 1e-10; + CoalScalar half_h = cylinder->halfLength; + CoalScalar r = cylinder->radius; const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision); if (dir_is_aligned_with_z) half_h *= inflate; @@ -293,7 +293,7 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support, } assert(fabs(support[0] * dir[1] - support[1] * dir[0]) < - sqrt(std::numeric_limits::epsilon())); + sqrt(std::numeric_limits::epsilon())); if (_SupportOptions == SupportOptions::WithSweptSphere) { support += cylinder->getSweptSphereRadius() * dir.normalized(); @@ -303,23 +303,23 @@ getShapeSupportTplInstantiation(Cylinder); // ============================================================================ template -void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, +void getShapeSupportLog(const ConvexBase* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& support_data) { assert(convex->neighbors != nullptr && "Convex has no neighbors."); // Use warm start if current support direction is distant from last support // direction. const double use_warm_start_threshold = 0.9; - Vec3f dir_normalized = dir.normalized(); + Vec3s dir_normalized = dir.normalized(); if (!support_data.last_dir.isZero() && !convex->support_warm_starts.points.empty() && support_data.last_dir.dot(dir_normalized) < use_warm_start_threshold) { // Change hint if last dir is too far from current dir. - FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir); + CoalScalar maxdot = convex->support_warm_starts.points[0].dot(dir); hint = convex->support_warm_starts.indices[0]; for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) { - FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir); + CoalScalar dot = convex->support_warm_starts.points[i].dot(dir); if (dot > maxdot) { maxdot = dot; hint = convex->support_warm_starts.indices[i]; @@ -328,13 +328,13 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, } support_data.last_dir = dir_normalized; - const std::vector& pts = *(convex->points); + const std::vector& pts = *(convex->points); const std::vector& nn = *(convex->neighbors); if (hint < 0 || hint >= (int)convex->num_points) { hint = 0; } - FCL_REAL maxdot = pts[static_cast(hint)].dot(dir); + CoalScalar maxdot = pts[static_cast(hint)].dot(dir); std::vector& visited = support_data.visited; if (support_data.visited.size() == convex->num_points) { std::fill(visited.begin(), visited.end(), false); @@ -356,7 +356,7 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, const unsigned int ip = n[in]; if (visited[ip]) continue; visited[ip] = true; - const FCL_REAL dot = pts[ip].dot(dir); + const CoalScalar dot = pts[ip].dot(dir); bool better = false; if (dot > maxdot) { better = true; @@ -380,15 +380,15 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir, // ============================================================================ template -void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, - Vec3f& support, int& hint, +void getShapeSupportLinear(const ConvexBase* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& /*unused*/) { - const std::vector& pts = *(convex->points); + const std::vector& pts = *(convex->points); hint = 0; - FCL_REAL maxdot = pts[0].dot(dir); + CoalScalar maxdot = pts[0].dot(dir); for (int i = 1; i < (int)convex->num_points; ++i) { - FCL_REAL dot = pts[static_cast(i)].dot(dir); + CoalScalar dot = pts[static_cast(i)].dot(dir); if (dot > maxdot) { maxdot = dot; hint = i; @@ -404,7 +404,7 @@ void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir, // ============================================================================ template -void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, +void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support, int& hint, ShapeSupportData& support_data) { // TODO add benchmark to set a proper value for switching between linear and // logarithmic. @@ -417,15 +417,13 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support, support_data); } } -// clang-format off -getShapeSupportTplInstantiation(ConvexBase) - // clang-format on - - // ============================================================================ - template - inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, - ShapeSupportData& support_data) { +getShapeSupportTplInstantiation(ConvexBase); + +// ============================================================================ +template +inline void getShapeSupport(const SmallConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, + ShapeSupportData& support_data) { getShapeSupportLinear<_SupportOptions>( reinterpret_cast(convex), dir, support, hint, support_data); @@ -434,8 +432,8 @@ getShapeSupportTplInstantiation(SmallConvex); // ============================================================================ template -inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir, - Vec3f& support, int& hint, +inline void getShapeSupport(const LargeConvex* convex, const Vec3s& dir, + Vec3s& support, int& hint, ShapeSupportData& support_data) { getShapeSupportLog<_SupportOptions>( reinterpret_cast(convex), dir, support, hint, @@ -450,7 +448,7 @@ getShapeSupportTplInstantiation(LargeConvex); max_num_supports, tol) template void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, - size_t max_num_supports, FCL_REAL tol) { + size_t max_num_supports, CoalScalar tol) { ShapeSupportData support_data; switch (shape->getNodeType()) { case GEOM_TRIANGLE: @@ -486,38 +484,38 @@ void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint, // Explicit instantiation // clang-format off -template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); +template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar); -template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL); +template COAL_DLLAPI void getSupportSet(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar); // clang-format on // ============================================================================ #define getShapeSupportSetTplInstantiation(ShapeType) \ template COAL_DLLAPI void getShapeSupportSet( \ const ShapeType* shape_, SupportSet& support_set, int& hint, \ - ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); \ + ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol); \ \ template COAL_DLLAPI void \ getShapeSupportSet( \ const ShapeType* shape_, SupportSet& support_set, int& hint, \ - ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); + ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol); // ============================================================================ template void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL tol) { + size_t /*unused*/, CoalScalar tol) { assert(tol > 0); support_set.clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); // We simply want to compute the support value, no need to take the // swept-sphere radius into account. getShapeSupport(triangle, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); if (support_value - support_dir.dot(triangle->a) < tol) { // Note: at the moment, it's useless to take into account the @@ -553,36 +551,36 @@ getShapeSupportSetTplInstantiation(TriangleP); template void getShapeSupportSet(const Box* box, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, - size_t /*unused*/, FCL_REAL tol) { + size_t /*unused*/, CoalScalar tol) { assert(tol > 0); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(box, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); - - const FCL_REAL x = box->halfSide[0]; - const FCL_REAL y = box->halfSide[1]; - const FCL_REAL z = box->halfSide[2]; - const std::array corners = { - Vec3f(x, y, z), Vec3f(-x, y, z), Vec3f(-x, -y, z), Vec3f(x, -y, z), - Vec3f(x, y, -z), Vec3f(-x, y, -z), Vec3f(-x, -y, -z), Vec3f(x, -y, -z), + const CoalScalar support_value = support.dot(support_dir); + + const CoalScalar x = box->halfSide[0]; + const CoalScalar y = box->halfSide[1]; + const CoalScalar z = box->halfSide[2]; + const std::array corners = { + Vec3s(x, y, z), Vec3s(-x, y, z), Vec3s(-x, -y, z), Vec3s(x, -y, z), + Vec3s(x, y, -z), Vec3s(-x, y, -z), Vec3s(-x, -y, -z), Vec3s(x, -y, -z), }; SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); const Transform3f& tf = support_set.tf; - for (const Vec3f& corner : corners) { - const FCL_REAL val = corner.dot(support_dir); + for (const Vec3s& corner : corners) { + const CoalScalar val = corner.dot(support_dir); if (support_value - val < tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec2f p = + const Vec2s p = tf.inverseTransform(corner + box->getSweptSphereRadius() * support_dir) .template head<2>(); polygon.emplace_back(p); } else { - const Vec2f p = tf.inverseTransform(corner).template head<2>(); + const Vec2s p = tf.inverseTransform(corner).template head<2>(); polygon.emplace_back(p); } } @@ -596,11 +594,11 @@ template void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL /*unused*/) { + size_t /*unused*/, CoalScalar /*unused*/) { support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport<_SupportOptions>(sphere, support_dir, support, hint, support_data); support_set.addPoint(support); @@ -611,11 +609,11 @@ getShapeSupportSetTplInstantiation(Sphere); template void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set, int& hint, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL /*unused*/) { + size_t /*unused*/, CoalScalar /*unused*/) { support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport<_SupportOptions>(ellipsoid, support_dir, support, hint, support_data); support_set.addPoint(support); @@ -627,28 +625,28 @@ template void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t /*unused*/, FCL_REAL tol) { + size_t /*unused*/, CoalScalar tol) { // clang-format on assert(tol > 0); support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(capsule, support_dir, support, hint, support_data); - const FCL_REAL support_value = + const CoalScalar support_value = support_dir.dot(support + capsule->radius * support_dir); // The support set of a capsule has either 2 points or 1 point. // The two candidate points lie at the frontier between the cylinder and // sphere parts of the capsule. - const FCL_REAL h = capsule->halfLength; - const FCL_REAL r = capsule->radius; - const Vec3f p1(r * support_dir[0], r * support_dir[1], h); - const Vec3f p2(r * support_dir[0], r * support_dir[1], -h); + const CoalScalar h = capsule->halfLength; + const CoalScalar r = capsule->radius; + const Vec3s p1(r * support_dir[0], r * support_dir[1], h); + const Vec3s p2(r * support_dir[0], r * support_dir[1], -h); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius(); + const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius(); support_set.addPoint(p1 + ssr_vec); support_set.addPoint(p2 + ssr_vec); } else { @@ -657,7 +655,7 @@ void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set, } } else { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius(); + const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius(); support_set.addPoint(support + ssr_vec); } else { support_set.addPoint(support); @@ -671,29 +669,29 @@ template void getShapeSupportSet(const Cone* cone, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t num_sampled_supports, FCL_REAL tol) { + size_t num_sampled_supports, CoalScalar tol) { assert(tol > 0); support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(cone, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); // If the support direction is perpendicular to the cone's basis, there is an // infinite amount of support points; otherwise there are up to two support // points (two if direction is perpendicular to the side of the cone and one // otherwise). // - // To check this condition, we look at two points on the cone's basis; these - // two points are symmetrical w.r.t the center of the circle. If both these - // points are tol away from the support plane, then all the points of the - // circle are tol away from the support plane. - const FCL_REAL r = cone->radius; - const FCL_REAL z = -cone->halfLength; - const Vec3f p1(r * support_dir[0], r * support_dir[1], z); - const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z); + // To check this condition, we look at two points on the cone's basis; + // these two points are symmetrical w.r.t the center of the circle. If + // both these points are tol away from the support plane, then all the + // points of the circle are tol away from the support plane. + const CoalScalar r = cone->radius; + const CoalScalar z = -cone->halfLength; + const Vec3s p1(r * support_dir[0], r * support_dir[1], z); + const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { @@ -701,11 +699,11 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, // the basis of the cone. We sample `num_sampled_supports` points on the // base of the cone. We are guaranteed that these points like at a distance // tol of the support plane. - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports)); + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports)); for (size_t i = 0; i < num_sampled_supports; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; - Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); + const CoalScalar theta = (CoalScalar)(i)*angle_increment; + Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_cone_base += cone->getSweptSphereRadius() * support_dir; @@ -716,7 +714,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, // There are two potential supports to add: the tip of the cone and a point // on the basis of the cone. We compare each of these points to the support // value. - Vec3f cone_tip(0, 0, cone->halfLength); + Vec3s cone_tip(0, 0, cone->halfLength); if (support_value - support_dir.dot(cone_tip) <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { cone_tip += cone->getSweptSphereRadius() * support_dir; @@ -724,7 +722,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set, support_set.addPoint(cone_tip); } - Vec3f point_on_cone_base = Vec3f(cone->radius * support_dir[0], // + Vec3s point_on_cone_base = Vec3s(cone->radius * support_dir[0], // cone->radius * support_dir[1], // z); if (support_value - support_dir.dot(point_on_cone_base) <= tol) { @@ -742,31 +740,31 @@ template void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t num_sampled_supports, FCL_REAL tol) { + size_t num_sampled_supports, CoalScalar tol) { assert(tol > 0); support_set.points().clear(); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(cylinder, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); // The following is very similar to what is done for Cone's support set // computation. - const FCL_REAL r = cylinder->radius; - const FCL_REAL z = + const CoalScalar r = cylinder->radius; + const CoalScalar z = support_dir[2] <= 0 ? -cylinder->halfLength : cylinder->halfLength; - const Vec3f p1(r * support_dir[0], r * support_dir[1], z); - const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z); + const Vec3s p1(r * support_dir[0], r * support_dir[1], z); + const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z); if ((support_value - support_dir.dot(p1) <= tol) && (support_value - support_dir.dot(p2) <= tol)) { - const FCL_REAL angle_increment = - 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports)); + const CoalScalar angle_increment = + 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports)); for (size_t i = 0; i < num_sampled_supports; ++i) { - const FCL_REAL theta = (FCL_REAL)(i)*angle_increment; - Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); + const CoalScalar theta = (CoalScalar)(i)*angle_increment; + Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z); assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol); if (_SupportOptions == SupportOptions::WithSweptSphere) { point_on_cone_base += cylinder->getSweptSphereRadius() * support_dir; @@ -776,7 +774,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, } else { // There are two potential supports to add: one on each circle bases of the // cylinder. - Vec3f point_on_lower_circle = Vec3f(cylinder->radius * support_dir[0], // + Vec3s point_on_lower_circle = Vec3s(cylinder->radius * support_dir[0], // cylinder->radius * support_dir[1], // -cylinder->halfLength); if (support_value - support_dir.dot(point_on_lower_circle) <= tol) { @@ -786,7 +784,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set, support_set.addPoint(point_on_lower_circle); } - Vec3f point_on_upper_circle = Vec3f(cylinder->radius * support_dir[0], // + Vec3s point_on_upper_circle = Vec3s(cylinder->radius * support_dir[0], // cylinder->radius * support_dir[1], // cylinder->halfLength); if (support_value - support_dir.dot(point_on_upper_circle) <= tol) { @@ -804,29 +802,29 @@ template void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data, size_t /*unused*/, - FCL_REAL tol) { + CoalScalar tol) { assert(tol > 0); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupport(convex, support_dir, support, hint, support_data); - const FCL_REAL support_value = support_dir.dot(support); + const CoalScalar support_value = support_dir.dot(support); - const std::vector& points = *(convex->points); + const std::vector& points = *(convex->points); SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); const Transform3f& tf = support_set.tf; - for (const Vec3f& point : points) { - const FCL_REAL dot = support_dir.dot(point); + for (const Vec3s& point : points) { + const CoalScalar dot = support_dir.dot(point); if (support_value - dot <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec2f p = + const Vec2s p = tf.inverseTransform(point + convex->getSweptSphereRadius() * support_dir) .template head<2>(); polygon.emplace_back(p); } else { - const Vec2f p = tf.inverseTransform(point).template head<2>(); + const Vec2s p = tf.inverseTransform(point).template head<2>(); polygon.emplace_back(p); } } @@ -838,30 +836,30 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set, // ============================================================================ template void convexSupportSetRecurse( - const std::vector& points, + const std::vector& points, const std::vector& neighbors, - const FCL_REAL swept_sphere_radius, const size_t vertex_idx, - const Vec3f& support_dir, const FCL_REAL support_value, + const CoalScalar swept_sphere_radius, const size_t vertex_idx, + const Vec3s& support_dir, const CoalScalar support_value, const Transform3f& tf, std::vector& visited, - SupportSet::Polygon& polygon, FCL_REAL tol) { - HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius); + SupportSet::Polygon& polygon, CoalScalar tol) { + COAL_UNUSED_VARIABLE(swept_sphere_radius); if (visited[vertex_idx]) { return; } visited[vertex_idx] = true; - const Vec3f& point = points[vertex_idx]; - const FCL_REAL val = point.dot(support_dir); + const Vec3s& point = points[vertex_idx]; + const CoalScalar val = point.dot(support_dir); if (support_value - val <= tol) { if (_SupportOptions == SupportOptions::WithSweptSphere) { - const Vec2f p = + const Vec2s p = tf.inverseTransform(point + swept_sphere_radius * support_dir) .template head<2>(); polygon.emplace_back(p); } else { - const Vec2f p = tf.inverseTransform(point).template head<2>(); + const Vec2s p = tf.inverseTransform(point).template head<2>(); polygon.emplace_back(p); } @@ -879,17 +877,17 @@ void convexSupportSetRecurse( template void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, - size_t /*unused*/, FCL_REAL tol) { + size_t /*unused*/, CoalScalar tol) { assert(tol > 0); - Vec3f support; - const Vec3f& support_dir = support_set.getNormal(); + Vec3s support; + const Vec3s& support_dir = support_set.getNormal(); getShapeSupportLog( convex, support_dir, support, hint, support_data); - const FCL_REAL support_value = support.dot(support_dir); + const CoalScalar support_value = support.dot(support_dir); - const std::vector& points = *(convex->points); + const std::vector& points = *(convex->points); const std::vector& neighbors = *(convex->neighbors); - const FCL_REAL swept_sphere_radius = convex->getSweptSphereRadius(); + const CoalScalar swept_sphere_radius = convex->getSweptSphereRadius(); std::vector& visited = support_data.visited; // `visited` is guaranteed to be of right size due to previous call to convex // log support function. @@ -911,7 +909,8 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set, template void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, - size_t num_sampled_supports /*unused*/, FCL_REAL tol) { + size_t num_sampled_supports /*unused*/, + CoalScalar tol) { if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold && convex->neighbors != nullptr) { getShapeSupportSetLog<_SupportOptions>( @@ -928,7 +927,8 @@ template void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set, int& hint /*unused*/, ShapeSupportData& support_data /*unused*/, - size_t num_sampled_supports /*unused*/, FCL_REAL tol) { + size_t num_sampled_supports /*unused*/, + CoalScalar tol) { getShapeSupportSetLinear<_SupportOptions>( reinterpret_cast(convex), support_set, hint, support_data, num_sampled_supports, tol); @@ -939,7 +939,8 @@ getShapeSupportSetTplInstantiation(SmallConvex); template void getShapeSupportSet(const LargeConvex* convex, SupportSet& support_set, int& hint, ShapeSupportData& support_data, - size_t num_sampled_supports /*unused*/, FCL_REAL tol) { + size_t num_sampled_supports /*unused*/, + CoalScalar tol) { getShapeSupportSetLog<_SupportOptions>( reinterpret_cast(convex), support_set, hint, support_data, num_sampled_supports, tol); @@ -954,7 +955,7 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud, if (cloud.size() <= 2) { // Point or segment, nothing to do. - for (const Vec2f& point : cloud) { + for (const Vec2s& point : cloud) { cvx_hull.emplace_back(point); } return; @@ -971,16 +972,16 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud, if (cloud[0](1) > cloud[2](1)) { std::swap(cloud[0], cloud[2]); } - const Vec2f& a = cloud[0]; - const Vec2f& b = cloud[1]; - const Vec2f& c = cloud[2]; - const FCL_REAL det = + const Vec2s& a = cloud[0]; + const Vec2s& b = cloud[1]; + const Vec2s& c = cloud[2]; + const CoalScalar det = (b(0) - a(0)) * (c(1) - a(1)) - (b(1) - a(1)) * (c(0) - a(0)); if (det < 0) { std::swap(cloud[1], cloud[2]); } - for (const Vec2f& point : cloud) { + for (const Vec2s& point : cloud) { cvx_hull.emplace_back(point); } return; @@ -994,9 +995,9 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud, // in the direction (0, -1) (take the element of the set which has the lowest // y coordinate). size_t support_idx = 0; - FCL_REAL support_val = cloud[0](1); + CoalScalar support_val = cloud[0](1); for (size_t i = 1; i < cloud.size(); ++i) { - const FCL_REAL val = cloud[i](1); + const CoalScalar val = cloud[i](1); if (val < support_val) { support_val = val; support_idx = i; @@ -1005,17 +1006,17 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud, std::swap(cloud[0], cloud[support_idx]); cvx_hull.clear(); cvx_hull.emplace_back(cloud[0]); - const Vec2f& v = cvx_hull[0]; + const Vec2s& v = cvx_hull[0]; // Step 2 - Sort the rest of the point cloud according to the angle made with // v. Note: we use stable_sort instead of sort because sort can fail if two // values are identical. std::stable_sort( - cloud.begin() + 1, cloud.end(), [&v](const Vec2f& p1, const Vec2f& p2) { + cloud.begin() + 1, cloud.end(), [&v](const Vec2s& p1, const Vec2s& p2) { // p1 is "smaller" than p2 if det(p1 - v, p2 - v) >= 0 - const FCL_REAL det = + const CoalScalar det = (p1(0) - v(0)) * (p2(1) - v(1)) - (p1(1) - v(1)) * (p2(0) - v(0)); - if (std::abs(det) <= Eigen::NumTraits::dummy_precision()) { + if (std::abs(det) <= Eigen::NumTraits::dummy_precision()) { // If two points are identical or (v, p1, p2) are colinear, p1 is // "smaller" if it is closer to v. return ((p1 - v).squaredNorm() <= (p2 - v).squaredNorm()); @@ -1027,14 +1028,14 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud, // to the cvx-hull if they successively form "left turns" only. A left turn // is: considering the last three points of the cvx-hull, if they form a // right-hand basis (determinant > 0) then they make a left turn. - auto isRightSided = [](const Vec2f& p1, const Vec2f& p2, const Vec2f& p3) { + auto isRightSided = [](const Vec2s& p1, const Vec2s& p2, const Vec2s& p3) { // Checks if (p2 - p1, p3 - p1) forms a right-sided base based on // det(p2 - p1, p3 - p1) - const FCL_REAL det = + const CoalScalar det = (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(1)) * (p3(0) - p1(0)); // Note: we set a dummy precision threshold so that identical points or // colinear pionts are not added to the cvx-hull. - return det > Eigen::NumTraits::dummy_precision(); + return det > Eigen::NumTraits::dummy_precision(); }; // We initialize the cvx-hull algo by adding the first three @@ -1043,9 +1044,9 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud, // to form a right sided basis, hence to form a left turn. size_t cloud_beginning_idx = 1; while (cvx_hull.size() < 3) { - const Vec2f& vec = cloud[cloud_beginning_idx]; + const Vec2s& vec = cloud[cloud_beginning_idx]; if ((cvx_hull.back() - vec).squaredNorm() > - Eigen::NumTraits::epsilon()) { + Eigen::NumTraits::epsilon()) { cvx_hull.emplace_back(vec); } ++cloud_beginning_idx; @@ -1056,7 +1057,7 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud, // When we do a turn in the correct direction, we add a point to the // convex-hull. for (size_t i = cloud_beginning_idx; i < cloud.size(); ++i) { - const Vec2f& vec = cloud[i]; + const Vec2s& vec = cloud[i]; while (cvx_hull.size() > 1 && !isRightSided(cvx_hull[cvx_hull.size() - 2], cvx_hull[cvx_hull.size() - 1], vec)) { diff --git a/src/octree.cpp b/src/octree.cpp index 6ca93eff0..186ad51a9 100644 --- a/src/octree.cpp +++ b/src/octree.cpp @@ -55,13 +55,13 @@ struct Neighbors { void hasNeighboordPlusZ() { value |= 0x20; } }; // struct neighbors -void computeNeighbors(const std::vector& boxes, +void computeNeighbors(const std::vector& boxes, std::vector& neighbors) { - typedef std::vector VectorVec6f; + typedef std::vector VectorVec6s; CoalScalar fixedSize = -1; CoalScalar e(1e-8); for (std::size_t i = 0; i < boxes.size(); ++i) { - const Vec6f& box(boxes[i]); + const Vec6s& box(boxes[i]); Neighbors& n(neighbors[i]); CoalScalar x(box[0]); CoalScalar y(box[1]); @@ -72,9 +72,9 @@ void computeNeighbors(const std::vector& boxes, else assert(s == fixedSize); - for (VectorVec6f::const_iterator it = boxes.begin(); it != boxes.end(); + for (VectorVec6s::const_iterator it = boxes.begin(); it != boxes.end(); ++it) { - const Vec6f& otherBox = *it; + const Vec6s& otherBox = *it; CoalScalar xo(otherBox[0]); CoalScalar yo(otherBox[1]); CoalScalar zo(otherBox[2]); @@ -106,20 +106,20 @@ void computeNeighbors(const std::vector& boxes, } // namespace internal void OcTree::exportAsObjFile(const std::string& filename) const { - std::vector boxes(this->toBoxes()); + std::vector boxes(this->toBoxes()); std::vector neighbors(boxes.size()); internal::computeNeighbors(boxes, neighbors); // compute list of vertices and faces - typedef std::vector VectorVec3f; - std::vector vertices; + typedef std::vector VectorVec3s; + std::vector vertices; typedef std::array Array4; typedef std::vector VectorArray4; std::vector faces; for (std::size_t i = 0; i < boxes.size(); ++i) { - const Vec6f& box(boxes[i]); + const Vec6s& box(boxes[i]); internal::Neighbors& n(neighbors[i]); CoalScalar x(box[0]); @@ -127,14 +127,14 @@ void OcTree::exportAsObjFile(const std::string& filename) const { CoalScalar z(box[2]); CoalScalar size(box[3]); - vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z - .5 * size)); - vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z + .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z + .5 * size)); - vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z + .5 * size)); - vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z - .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z + .5 * size)); + vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z + .5 * size)); // Add face only if box has no neighbor with the same face if (!n.minusX()) { @@ -172,9 +172,9 @@ void OcTree::exportAsObjFile(const std::string& filename) const { std::runtime_error); // write vertices os << "# list of vertices\n"; - for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end(); + for (VectorVec3s::const_iterator it = vertices.begin(); it != vertices.end(); ++it) { - const Vec3f& v = *it; + const Vec3s& v = *it; os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n'; } os << "\n# list of faces\n"; diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index 98beb82a6..e32677a03 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -22,16 +22,16 @@ namespace coal { // Reorders `tri` such that the dot product between the normal of triangle and // the vector `triangle barycentre - convex_tri.center` is positive. void reorderTriangle(const Convex* convex_tri, Triangle& tri) { - Vec3f p0, p1, p2; + Vec3s p0, p1, p2; p0 = (*(convex_tri->points))[tri[0]]; p1 = (*(convex_tri->points))[tri[1]]; p2 = (*(convex_tri->points))[tri[2]]; - Vec3f barycentre_tri, center_barycenter; + Vec3s barycentre_tri, center_barycenter; barycentre_tri = (p0 + p1 + p2) / 3; center_barycenter = barycentre_tri - convex_tri->center; - Vec3f edge_tri1, edge_tri2, n_tri; + Vec3s edge_tri1, edge_tri2, n_tri; edge_tri1 = p1 - p0; edge_tri2 = p2 - p1; n_tri = edge_tri1.cross(edge_tri2); @@ -41,7 +41,7 @@ void reorderTriangle(const Convex* convex_tri, Triangle& tri) { } } -ConvexBase* ConvexBase::convexHull(std::shared_ptr>& pts, +ConvexBase* ConvexBase::convexHull(std::shared_ptr>& pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { COAL_COMPILER_DIAGNOSTIC_PUSH @@ -51,7 +51,7 @@ ConvexBase* ConvexBase::convexHull(std::shared_ptr>& pts, COAL_COMPILER_DIAGNOSTIC_POP } -ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, +ConvexBase* ConvexBase::convexHull(const Vec3s* pts, unsigned int num_points, bool keepTriangles, const char* qhullCommand) { #ifdef COAL_HAS_QHULL @@ -81,15 +81,15 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, // Initialize the vertices size_t nvertex = static_cast(qh.vertexCount()); - std::shared_ptr> vertices( - new std::vector(size_t(nvertex))); + std::shared_ptr> vertices( + new std::vector(size_t(nvertex))); QhullVertexList vertexList(qh.vertexList()); size_t i_vertex = 0; for (QhullVertexList::const_iterator v = vertexList.begin(); v != vertexList.end(); ++v) { QhullPoint pt((*v).point()); pts_to_vertices[(size_t)pt.id()] = (int)i_vertex; - (*vertices)[i_vertex] = Vec3f(pt[0], pt[1], pt[2]); + (*vertices)[i_vertex] = Vec3s(pt[0], pt[1], pt[2]); ++i_vertex; } assert(i_vertex == nvertex); @@ -230,15 +230,15 @@ void ConvexBase::buildDoubleDescription() { void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) { num_normals_and_offsets = static_cast(qh.facetCount()); - normals.reset(new std::vector(num_normals_and_offsets)); - std::vector& normals_ = *normals; + normals.reset(new std::vector(num_normals_and_offsets)); + std::vector& normals_ = *normals; offsets.reset(new std::vector(num_normals_and_offsets)); std::vector& offsets_ = *offsets; unsigned int i_normal = 0; for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet(); facet = facet.next()) { const orgQhull::QhullHyperplane& plane = facet.hyperplane(); - normals_[i_normal] = Vec3f(plane.coordinates()[0], plane.coordinates()[1], + normals_[i_normal] = Vec3s(plane.coordinates()[0], plane.coordinates()[1], plane.coordinates()[2]); offsets_[i_normal] = plane.offset(); i_normal++; diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index ba7a20785..ab6c04a73 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -40,7 +40,7 @@ namespace coal { -void ConvexBase::initialize(std::shared_ptr> points_, +void ConvexBase::initialize(std::shared_ptr> points_, unsigned int num_points_) { this->points = points_; this->num_points = num_points_; @@ -54,7 +54,7 @@ void ConvexBase::initialize(std::shared_ptr> points_, this->computeCenter(); } -void ConvexBase::set(std::shared_ptr> points_, +void ConvexBase::set(std::shared_ptr> points_, unsigned int num_points_) { initialize(points_, num_points_); } @@ -66,7 +66,7 @@ ConvexBase::ConvexBase(const ConvexBase& other) center(other.center) { if (other.points.get() && other.points->size() > 0) { // Deep copy of other points - points.reset(new std::vector(*other.points)); + points.reset(new std::vector(*other.points)); } else points.reset(); @@ -93,7 +93,7 @@ ConvexBase::ConvexBase(const ConvexBase& other) nneighbors_.reset(); if (other.normals.get() && other.normals->size() > 0) { - normals.reset(new std::vector(*(other.normals))); + normals.reset(new std::vector(*(other.normals))); } else normals.reset(); @@ -111,7 +111,7 @@ ConvexBase::~ConvexBase() {} void ConvexBase::computeCenter() { center.setZero(); - const std::vector& points_ = *points; + const std::vector& points_ = *points; for (std::size_t i = 0; i < num_points; ++i) center += points_[i]; // TODO(jcarpent): vectorization center /= num_points; @@ -145,8 +145,8 @@ void Box::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -156,8 +156,8 @@ void Sphere::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = radius; @@ -167,8 +167,8 @@ void Ellipsoid::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -178,8 +178,8 @@ void Capsule::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -189,8 +189,8 @@ void Cone::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -200,8 +200,8 @@ void Cylinder::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -211,8 +211,8 @@ void ConvexBase::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -222,8 +222,8 @@ void Halfspace::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -233,8 +233,8 @@ void Plane::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); @@ -244,8 +244,8 @@ void TriangleP::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { - aabb_local.min_ -= Vec3f::Constant(ssr); - aabb_local.max_ += Vec3f::Constant(ssr); + aabb_local.min_ -= Vec3s::Constant(ssr); + aabb_local.max_ += Vec3s::Constant(ssr); } aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).norm(); diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index b5d4bf2fc..119607292 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -43,52 +43,52 @@ namespace coal { namespace details { -std::vector getBoundVertices(const Box& box, const Transform3f& tf) { - std::vector result(8); +std::vector getBoundVertices(const Box& box, const Transform3f& tf) { + std::vector result(8); CoalScalar a = box.halfSide[0]; CoalScalar b = box.halfSide[1]; CoalScalar c = box.halfSide[2]; - result[0] = tf.transform(Vec3f(a, b, c)); - result[1] = tf.transform(Vec3f(a, b, -c)); - result[2] = tf.transform(Vec3f(a, -b, c)); - result[3] = tf.transform(Vec3f(a, -b, -c)); - result[4] = tf.transform(Vec3f(-a, b, c)); - result[5] = tf.transform(Vec3f(-a, b, -c)); - result[6] = tf.transform(Vec3f(-a, -b, c)); - result[7] = tf.transform(Vec3f(-a, -b, -c)); + result[0] = tf.transform(Vec3s(a, b, c)); + result[1] = tf.transform(Vec3s(a, b, -c)); + result[2] = tf.transform(Vec3s(a, -b, c)); + result[3] = tf.transform(Vec3s(a, -b, -c)); + result[4] = tf.transform(Vec3s(-a, b, c)); + result[5] = tf.transform(Vec3s(-a, b, -c)); + result[6] = tf.transform(Vec3s(-a, -b, c)); + result[7] = tf.transform(Vec3s(-a, -b, -c)); return result; } // we use icosahedron to bound the sphere -std::vector getBoundVertices(const Sphere& sphere, +std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf) { - std::vector result(12); + std::vector result(12); const CoalScalar m = (1 + sqrt(5.0)) / 2.0; CoalScalar edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); CoalScalar a = edge_size; CoalScalar b = m * edge_size; - result[0] = tf.transform(Vec3f(0, a, b)); - result[1] = tf.transform(Vec3f(0, -a, b)); - result[2] = tf.transform(Vec3f(0, a, -b)); - result[3] = tf.transform(Vec3f(0, -a, -b)); - result[4] = tf.transform(Vec3f(a, b, 0)); - result[5] = tf.transform(Vec3f(-a, b, 0)); - result[6] = tf.transform(Vec3f(a, -b, 0)); - result[7] = tf.transform(Vec3f(-a, -b, 0)); - result[8] = tf.transform(Vec3f(b, 0, a)); - result[9] = tf.transform(Vec3f(b, 0, -a)); - result[10] = tf.transform(Vec3f(-b, 0, a)); - result[11] = tf.transform(Vec3f(-b, 0, -a)); + result[0] = tf.transform(Vec3s(0, a, b)); + result[1] = tf.transform(Vec3s(0, -a, b)); + result[2] = tf.transform(Vec3s(0, a, -b)); + result[3] = tf.transform(Vec3s(0, -a, -b)); + result[4] = tf.transform(Vec3s(a, b, 0)); + result[5] = tf.transform(Vec3s(-a, b, 0)); + result[6] = tf.transform(Vec3s(a, -b, 0)); + result[7] = tf.transform(Vec3s(-a, -b, 0)); + result[8] = tf.transform(Vec3s(b, 0, a)); + result[9] = tf.transform(Vec3s(b, 0, -a)); + result[10] = tf.transform(Vec3s(-b, 0, a)); + result[11] = tf.transform(Vec3s(-b, 0, -a)); return result; } // we use scaled icosahedron to bound the ellipsoid -std::vector getBoundVertices(const Ellipsoid& ellipsoid, +std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf) { - std::vector result(12); + std::vector result(12); const CoalScalar phi = (1 + sqrt(5.0)) / 2.0; const CoalScalar a = sqrt(3.0) / (phi * phi); @@ -104,25 +104,25 @@ std::vector getBoundVertices(const Ellipsoid& ellipsoid, CoalScalar Bb = B * b; CoalScalar Ca = C * a; CoalScalar Cb = C * b; - result[0] = tf.transform(Vec3f(0, Ba, Cb)); - result[1] = tf.transform(Vec3f(0, -Ba, Cb)); - result[2] = tf.transform(Vec3f(0, Ba, -Cb)); - result[3] = tf.transform(Vec3f(0, -Ba, -Cb)); - result[4] = tf.transform(Vec3f(Aa, Bb, 0)); - result[5] = tf.transform(Vec3f(-Aa, Bb, 0)); - result[6] = tf.transform(Vec3f(Aa, -Bb, 0)); - result[7] = tf.transform(Vec3f(-Aa, -Bb, 0)); - result[8] = tf.transform(Vec3f(Ab, 0, Ca)); - result[9] = tf.transform(Vec3f(Ab, 0, -Ca)); - result[10] = tf.transform(Vec3f(-Ab, 0, Ca)); - result[11] = tf.transform(Vec3f(-Ab, 0, -Ca)); + result[0] = tf.transform(Vec3s(0, Ba, Cb)); + result[1] = tf.transform(Vec3s(0, -Ba, Cb)); + result[2] = tf.transform(Vec3s(0, Ba, -Cb)); + result[3] = tf.transform(Vec3s(0, -Ba, -Cb)); + result[4] = tf.transform(Vec3s(Aa, Bb, 0)); + result[5] = tf.transform(Vec3s(-Aa, Bb, 0)); + result[6] = tf.transform(Vec3s(Aa, -Bb, 0)); + result[7] = tf.transform(Vec3s(-Aa, -Bb, 0)); + result[8] = tf.transform(Vec3s(Ab, 0, Ca)); + result[9] = tf.transform(Vec3s(Ab, 0, -Ca)); + result[10] = tf.transform(Vec3s(-Ab, 0, Ca)); + result[11] = tf.transform(Vec3s(-Ab, 0, -Ca)); return result; } -std::vector getBoundVertices(const Capsule& capsule, +std::vector getBoundVertices(const Capsule& capsule, const Transform3f& tf) { - std::vector result(36); + std::vector result(36); const CoalScalar m = (1 + sqrt(5.0)) / 2.0; CoalScalar hl = capsule.halfLength; @@ -131,101 +131,101 @@ std::vector getBoundVertices(const Capsule& capsule, CoalScalar b = m * edge_size; CoalScalar r2 = capsule.radius * 2 / sqrt(3.0); - result[0] = tf.transform(Vec3f(0, a, b + hl)); - result[1] = tf.transform(Vec3f(0, -a, b + hl)); - result[2] = tf.transform(Vec3f(0, a, -b + hl)); - result[3] = tf.transform(Vec3f(0, -a, -b + hl)); - result[4] = tf.transform(Vec3f(a, b, hl)); - result[5] = tf.transform(Vec3f(-a, b, hl)); - result[6] = tf.transform(Vec3f(a, -b, hl)); - result[7] = tf.transform(Vec3f(-a, -b, hl)); - result[8] = tf.transform(Vec3f(b, 0, a + hl)); - result[9] = tf.transform(Vec3f(b, 0, -a + hl)); - result[10] = tf.transform(Vec3f(-b, 0, a + hl)); - result[11] = tf.transform(Vec3f(-b, 0, -a + hl)); - - result[12] = tf.transform(Vec3f(0, a, b - hl)); - result[13] = tf.transform(Vec3f(0, -a, b - hl)); - result[14] = tf.transform(Vec3f(0, a, -b - hl)); - result[15] = tf.transform(Vec3f(0, -a, -b - hl)); - result[16] = tf.transform(Vec3f(a, b, -hl)); - result[17] = tf.transform(Vec3f(-a, b, -hl)); - result[18] = tf.transform(Vec3f(a, -b, -hl)); - result[19] = tf.transform(Vec3f(-a, -b, -hl)); - result[20] = tf.transform(Vec3f(b, 0, a - hl)); - result[21] = tf.transform(Vec3f(b, 0, -a - hl)); - result[22] = tf.transform(Vec3f(-b, 0, a - hl)); - result[23] = tf.transform(Vec3f(-b, 0, -a - hl)); + result[0] = tf.transform(Vec3s(0, a, b + hl)); + result[1] = tf.transform(Vec3s(0, -a, b + hl)); + result[2] = tf.transform(Vec3s(0, a, -b + hl)); + result[3] = tf.transform(Vec3s(0, -a, -b + hl)); + result[4] = tf.transform(Vec3s(a, b, hl)); + result[5] = tf.transform(Vec3s(-a, b, hl)); + result[6] = tf.transform(Vec3s(a, -b, hl)); + result[7] = tf.transform(Vec3s(-a, -b, hl)); + result[8] = tf.transform(Vec3s(b, 0, a + hl)); + result[9] = tf.transform(Vec3s(b, 0, -a + hl)); + result[10] = tf.transform(Vec3s(-b, 0, a + hl)); + result[11] = tf.transform(Vec3s(-b, 0, -a + hl)); + + result[12] = tf.transform(Vec3s(0, a, b - hl)); + result[13] = tf.transform(Vec3s(0, -a, b - hl)); + result[14] = tf.transform(Vec3s(0, a, -b - hl)); + result[15] = tf.transform(Vec3s(0, -a, -b - hl)); + result[16] = tf.transform(Vec3s(a, b, -hl)); + result[17] = tf.transform(Vec3s(-a, b, -hl)); + result[18] = tf.transform(Vec3s(a, -b, -hl)); + result[19] = tf.transform(Vec3s(-a, -b, -hl)); + result[20] = tf.transform(Vec3s(b, 0, a - hl)); + result[21] = tf.transform(Vec3s(b, 0, -a - hl)); + result[22] = tf.transform(Vec3s(-b, 0, a - hl)); + result[23] = tf.transform(Vec3s(-b, 0, -a - hl)); CoalScalar c = 0.5 * r2; CoalScalar d = capsule.radius; - result[24] = tf.transform(Vec3f(r2, 0, hl)); - result[25] = tf.transform(Vec3f(c, d, hl)); - result[26] = tf.transform(Vec3f(-c, d, hl)); - result[27] = tf.transform(Vec3f(-r2, 0, hl)); - result[28] = tf.transform(Vec3f(-c, -d, hl)); - result[29] = tf.transform(Vec3f(c, -d, hl)); - - result[30] = tf.transform(Vec3f(r2, 0, -hl)); - result[31] = tf.transform(Vec3f(c, d, -hl)); - result[32] = tf.transform(Vec3f(-c, d, -hl)); - result[33] = tf.transform(Vec3f(-r2, 0, -hl)); - result[34] = tf.transform(Vec3f(-c, -d, -hl)); - result[35] = tf.transform(Vec3f(c, -d, -hl)); + result[24] = tf.transform(Vec3s(r2, 0, hl)); + result[25] = tf.transform(Vec3s(c, d, hl)); + result[26] = tf.transform(Vec3s(-c, d, hl)); + result[27] = tf.transform(Vec3s(-r2, 0, hl)); + result[28] = tf.transform(Vec3s(-c, -d, hl)); + result[29] = tf.transform(Vec3s(c, -d, hl)); + + result[30] = tf.transform(Vec3s(r2, 0, -hl)); + result[31] = tf.transform(Vec3s(c, d, -hl)); + result[32] = tf.transform(Vec3s(-c, d, -hl)); + result[33] = tf.transform(Vec3s(-r2, 0, -hl)); + result[34] = tf.transform(Vec3s(-c, -d, -hl)); + result[35] = tf.transform(Vec3s(c, -d, -hl)); return result; } -std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { - std::vector result(7); +std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { + std::vector result(7); CoalScalar hl = cone.halfLength; CoalScalar r2 = cone.radius * 2 / sqrt(3.0); CoalScalar a = 0.5 * r2; CoalScalar b = cone.radius; - result[0] = tf.transform(Vec3f(r2, 0, -hl)); - result[1] = tf.transform(Vec3f(a, b, -hl)); - result[2] = tf.transform(Vec3f(-a, b, -hl)); - result[3] = tf.transform(Vec3f(-r2, 0, -hl)); - result[4] = tf.transform(Vec3f(-a, -b, -hl)); - result[5] = tf.transform(Vec3f(a, -b, -hl)); + result[0] = tf.transform(Vec3s(r2, 0, -hl)); + result[1] = tf.transform(Vec3s(a, b, -hl)); + result[2] = tf.transform(Vec3s(-a, b, -hl)); + result[3] = tf.transform(Vec3s(-r2, 0, -hl)); + result[4] = tf.transform(Vec3s(-a, -b, -hl)); + result[5] = tf.transform(Vec3s(a, -b, -hl)); - result[6] = tf.transform(Vec3f(0, 0, hl)); + result[6] = tf.transform(Vec3s(0, 0, hl)); return result; } -std::vector getBoundVertices(const Cylinder& cylinder, +std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& tf) { - std::vector result(12); + std::vector result(12); CoalScalar hl = cylinder.halfLength; CoalScalar r2 = cylinder.radius * 2 / sqrt(3.0); CoalScalar a = 0.5 * r2; CoalScalar b = cylinder.radius; - result[0] = tf.transform(Vec3f(r2, 0, -hl)); - result[1] = tf.transform(Vec3f(a, b, -hl)); - result[2] = tf.transform(Vec3f(-a, b, -hl)); - result[3] = tf.transform(Vec3f(-r2, 0, -hl)); - result[4] = tf.transform(Vec3f(-a, -b, -hl)); - result[5] = tf.transform(Vec3f(a, -b, -hl)); + result[0] = tf.transform(Vec3s(r2, 0, -hl)); + result[1] = tf.transform(Vec3s(a, b, -hl)); + result[2] = tf.transform(Vec3s(-a, b, -hl)); + result[3] = tf.transform(Vec3s(-r2, 0, -hl)); + result[4] = tf.transform(Vec3s(-a, -b, -hl)); + result[5] = tf.transform(Vec3s(a, -b, -hl)); - result[6] = tf.transform(Vec3f(r2, 0, hl)); - result[7] = tf.transform(Vec3f(a, b, hl)); - result[8] = tf.transform(Vec3f(-a, b, hl)); - result[9] = tf.transform(Vec3f(-r2, 0, hl)); - result[10] = tf.transform(Vec3f(-a, -b, hl)); - result[11] = tf.transform(Vec3f(a, -b, hl)); + result[6] = tf.transform(Vec3s(r2, 0, hl)); + result[7] = tf.transform(Vec3s(a, b, hl)); + result[8] = tf.transform(Vec3s(-a, b, hl)); + result[9] = tf.transform(Vec3s(-r2, 0, hl)); + result[10] = tf.transform(Vec3s(-a, -b, hl)); + result[11] = tf.transform(Vec3s(a, -b, hl)); return result; } -std::vector getBoundVertices(const ConvexBase& convex, +std::vector getBoundVertices(const ConvexBase& convex, const Transform3f& tf) { - std::vector result(convex.num_points); - const std::vector& points_ = *(convex.points); + std::vector result(convex.num_points); + const std::vector& points_ = *(convex.points); for (std::size_t i = 0; i < convex.num_points; ++i) { result[i] = tf.transform(points_[i]); } @@ -233,9 +233,9 @@ std::vector getBoundVertices(const ConvexBase& convex, return result; } -std::vector getBoundVertices(const TriangleP& triangle, +std::vector getBoundVertices(const TriangleP& triangle, const Transform3f& tf) { - std::vector result(3); + std::vector result(3); result[0] = tf.transform(triangle.a); result[1] = tf.transform(triangle.b); result[2] = tf.transform(triangle.c); @@ -252,7 +252,7 @@ Halfspace transform(const Halfspace& a, const Transform3f& tf) { /// where n' = R * n /// and d' = d + n' * T - Vec3f n = tf.getRotation() * a.n; + Vec3s n = tf.getRotation() * a.n; CoalScalar d = a.d + n.dot(tf.getTranslation()); Halfspace result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); @@ -267,7 +267,7 @@ Plane transform(const Plane& a, const Transform3f& tf) { /// where n' = R * n /// and d' = d + n' * T - Vec3f n = tf.getRotation() * a.n; + Vec3s n = tf.getRotation() * a.n; CoalScalar d = a.d + n.dot(tf.getTranslation()); Plane result(n, d); result.setSweptSphereRadius(a.getSweptSphereRadius()); @@ -279,7 +279,7 @@ std::array transformToHalfspaces(const Plane& a, const Transform3f& tf) { // A plane can be represented by two halfspaces - Vec3f n = tf.getRotation() * a.n; + Vec3s n = tf.getRotation() * a.n; CoalScalar d = a.d + n.dot(tf.getTranslation()); std::array result = {Halfspace(n, d), Halfspace(-n, -d)}; result[0].setSweptSphereRadius(a.getSweptSphereRadius()); @@ -290,19 +290,19 @@ std::array transformToHalfspaces(const Plane& a, template <> void computeBV(const Box& s, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta(R.cwiseAbs() * s.halfSide); + Vec3s v_delta(R.cwiseAbs() * s.halfSide); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) { - const Vec3f& T = tf.getTranslation(); + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta(Vec3f::Constant(s.radius)); + Vec3s v_delta(Vec3s::Constant(s.radius)); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } @@ -310,10 +310,10 @@ void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) { template <> void computeBV(const Ellipsoid& e, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta = R * e.radii; + Vec3s v_delta = R * e.radii; bv.max_ = T + v_delta; bv.min_ = T - v_delta; } @@ -321,18 +321,18 @@ void computeBV(const Ellipsoid& e, const Transform3f& tf, template <> void computeBV(const Capsule& s, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); - Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius)); + Vec3s v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3s::Constant(s.radius)); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template <> void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength); @@ -341,7 +341,7 @@ void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength); - Vec3f v_delta(x_range, y_range, z_range); + Vec3s v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } @@ -349,8 +349,8 @@ void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { template <> void computeBV(const Cylinder& s, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength); @@ -359,7 +359,7 @@ void computeBV(const Cylinder& s, const Transform3f& tf, CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength); - Vec3f v_delta(x_range, y_range, z_range); + Vec3s v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } @@ -367,13 +367,13 @@ void computeBV(const Cylinder& s, const Transform3f& tf, template <> void computeBV(const ConvexBase& s, const Transform3f& tf, AABB& bv) { - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); AABB bv_; - const std::vector& points_ = *(s.points); + const std::vector& points_ = *(s.points); for (std::size_t i = 0; i < s.num_points; ++i) { - Vec3f new_p = R * points_[i] + T; + Vec3s new_p = R * points_[i] + T; bv_ += new_p; } @@ -390,12 +390,12 @@ template <> void computeBV(const Halfspace& s, const Transform3f& tf, AABB& bv) { Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; AABB bv_; - bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); - bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); + bv_.min_ = Vec3s::Constant(-(std::numeric_limits::max)()); + bv_.max_ = Vec3s::Constant((std::numeric_limits::max)()); if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with x axis if (n[0] < 0) @@ -422,12 +422,12 @@ void computeBV(const Halfspace& s, const Transform3f& tf, template <> void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; AABB bv_; - bv_.min_ = Vec3f::Constant(-(std::numeric_limits::max)()); - bv_.max_ = Vec3f::Constant((std::numeric_limits::max)()); + bv_.min_ = Vec3s::Constant(-(std::numeric_limits::max)()); + bv_.max_ = Vec3s::Constant((std::numeric_limits::max)()); if (n[1] == (CoalScalar)0.0 && n[2] == (CoalScalar)0.0) { // normal aligned with x axis if (n[0] < 0) { @@ -460,8 +460,8 @@ void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To = T; bv.axes = R; @@ -474,7 +474,7 @@ void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - const Vec3f& T = tf.getTranslation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.setIdentity(); @@ -487,8 +487,8 @@ void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; @@ -501,8 +501,8 @@ void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; @@ -516,8 +516,8 @@ void computeBV(const Cylinder& s, const Transform3f& tf, COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); bv.To.noalias() = T; bv.axes.noalias() = R; @@ -531,8 +531,8 @@ void computeBV(const ConvexBase& s, const Transform3f& tf, COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - const Matrix3f& R = tf.getRotation(); - const Vec3f& T = tf.getTranslation(); + const Matrix3s& R = tf.getRotation(); + const Vec3s& T = tf.getTranslation(); fit(s.points->data(), s.num_points, bv); @@ -588,7 +588,7 @@ void computeBV(const Halfspace& s, const Transform3f& tf, } bv.num_spheres = 1; computeBV(s, tf, bv.obb); - bv.spheres[0].o = Vec3f(); + bv.spheres[0].o = Vec3s(); bv.spheres[0].r = (std::numeric_limits::max)(); } @@ -600,7 +600,7 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, std::runtime_error); } Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; const short D = 8; @@ -660,7 +660,7 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, std::runtime_error); } Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; const short D = 9; @@ -726,7 +726,7 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, std::runtime_error); } Halfspace new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; const short D = 12; @@ -805,14 +805,14 @@ void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - Vec3f n = tf.getRotation() * s.n; + Vec3s n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; bv.extent << 0, (std::numeric_limits::max)(), (std::numeric_limits::max)(); - Vec3f p = s.n * s.d; + Vec3s p = s.n * s.d; bv.To = tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T } @@ -823,7 +823,7 @@ void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); } - Vec3f n = tf.getRotation() * s.n; + Vec3s n = tf.getRotation() * s.n; generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); bv.axes.col(0).noalias() = n; @@ -833,7 +833,7 @@ void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { bv.radius = 0; - Vec3f p = s.n * s.d; + Vec3s p = s.n * s.d; bv.Tr = tf.transform(p); } @@ -856,7 +856,7 @@ void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { } bv.num_spheres = 1; computeBV(s, tf, bv.obb); - bv.spheres[0].o = Vec3f(); + bv.spheres[0].o = Vec3s(); bv.spheres[0].r = (std::numeric_limits::max)(); } @@ -868,7 +868,7 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, std::runtime_error); } Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; const short D = 8; @@ -914,7 +914,7 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, std::runtime_error); } Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; const short D = 9; @@ -962,7 +962,7 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, std::runtime_error); } Plane new_s = transform(s, tf); - const Vec3f& n = new_s.n; + const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; const short D = 12; diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp index 0526e4994..624c422a4 100644 --- a/test/accelerated_gjk.cpp +++ b/test/accelerated_gjk.cpp @@ -56,7 +56,7 @@ using coal::ShapeBase; using coal::support_func_guess_t; using coal::Transform3f; using coal::Triangle; -using coal::Vec3f; +using coal::Vec3s; using coal::details::GJK; using coal::details::MinkowskiDiff; using coal::details::SupportOptions; @@ -125,7 +125,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { Transform3f identity = Transform3f::Identity(); // Same init for both solvers - Vec3f init_guess = Vec3f(1, 0, 0); + Vec3s init_guess = Vec3s(1, 0, 0); support_func_guess_t init_support_guess; init_support_guess.setZero(); @@ -139,7 +139,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Evaluate both solvers twice, make sure they give the same solution GJK::Status res_gjk_1 = gjk.evaluate(mink_diff, init_guess, init_support_guess); - Vec3f ray_gjk = gjk.ray; + Vec3s ray_gjk = gjk.ray; GJK::Status res_gjk_2 = gjk.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res_gjk_1 == res_gjk_2); @@ -150,7 +150,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // -------------- GJK::Status res_nesterov_gjk_1 = gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess); - Vec3f ray_nesterov = gjk_nesterov.ray; + Vec3s ray_nesterov = gjk_nesterov.ray; GJK::Status res_nesterov_gjk_2 = gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res_nesterov_gjk_1 == res_nesterov_gjk_2); @@ -171,7 +171,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // ------------ GJK::Status res_polyak_gjk_1 = gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess); - Vec3f ray_polyak = gjk_polyak.ray; + Vec3s ray_polyak = gjk_polyak.ray; GJK::Status res_polyak_gjk_2 = gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res_polyak_gjk_1 == res_polyak_gjk_2); diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 584ceb649..2f7b37422 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -33,7 +33,7 @@ bool verbose = false; CoalScalar DELTA = 0.001; template -void makeModel(const std::vector& vertices, +void makeModel(const std::vector& vertices, const std::vector& triangles, SplitMethodType split_method, BVHModel& model); @@ -78,7 +78,7 @@ struct traits { }; template -void makeModel(const std::vector& vertices, +void makeModel(const std::vector& vertices, const std::vector& triangles, SplitMethodType split_method, BVHModel& model) { model.bv_splitter.reset(new BVSplitter(split_method)); @@ -166,7 +166,7 @@ double run(const std::vector& tf, } int main(int, char*[]) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp index 2633febdc..8a99e3b11 100644 --- a/test/box_box_collision.cpp +++ b/test/box_box_collision.cpp @@ -15,7 +15,7 @@ using coal::CollisionRequest; using coal::CollisionResult; using coal::ComputeCollision; using coal::Transform3f; -using coal::Vec3f; +using coal::Vec3s; BOOST_AUTO_TEST_CASE(box_box_collision) { // Define boxes @@ -33,13 +33,13 @@ BOOST_AUTO_TEST_CASE(box_box_collision) { CollisionResult res; ComputeCollision collide_functor(&shape1, &shape2); - T1.setTranslation(Vec3f(0, 0, 0)); + T1.setTranslation(Vec3s(0, 0, 0)); res.clear(); BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == true); res.clear(); BOOST_CHECK(collide_functor(T1, T2, req, res) == true); - T1.setTranslation(Vec3f(2, 0, 0)); + T1.setTranslation(Vec3s(2, 0, 0)); res.clear(); BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == false); res.clear(); diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp index e559ab2c1..dc5189a1e 100644 --- a/test/box_box_distance.cpp +++ b/test/box_box_distance.cpp @@ -57,14 +57,14 @@ using coal::CollisionObject; using coal::DistanceRequest; using coal::DistanceResult; using coal::Transform3f; -using coal::Vec3f; +using coal::Vec3s; BOOST_AUTO_TEST_CASE(distance_box_box_1) { CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); Transform3f tf1; - Transform3f tf2(Vec3f(25, 20, 5)); + Transform3f tf2(Vec3s(25, 20, 5)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -87,8 +87,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) { double dy = 20 - 5 - 1; double dz = 5 - 1 - 1; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, sqrt(dx * dx + dy * dy + dz * dz), 1e-4); @@ -107,7 +107,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { Transform3f tf1; Transform3f tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), - Vec3f(0, 0, 10)); + Vec3s(0, 0, 10)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -127,8 +127,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; double distance = -1.62123444 + 10 - 1; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); @@ -145,9 +145,9 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1)); static double pi = M_PI; Transform3f tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), - Vec3f(-2, 1, .5)); + Vec3s(-2, 1, .5)); Transform3f tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), - Vec3f(2, .5, .5)); + Vec3s(2, .5, .5)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -167,13 +167,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { << ", p2 = " << distanceResult.nearest_points[1] << ", distance = " << distanceResult.min_distance << std::endl; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; double distance = 4 - sqrt(2); BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5); - const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5); + const Vec3s p1Ref(sqrt(2) / 2 - 2, 1, .5); + const Vec3s p2Ref(2 - sqrt(2) / 2, 1, .5); BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4); BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4); BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4); @@ -184,7 +184,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { // Apply the same global transform to both objects and recompute Transform3f tf3(coal::makeQuat(0.435952844074, -0.718287018243, 0.310622451066, 0.444435113443), - Vec3f(4, 5, 6)); + Vec3s(4, 5, 6)); tf1 = tf3 * tf1; tf2 = tf3 * tf2; o1 = CollisionObject(s1, tf1); @@ -204,8 +204,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - const Vec3f p1Moved = tf3.transform(p1Ref); - const Vec3f p2Moved = tf3.transform(p2Ref); + const Vec3s p1Moved = tf3.transform(p1Ref); + const Vec3s p2Moved = tf3.transform(p2Ref); BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4); BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4); BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4); @@ -223,28 +223,28 @@ BOOST_AUTO_TEST_CASE(distance_box_box_4) { DistanceResult distanceResult; double distance; - Transform3f tf1(Vec3f(2, 0, 0)); + Transform3f tf1(Vec3s(2, 0, 0)); Transform3f tf2; coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 1.; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4); - tf1.setTranslation(Vec3f(1.01, 0, 0)); + tf1.setTranslation(Vec3s(1.01, 0, 0)); distanceResult.clear(); coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); - tf1.setTranslation(Vec3f(0.99, 0, 0)); + tf1.setTranslation(Vec3s(0.99, 0, 0)); distanceResult.clear(); coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = -0.01; BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3); - tf1.setTranslation(Vec3f(0, 0, 0)); + tf1.setTranslation(Vec3s(0, 0, 0)); distanceResult.clear(); coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); diff --git a/test/broadphase.cpp b/test/broadphase.cpp index 4bb9b642f..8750e55d0 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -159,7 +159,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Box* box = new Box(single_size, single_size, single_size); env.push_back(new CollisionObject( shared_ptr(box), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -174,7 +174,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Sphere* sphere = new Sphere(single_size / 2); env.push_back(new CollisionObject( shared_ptr(sphere), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -189,7 +189,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Cylinder* cylinder = new Cylinder(single_size / 2, single_size); env.push_back(new CollisionObject( shared_ptr(cylinder), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -204,7 +204,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Cone* cone = new Cone(single_size / 2, single_size); env.push_back(new CollisionObject( shared_ptr(cone), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -231,7 +231,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, generateBVHModel(*model, box, Transform3f()); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -248,7 +248,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, generateBVHModel(*model, sphere, Transform3f(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -265,7 +265,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, generateBVHModel(*model, cylinder, Transform3f(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -282,7 +282,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, generateBVHModel(*model, cone, Transform3f(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3f( + Transform3f(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -308,7 +308,7 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, @@ -458,7 +458,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp index 2655390fe..4afeda0c5 100644 --- a/test/broadphase_collision_1.cpp +++ b/test/broadphase_collision_1.cpp @@ -212,7 +212,7 @@ void broad_phase_duplicate_check_test(CoalScalar env_scale, managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, @@ -283,13 +283,13 @@ void broad_phase_duplicate_check_test(CoalScalar env_scale, CoalScalar rand_trans_z = 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; - Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * - Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * - Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); - Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); + Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) * + Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) * + Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ())); + Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z); - Matrix3f R = env[i]->getRotation(); - Vec3f T = env[i]->getTranslation(); + Matrix3s R = env[i]->getRotation(); + Vec3s T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } @@ -383,7 +383,7 @@ void broad_phase_update_collision_test(CoalScalar env_scale, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); CoalScalar cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, @@ -456,13 +456,13 @@ void broad_phase_update_collision_test(CoalScalar env_scale, CoalScalar rand_trans_z = 2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max; - Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) * - Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) * - Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ())); - Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); + Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) * + Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) * + Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ())); + Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z); - Matrix3f R = env[i]->getRotation(); - Vec3f T = env[i]->getTranslation(); + Matrix3s R = env[i]->getRotation(); + Vec3s T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp index 329ebbed3..fe5465829 100644 --- a/test/broadphase_collision_2.cpp +++ b/test/broadphase_collision_2.cpp @@ -208,7 +208,7 @@ void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size, managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); - Vec3f lower_limit, upper_limit; + Vec3s lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); // CoalScalar ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0); CoalScalar ncell_per_axis = 20; diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 4b1f8aed5..b4abb8618 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -56,11 +56,11 @@ using namespace coal; template void testBVHModelPointCloud() { - Box box(Vec3f::Ones()); + Box box(Vec3s::Ones()); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; - std::vector points(8); + std::vector points(8); points[0] << a, -b, c; points[1] << a, b, c; points[2] << -a, b, c; @@ -114,7 +114,7 @@ void testBVHModelPointCloud() { return; } - Matrixx3f all_points((Eigen::DenseIndex)points.size(), 3); + MatrixX3s all_points((Eigen::DenseIndex)points.size(), 3); for (size_t k = 0; k < points.size(); ++k) all_points.row((Eigen::DenseIndex)k) = points[k].transpose(); @@ -139,13 +139,13 @@ void testBVHModelPointCloud() { template void testBVHModelTriangles() { shared_ptr > model(new BVHModel); - Box box(Vec3f::Ones()); - AABB aabb(Vec3f(-1, 0, -1), Vec3f(1, 1, 1)); + Box box(Vec3s::Ones()); + AABB aabb(Vec3s(-1, 0, -1), Vec3s(1, 1, 1)); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; - std::vector points(8); + std::vector points(8); std::vector tri_indices(12); points[0] << a, -b, c; points[1] << a, b, c; @@ -197,14 +197,14 @@ void testBVHModelTriangles() { BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(Vec3f(0, 1, 0)); + pose.setTranslation(Vec3s(0, 1, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(Vec3f(0, 0, 0)); + pose.setTranslation(Vec3s(0, 0, 0)); CoalScalar sqrt2_2 = std::sqrt(2) / 2; pose.setQuatRotation(Quatf(sqrt2_2, sqrt2_2, 0, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); @@ -213,13 +213,13 @@ void testBVHModelTriangles() { BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6); BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2); - pose.setTranslation(-Vec3f(1, 1, 1)); + pose.setTranslation(-Vec3s(1, 1, 1)); pose.setQuatRotation(Quatf::Identity()); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_CHECK(!cropped); - aabb = AABB(Vec3f(-0.1, -0.1, -0.1), Vec3f(0.1, 0.1, 0.1)); - pose.setTranslation(Vec3f(-0.5, -0.5, 0)); + aabb = AABB(Vec3s(-0.1, -0.1, -0.1), Vec3s(0.1, 0.1, 0.1)); + pose.setTranslation(Vec3s(-0.5, -0.5, 0)); cropped.reset(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK_EQUAL(cropped->num_tris, 2); @@ -229,12 +229,12 @@ void testBVHModelTriangles() { template void testBVHModelSubModel() { shared_ptr > model(new BVHModel); - Box box(Vec3f::Ones()); + Box box(Vec3s::Ones()); double a = box.halfSide[0]; double b = box.halfSide[1]; double c = box.halfSide[2]; - std::vector points(8); + std::vector points(8); std::vector tri_indices(12); points[0] << a, -b, c; points[1] << a, b, c; @@ -304,7 +304,7 @@ void testLoadPolyhedron() { typedef shared_ptr PolyhedronPtr_t; PolyhedronPtr_t P1(new Polyhedron_t), P2; - Vec3f scale; + Vec3s scale; scale.setConstant(1); loadPolyhedronFromResource(env, scale, P1); @@ -331,11 +331,11 @@ void testLoadGerardBauzil() { typedef shared_ptr PolyhedronPtr_t; PolyhedronPtr_t P1(new Polyhedron_t), P2; - Vec3f scale; + Vec3s scale; scale.setConstant(1); loadPolyhedronFromResource(env, scale, P1); CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27)); - Transform3f pos(Vec3f(-1.33, 1.36, .14)); + Transform3f pos(Vec3s(-1.33, 1.36, .14)); CollisionObject obj(cylinder, pos); CollisionObject stairs(P1); diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp index 568bfe3ee..386aecf46 100644 --- a/test/capsule_box_1.cpp +++ b/test/capsule_box_1.cpp @@ -59,7 +59,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { coal::DistanceRequest distanceRequest(true, 0, 0); coal::DistanceResult distanceResult; - coal::Transform3f tf1(coal::Vec3f(3., 0, 0)); + coal::Transform3f tf1(coal::Vec3s(3., 0, 0)); coal::Transform3f tf2; coal::CollisionObject capsule(capsuleGeometry, tf1); coal::CollisionObject box(boxGeometry, tf2); @@ -67,9 +67,9 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { // test distance coal::distance(&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule - coal::Vec3f o1(distanceResult.nearest_points[0]); + coal::Vec3s o1(distanceResult.nearest_points[0]); // Nearest point on box - coal::Vec3f o2(distanceResult.nearest_points[1]); + coal::Vec3s o2(distanceResult.nearest_points[1]); BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1); BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1); CHECK_CLOSE_TO_0(o1[1], 1e-1); @@ -77,7 +77,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { CHECK_CLOSE_TO_0(o2[1], 1e-1); // Move capsule above box - tf1 = coal::Transform3f(coal::Vec3f(0., 0., 8.)); + tf1 = coal::Transform3f(coal::Vec3s(0., 0., 8.)); capsule.setTransform(tf1); // test distance @@ -96,7 +96,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1); // Rotate capsule around y axis by pi/2 and move it behind box - tf1.setTranslation(coal::Vec3f(-10., 0., 0.)); + tf1.setTranslation(coal::Vec3s(-10., 0., 0.)); tf1.setQuatRotation(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0)); capsule.setTransform(tf1); diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp index b16d523cf..8e3bb3ba7 100644 --- a/test/capsule_box_2.cpp +++ b/test/capsule_box_2.cpp @@ -61,7 +61,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { // Rotate capsule around y axis by pi/2 and move it behind box coal::Transform3f tf1(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), - coal::Vec3f(-10., 0.8, 1.5)); + coal::Vec3s(-10., 0.8, 1.5)); coal::Transform3f tf2; coal::CollisionObject capsule(capsuleGeometry, tf1); coal::CollisionObject box(boxGeometry, tf2); @@ -69,8 +69,8 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { // test distance distanceResult.clear(); coal::distance(&capsule, &box, distanceRequest, distanceResult); - coal::Vec3f o1 = distanceResult.nearest_points[0]; - coal::Vec3f o2 = distanceResult.nearest_points[1]; + coal::Vec3s o1 = distanceResult.nearest_points[0]; + coal::Vec3s o2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-2); BOOST_CHECK_CLOSE(o1[0], -6, 1e-2); diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index a20f0fe8a..4534af243 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -241,7 +241,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) { CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2(Vec3f(20.1, 0, 0)); + Transform3f tf2(Vec3s(20.1, 0, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -267,7 +267,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2(Vec3f(20, 20, 0)); + Transform3f tf2(Vec3s(20, 20, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) { CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2(Vec3f(0, 0, 20.1)); + Transform3f tf2(Vec3s(0, 0, 20.1)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -320,7 +320,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { CollisionGeometryPtr_t s2(new Capsule(5, 10)); Transform3f tf1; - Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3f(0, 0, 25.1)); + Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3s(0, 0, 25.1)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -343,8 +343,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { << std::endl << "distance = " << distanceResult.min_distance << std::endl; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; + const Vec3s& p1 = distanceResult.nearest_points[0]; + const Vec3s& p2 = distanceResult.nearest_points[1]; BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6); CHECK_CLOSE_TO_0(p1[0], 1e-4); diff --git a/test/collision.cpp b/test/collision.cpp index 76bd88244..d02959948 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -78,8 +78,8 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; - aabb1.min_ = Vec3f(-600, -600, -600); - aabb1.max_ = Vec3f(600, 600, 600); + aabb1.min_ = Vec3s(-600, -600, -600); + aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); @@ -128,8 +128,8 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) { generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; - aabb1.min_ = Vec3f(-600, -600, -600); - aabb1.max_ = Vec3f(600, 600, 600); + aabb1.min_ = Vec3s(-600, -600, -600); + aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); @@ -213,8 +213,8 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) { generateRandomTransforms(extents, transforms, n); AABB aabb1; - aabb1.min_ = Vec3f(-600, -600, -600); - aabb1.max_ = Vec3f(600, 600, 600); + aabb1.min_ = Vec3s(-600, -600, -600); + aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; convertBV(aabb1, Transform3f(), obb1); @@ -369,9 +369,9 @@ struct mesh_mesh_run_test { model1->bv_splitter.reset(new BVSplitter(splitMethod)); model2->bv_splitter.reset(new BVSplitter(splitMethod)); - loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(), + loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3s::Ones(), model1); - loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(), + loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3s::Ones(), model2); Timer timer(false); diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp index d1dbc9e3f..3214a96c1 100644 --- a/test/collision_node_asserts.cpp +++ b/test/collision_node_asserts.cpp @@ -13,12 +13,12 @@ double DegToRad(const double& deg) { static double degToRad = pi / 180.; return deg * degToRad; } -std::vector dirs{Vec3f::UnitZ(), -Vec3f::UnitZ(), Vec3f::UnitY(), - -Vec3f::UnitY(), Vec3f::UnitX(), -Vec3f::UnitX()}; +std::vector dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(), + -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()}; BOOST_AUTO_TEST_CASE(TestTriangles) { - std::vector triVertices{Vec3f(1, 0, 0), Vec3f(1, 1, 0), - Vec3f(0, 1, 0)}; + std::vector triVertices{Vec3s(1, 0, 0), Vec3s(1, 1, 0), + Vec3s(0, 1, 0)}; std::vector triangle{{0, 1, 2}}; BVHModel tri1{}; @@ -44,11 +44,11 @@ BOOST_AUTO_TEST_CASE(TestTriangles) { for (int j = 0; j < 180; j += 30) { for (int k = 0; k < 180; k += 30) { tri1Tf.setQuatRotation( - Eigen::AngleAxis(0., Vec3f::UnitZ()) * - Eigen::AngleAxis(DegToRad(k), Vec3f::UnitY())); + Eigen::AngleAxis(0., Vec3s::UnitZ()) * + Eigen::AngleAxis(DegToRad(k), Vec3s::UnitY())); tri2Tf.setQuatRotation( - Eigen::AngleAxis(DegToRad(i), Vec3f::UnitZ()) * - Eigen::AngleAxis(DegToRad(j), Vec3f::UnitY())); + Eigen::AngleAxis(DegToRad(i), Vec3s::UnitZ()) * + Eigen::AngleAxis(DegToRad(j), Vec3s::UnitY())); CollisionResult result; /// assertion: src/collision_node.cpp:58 diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp index 0a02bbe97..6db7f1069 100644 --- a/test/contact_patch.cpp +++ b/test/contact_patch.cpp @@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE(box_box_no_collision) { Transform3f tf2; // set translation to separate the shapes const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside + offset)); + tf2.setTranslation(Vec3s(0, 0, 2 * halfside + offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -79,7 +79,7 @@ BOOST_AUTO_TEST_CASE(box_sphere) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -117,7 +117,7 @@ BOOST_AUTO_TEST_CASE(box_box) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -142,7 +142,7 @@ BOOST_AUTO_TEST_CASE(box_box) { patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const CoalScalar tol = 1e-6; - EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); + EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); @@ -150,11 +150,11 @@ BOOST_AUTO_TEST_CASE(box_box) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const std::array corners = { - Vec3f(halfside, halfside, halfside), - Vec3f(halfside, -halfside, halfside), - Vec3f(-halfside, -halfside, halfside), - Vec3f(-halfside, halfside, halfside), + const std::array corners = { + Vec3s(halfside, halfside, halfside), + Vec3s(halfside, -halfside, halfside), + Vec3s(-halfside, -halfside, halfside), + Vec3s(-halfside, halfside, halfside), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] + @@ -175,7 +175,7 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, halfside - offset)); + tf2.setTranslation(Vec3s(0, 0, halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -201,7 +201,7 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { const Contact& contact = col_res.getContact(0); const CoalScalar tol = 1e-6; EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol); - EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3f(0, 0, 1), tol); + EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); @@ -209,11 +209,11 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const std::array corners = { - tf2.transform(Vec3f(halfside, halfside, -halfside)), - tf2.transform(Vec3f(halfside, -halfside, -halfside)), - tf2.transform(Vec3f(-halfside, -halfside, -halfside)), - tf2.transform(Vec3f(-halfside, halfside, -halfside)), + const std::array corners = { + tf2.transform(Vec3s(halfside, halfside, -halfside)), + tf2.transform(Vec3s(halfside, -halfside, -halfside)), + tf2.transform(Vec3s(-halfside, -halfside, -halfside)), + tf2.transform(Vec3s(-halfside, halfside, -halfside)), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] - @@ -235,7 +235,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -263,7 +263,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f capsule_end(0, 0, -capsule.halfLength); + const Vec3s capsule_end(0, 0, -capsule.halfLength); expected.addPoint(tf2.transform(capsule_end)); const ContactPatch& contact_patch = patch_res.getContactPatch(0); @@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f capsule_end(0, 0, capsule.halfLength); + const Vec3s capsule_end(0, 0, capsule.halfLength); expected.addPoint(tf2.transform(capsule_end)); const ContactPatch& contact_patch = patch_res.getContactPatch(0); @@ -326,8 +326,8 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f p1(-capsule.radius, 0, capsule.halfLength); - const Vec3f p2(-capsule.radius, 0, -capsule.halfLength); + const Vec3s p1(-capsule.radius, 0, capsule.halfLength); + const Vec3s p2(-capsule.radius, 0, -capsule.halfLength); expected.addPoint(tf2.transform(p1)); expected.addPoint(tf2.transform(p2)); @@ -347,7 +347,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -375,12 +375,12 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - std::array points; + std::array points; const CoalScalar angle_increment = 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { const CoalScalar theta = (CoalScalar)(i)*angle_increment; - Vec3f point_on_cone_base(std::cos(theta) * cone.radius, + Vec3s point_on_cone_base(std::cos(theta) * cone.radius, std::sin(theta) * cone.radius, -cone.halfLength); expected.addPoint(tf2.transform(point_on_cone_base)); } @@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f cone_tip(0, 0, cone.halfLength); + const Vec3s cone_tip(0, 0, cone.halfLength); expected.addPoint(tf2.transform(cone_tip)); BOOST_CHECK(contact_patch.isSame(expected, tol)); @@ -455,7 +455,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const Vec3f point_on_circle_basis(-cone.radius, 0, -cone.halfLength); + const Vec3s point_on_circle_basis(-cone.radius, 0, -cone.halfLength); expected.addPoint(tf2.transform(point_on_circle_basis)); BOOST_CHECK(contact_patch.isSame(expected, tol)); @@ -472,7 +472,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -490,12 +490,12 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - std::array points; + std::array points; const CoalScalar angle_increment = 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6)); for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) { const CoalScalar theta = (CoalScalar)(i)*angle_increment; - Vec3f point_on_cone_base(std::cos(theta) * cylinder.radius, + Vec3s point_on_cone_base(std::cos(theta) * cylinder.radius, std::sin(theta) * cylinder.radius, -cylinder.halfLength); expected.addPoint(tf2.transform(point_on_cone_base)); @@ -563,9 +563,9 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; expected.addPoint( - tf2.transform(Vec3f(cylinder.radius, 0, cylinder.halfLength))); + tf2.transform(Vec3s(cylinder.radius, 0, cylinder.halfLength))); expected.addPoint( - tf2.transform(Vec3f(cylinder.radius, 0, -cylinder.halfLength))); + tf2.transform(Vec3s(cylinder.radius, 0, -cylinder.halfLength))); const ContactPatch& contact_patch = patch_res.getContactPatch(0); BOOST_CHECK(expected.isSame(contact_patch, tol)); @@ -581,7 +581,7 @@ BOOST_AUTO_TEST_CASE(convex_convex) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset)); + tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -606,7 +606,7 @@ BOOST_AUTO_TEST_CASE(convex_convex) { patch_res2.numContactPatches() > 0 && col_res.isCollision()) { const Contact& contact = col_res.getContact(0); const CoalScalar tol = 1e-6; - EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol); + EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol); const size_t expected_size = 4; ContactPatch expected(expected_size); @@ -614,11 +614,11 @@ BOOST_AUTO_TEST_CASE(convex_convex) { constructOrthonormalBasisFromVector(contact.normal); expected.tf.translation() = contact.pos; expected.penetration_depth = contact.penetration_depth; - const std::array corners = { - Vec3f(halfside, halfside, halfside), - Vec3f(halfside, -halfside, halfside), - Vec3f(-halfside, -halfside, halfside), - Vec3f(-halfside, halfside, halfside), + const std::array corners = { + Vec3s(halfside, halfside, halfside), + Vec3s(halfside, -halfside, halfside), + Vec3s(-halfside, -halfside, halfside), + Vec3s(-halfside, halfside, halfside), }; for (size_t i = 0; i < expected_size; ++i) { expected.addPoint(corners[i] + @@ -635,8 +635,8 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { // Two tetrahedrons make contact on one of their edge. const size_t expected_size = 2; - const Vec3f expected_cp1(0, 0.5, 0); - const Vec3f expected_cp2(0, 1, 0); + const Vec3s expected_cp1(0, 0.5, 0); + const Vec3s expected_cp2(0, 1, 0); const Transform3f tf1; // identity const Transform3f tf2; // identity @@ -650,22 +650,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { { // Case 1 - Face-Face contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(0, 0.5, 0), - Vec3f(0, 1.5, 0), - Vec3f(1, 0.5, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(0, 0.5, 0), + Vec3s(0, 1.5, 0), + Vec3s(1, 0.5, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -701,22 +701,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { { // Case 2 - Face-Segment contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.2), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.2), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(0, 0.5, 0), - Vec3f(0, 1.5, 0), - Vec3f(1, 0.5, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(0, 0.5, 0), + Vec3s(0, 1.5, 0), + Vec3s(1, 0.5, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -750,22 +750,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { { // Case 3 - Segment-Segment contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.2), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.2), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(0, 0.5, 0), - Vec3f(0, 1.5, 0), - Vec3f(1, 0.5, 0.5), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(0, 0.5, 0), + Vec3s(0, 1.5, 0), + Vec3s(1, 0.5, 0.5), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -802,7 +802,7 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { // This case covers the vertex-vertex edge case of contact patches. // Two tetrahedrons make contact on one of their vertex. const size_t expected_size = 1; - const Vec3f expected_cp(0, 0, 0); + const Vec3s expected_cp(0, 0, 0); const Transform3f tf1; // identity const Transform3f tf2; // identity @@ -816,22 +816,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { { // Case 1 - Face-Face contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, -1, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, -1, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -864,22 +864,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { { // Case 2 - Segment-Face contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.5), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.5), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, -1, 0), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, -1, 0), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -912,22 +912,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { { // Case 2 - Segment-Segment contact - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0.2), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0.2), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(1, 0, 0), - Vec3f(0, 0, 0), - Vec3f(0, -1, 0.5), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(1, 0, 0), + Vec3s(0, 0, 0), + Vec3s(0, -1, 0.5), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), @@ -963,8 +963,8 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { // This case covers the segment-face edge case of contact patches. // Two tetrahedrons make contact on one of their segment/face respectively. const size_t expected_size = 2; - const Vec3f expected_cp1(0, 0, 0); - const Vec3f expected_cp2(-0.5, 0.5, 0); + const Vec3s expected_cp1(0, 0, 0); + const Vec3s expected_cp2(-0.5, 0.5, 0); const Transform3f tf1; // identity const Transform3f tf2; // identity @@ -977,22 +977,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { ContactPatchResult patch_res(patch_req); { - std::shared_ptr> pts1(new std::vector({ - Vec3f(-1, 0, -0), - Vec3f(0, 0, 0), - Vec3f(0, 1, 0), - Vec3f(-1, -1, -1), + std::shared_ptr> pts1(new std::vector({ + Vec3s(-1, 0, -0), + Vec3s(0, 0, 0), + Vec3s(0, 1, 0), + Vec3s(-1, -1, -1), })); std::shared_ptr> tris1( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), Triangle(0, 3, 1), Triangle(2, 1, 3)})); Convex tetra1(pts1, 4, tris1, 4); - std::shared_ptr> pts2(new std::vector({ - Vec3f(-0.5, 0.5, 0), - Vec3f(0.5, -0.5, 0), - Vec3f(1, 0.5, 0.5), - Vec3f(1, 1, 1), + std::shared_ptr> pts2(new std::vector({ + Vec3s(-0.5, 0.5, 0), + Vec3s(0.5, -0.5, 0), + Vec3s(1, 0.5, 0.5), + Vec3s(1, 1, 1), })); std::shared_ptr> tris2( new std::vector({Triangle(0, 1, 2), Triangle(0, 2, 3), diff --git a/test/convex.cpp b/test/convex.cpp index bc48ca8d0..02507b263 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -157,11 +157,11 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) { Transform3f tf1; Transform3f tf2; - tf2.setTranslation(Vec3f(3, 0, 0)); + tf2.setTranslation(Vec3s(3, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); compareShapeDistance(box, convex_box, tf1, tf2, eps); - tf2.setTranslation(Vec3f(0, 0, 0)); + tf2.setTranslation(Vec3s(0, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); compareShapeDistance(box, convex_box, tf1, tf2, eps); @@ -174,8 +174,8 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) { #ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(convex_hull_throw) { - std::shared_ptr> points( - new std::vector({Vec3f(1, 1, 1), Vec3f(0, 0, 0), Vec3f(1, 0, 0)})); + std::shared_ptr> points( + new std::vector({Vec3s(1, 1, 1), Vec3s(0, 0, 0), Vec3s(1, 0, 0)})); BOOST_CHECK_THROW(ConvexBase::convexHull(points, 0, false, NULL), std::invalid_argument); @@ -188,11 +188,11 @@ BOOST_AUTO_TEST_CASE(convex_hull_throw) { } BOOST_AUTO_TEST_CASE(convex_hull_quad) { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(0, 0, 0), - Vec3f(1, 0, 0), - Vec3f(0, 0, 1), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(0, 0, 0), + Vec3s(1, 0, 0), + Vec3s(0, 0, 1), })); ConvexBase* convexHull = ConvexBase::convexHull(points, 4, false, NULL); @@ -205,26 +205,26 @@ BOOST_AUTO_TEST_CASE(convex_hull_quad) { } BOOST_AUTO_TEST_CASE(convex_hull_box_like) { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(1, 1, -1), - Vec3f(1, -1, 1), - Vec3f(1, -1, -1), - Vec3f(-1, 1, 1), - Vec3f(-1, 1, -1), - Vec3f(-1, -1, 1), - Vec3f(-1, -1, -1), - Vec3f(0, 0, 0), - Vec3f(0, 0, 0.99), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(1, 1, -1), + Vec3s(1, -1, 1), + Vec3s(1, -1, -1), + Vec3s(-1, 1, 1), + Vec3s(-1, 1, -1), + Vec3s(-1, -1, 1), + Vec3s(-1, -1, -1), + Vec3s(0, 0, 0), + Vec3s(0, 0, 0.99), })); ConvexBase* convexHull = ConvexBase::convexHull(points, 9, false, NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); { - const std::vector& cvxhull_points = *(convexHull->points); + const std::vector& cvxhull_points = *(convexHull->points); for (size_t i = 0; i < 8; ++i) { - BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1)); + BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1)); BOOST_CHECK_EQUAL((*(convexHull->neighbors))[i].count(), 3); } } @@ -236,9 +236,9 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { BOOST_REQUIRE_EQUAL(8, convexHull->num_points); { - const std::vector& cvxhull_points = *(convexHull->points); + const std::vector& cvxhull_points = *(convexHull->points); for (size_t i = 0; i < 8; ++i) { - BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1)); + BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1)); BOOST_CHECK((*(convexHull->neighbors))[i].count() >= 3); BOOST_CHECK((*(convexHull->neighbors))[i].count() <= 6); } @@ -249,16 +249,16 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { BOOST_AUTO_TEST_CASE(convex_copy_constructor) { Convex* convexHullTriCopy; { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(1, 1, -1), - Vec3f(1, -1, 1), - Vec3f(1, -1, -1), - Vec3f(-1, 1, 1), - Vec3f(-1, 1, -1), - Vec3f(-1, -1, 1), - Vec3f(-1, -1, -1), - Vec3f(0, 0, 0), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(1, 1, -1), + Vec3s(1, -1, 1), + Vec3s(1, -1, -1), + Vec3s(-1, 1, 1), + Vec3s(-1, 1, -1), + Vec3s(-1, -1, 1), + Vec3s(-1, -1, -1), + Vec3s(0, 0, 0), })); Convex* convexHullTri = dynamic_cast*>( @@ -272,16 +272,16 @@ BOOST_AUTO_TEST_CASE(convex_copy_constructor) { } BOOST_AUTO_TEST_CASE(convex_clone) { - std::shared_ptr> points(new std::vector({ - Vec3f(1, 1, 1), - Vec3f(1, 1, -1), - Vec3f(1, -1, 1), - Vec3f(1, -1, -1), - Vec3f(-1, 1, 1), - Vec3f(-1, 1, -1), - Vec3f(-1, -1, 1), - Vec3f(-1, -1, -1), - Vec3f(0, 0, 0), + std::shared_ptr> points(new std::vector({ + Vec3s(1, 1, 1), + Vec3s(1, 1, -1), + Vec3s(1, -1, 1), + Vec3s(1, -1, -1), + Vec3s(-1, 1, 1), + Vec3s(-1, 1, -1), + Vec3s(-1, -1, 1), + Vec3s(-1, -1, -1), + Vec3s(0, 0, 0), })); Convex* convexHullTri = dynamic_cast*>( diff --git a/test/distance.cpp b/test/distance.cpp index 603d57bad..77a35f60c 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -56,30 +56,30 @@ bool verbose = false; CoalScalar DELTA = 0.001; template -void distance_Test(const Transform3f& tf, const std::vector& vertices1, +void distance_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose = true); bool collide_Test_OBB(const Transform3f& tf, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template void distance_Test_Oriented(const Transform3f& tf, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose = true); -inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) { +inline bool nearlyEqual(const Vec3s& a, const Vec3s& b) { if (fabs(a[0] - b[0]) > DELTA) return false; if (fabs(a[1] - b[1]) > DELTA) return false; if (fabs(a[2] - b[2]) > DELTA) return false; @@ -87,7 +87,7 @@ inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) { } BOOST_AUTO_TEST_CASE(mesh_distance) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); @@ -469,9 +469,9 @@ BOOST_AUTO_TEST_CASE(mesh_distance) { template void distance_Test_Oriented(const Transform3f& tf, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose) { @@ -499,8 +499,8 @@ void distance_Test_Oriented(const Transform3f& tf, distance(&node, NULL, qsize); // points are in local coordinate, to global coordinate - Vec3f p1 = local_result.nearest_points[0]; - Vec3f p2 = local_result.nearest_points[1]; + Vec3s p1 = local_result.nearest_points[0]; + Vec3s p2 = local_result.nearest_points[1]; distance_result.distance = local_result.min_distance; distance_result.p1 = p1; @@ -516,9 +516,9 @@ void distance_Test_Oriented(const Transform3f& tf, } template -void distance_Test(const Transform3f& tf, const std::vector& vertices1, +void distance_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose) { @@ -566,9 +566,9 @@ void distance_Test(const Transform3f& tf, const std::vector& vertices1, } bool collide_Test_OBB(const Transform3f& tf, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index 7c034f69a..21c2f3eea 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -58,7 +58,7 @@ using coal::OBBRSS; using coal::shared_ptr; using coal::Transform3f; using coal::Triangle; -using coal::Vec3f; +using coal::Vec3s; bool testDistanceLowerBound(const Transform3f& tf, const CollisionGeometryPtr_t& m1, @@ -113,7 +113,7 @@ bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1, } BOOST_AUTO_TEST_CASE(mesh_mesh) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -232,7 +232,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { } BOOST_AUTO_TEST_CASE(box_mesh) { - std::vector p1; + std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); diff --git a/test/frontlist.cpp b/test/frontlist.cpp index 2ed4dd76c..df1e6e6dc 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -52,9 +52,9 @@ namespace utf = boost::unit_test::framework; template bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose); @@ -62,23 +62,23 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, template bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template -bool collide_Test(const Transform3f& tf, const std::vector& vertices1, +bool collide_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); // TODO: randomly still have some runtime error BOOST_AUTO_TEST_CASE(front_list) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); @@ -271,9 +271,9 @@ BOOST_AUTO_TEST_CASE(front_list) { template bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose) { @@ -284,7 +284,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, BVHFrontList front_list; - std::vector vertices1_new(vertices1.size()); + std::vector vertices1_new(vertices1.size()); for (std::size_t i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf1.transform(vertices1[i]); } @@ -338,9 +338,9 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, template bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2, - const std::vector& vertices1, + const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { @@ -392,9 +392,9 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, } template -bool collide_Test(const Transform3f& tf, const std::vector& vertices1, +bool collide_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, - const std::vector& vertices2, + const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index c391e6a29..ad89819cc 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -80,8 +80,8 @@ template void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, - const Vec3f& contact_or_normal, - const Vec3f& expected_contact_or_normal, + const Vec3s& contact_or_normal, + const Vec3s& expected_contact_or_normal, bool check_opposite_normal, CoalScalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) @@ -127,10 +127,10 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, template void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, const Vec3f& contact, - Vec3f* expected_point, CoalScalar depth, - CoalScalar* expected_depth, const Vec3f& normal, - Vec3f* expected_normal, bool check_opposite_normal, + const Transform3f& tf2, const Vec3s& contact, + Vec3s* expected_point, CoalScalar depth, + CoalScalar* expected_depth, const Vec3s& normal, + Vec3s* expected_normal, bool check_opposite_normal, CoalScalar tol) { if (expected_point) { bool contact_equal = isEqual(contact, *expected_point, tol); @@ -164,16 +164,16 @@ void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, template void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, bool expect_collision, - Vec3f* expected_point = NULL, + Vec3s* expected_point = NULL, CoalScalar* expected_depth = NULL, - Vec3f* expected_normal = NULL, + Vec3s* expected_normal = NULL, bool check_opposite_normal = false, CoalScalar tol = 1e-9) { CollisionRequest request; CollisionResult result; - Vec3f contact; - Vec3f normal; // normal direction should be from object 1 to object 2 + Vec3s contact; + Vec3s normal; // normal direction should be from object 1 to object 2 bool collision; bool check_failed = false; @@ -244,46 +244,46 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) { Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; + // Vec3s point; // CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(40, 0, 0)); + tf2 = Transform3f(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(40, 0, 0)); + tf2 = transform * Transform3f(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(30, 0, 0)); + tf2 = Transform3f(Vec3s(30, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(30.01, 0, 0)); + tf2 = Transform3f(Vec3s(30.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3f(Vec3s(30.01, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(29.9, 0, 0)); + tf2 = Transform3f(Vec3s(29.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3f(Vec3s(29.9, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); @@ -302,45 +302,45 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-29.9, 0, 0)); + tf2 = Transform3f(Vec3s(-29.9, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); - normal = transform.getRotation() * Vec3f(-1, 0, 0); + tf2 = transform * Transform3f(Vec3s(-29.9, 0, 0)); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-30.0, 0, 0)); + tf2 = Transform3f(Vec3s(-30.0, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-30.01, 0, 0)); + tf2 = Transform3f(Vec3s(-30.01, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-30.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } -bool compareContactPoints(const Vec3f& c1, const Vec3f& c2) { +bool compareContactPoints(const Vec3s& c1, const Vec3s& c2) { return c1[2] < c2[2]; } // Ascending order -void testBoxBoxContactPoints(const Matrix3f& R) { +void testBoxBoxContactPoints(const Matrix3s& R) { Box s1(100, 100, 100); Box s2(10, 20, 30); // Vertices of s2 - std::vector vertices(8); + std::vector vertices(8); vertices[0] << 1, 1, 1; vertices[1] << 1, 1, -1; vertices[2] << 1, -1, 1; @@ -354,11 +354,11 @@ void testBoxBoxContactPoints(const Matrix3f& R) { vertices[i].array() *= s2.halfSide.array(); } - Transform3f tf1 = Transform3f(Vec3f(0, 0, -50)); + Transform3f tf1 = Transform3f(Vec3s(0, 0, -50)); Transform3f tf2 = Transform3f(R); - Vec3f normal; - Vec3f p1, p2; + Vec3s normal; + Vec3s p1, p2; // Make sure the two boxes are colliding solver1.gjk_tolerance = 1e-5; @@ -375,8 +375,8 @@ void testBoxBoxContactPoints(const Matrix3f& R) { std::sort(vertices.begin(), vertices.end(), compareContactPoints); // The lowest vertex along z-axis should be the contact point - FCL_CHECK(normal.isApprox(Vec3f(0, 0, 1), 1e-6)); - Vec3f point = 0.5 * (p1 + p2); + FCL_CHECK(normal.isApprox(Vec3s(0, 0, 1), 1e-6)); + Vec3s point = 0.5 * (p1 + p2); FCL_CHECK(vertices[0].head<2>().isApprox(point.head<2>(), 1e-6)); FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0); } @@ -391,9 +391,9 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; + // Vec3s point; // CoalScalar depth; - Vec3f normal; + Vec3s normal; Quatf q; q = AngleAxis((CoalScalar)3.140 / 6, UnitZ); @@ -410,23 +410,23 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (1, 0, 0). - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(15, 0, 0)); + tf2 = Transform3f(Vec3s(15, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-8); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(15.01, 0, 0)); + tf2 = Transform3f(Vec3s(15.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(15.01, 0, 0)); + tf2 = transform * Transform3f(Vec3s(15.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); @@ -438,7 +438,7 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { tf1 = transform; tf2 = transform * Transform3f(q); - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); @@ -461,9 +461,9 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; + // Vec3s point; // CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -481,36 +481,36 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(22.50001, 0, 0)); + tf2 = Transform3f(Vec3s(22.50001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(22.501, 0, 0)); + tf2 = transform * Transform3f(Vec3s(22.501, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(22.4, 0, 0)); + tf2 = Transform3f(Vec3s(22.4, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3f(Vec3s(22.4, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); } BOOST_AUTO_TEST_CASE(distance_spherebox) { - coal::Matrix3f rotSphere; + coal::Matrix3s rotSphere; rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - coal::Vec3f trSphere(0.0, 0.0, 0.0); + coal::Vec3s trSphere(0.0, 0.0, 0.0); - coal::Matrix3f rotBox; + coal::Matrix3s rotBox; rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - coal::Vec3f trBox(0.0, 5.0, 3.0); + coal::Vec3s trBox(0.0, 5.0, 3.0); coal::Sphere sphere(1); coal::Box box(10, 2, 10); @@ -521,9 +521,9 @@ BOOST_AUTO_TEST_CASE(distance_spherebox) { CoalScalar eps = Eigen::NumTraits::epsilon(); BOOST_CHECK_CLOSE(result.min_distance, 3., eps); - EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3f(0, 1, 0), eps); - EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3f(0, 4, 0), eps); - EIGEN_VECTOR_IS_APPROX(result.normal, Vec3f(0, 1, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3s(0, 1, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3s(0, 4, 0), eps); + EIGEN_VECTOR_IS_APPROX(result.normal, Vec3s(0, 1, 0), eps); } BOOST_AUTO_TEST_CASE(collide_spherecapsule) { @@ -536,9 +536,9 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) { Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; + // Vec3s point; // CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -555,38 +555,38 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(24.9, 0, 0)); + tf2 = Transform3f(Vec3s(24.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3f(Vec3s(24.9, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(25, 0, 0)); + tf2 = Transform3f(Vec3s(25, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(24.999999, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3f(Vec3s(24.999999, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(25.1, 0, 0)); + tf2 = Transform3f(Vec3s(25.1, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3f(Vec3s(25.1, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -601,9 +601,9 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; + // Vec3s point; // CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -620,36 +620,36 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0)); + tf2 = Transform3f(Vec3s(9.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 9.9, 0)); + tf2 = Transform3f(Vec3s(0, 9.9, 0)); normal << 0, 1, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0)); + tf2 = Transform3f(Vec3s(9.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); - normal = transform.getRotation() * Vec3f(1, 0, 0); + tf2 = transform * Transform3f(Vec3s(9.9, 0, 0)); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.01, 0, 0)); + tf2 = Transform3f(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); + tf2 = transform * Transform3f(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -664,9 +664,9 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; + // Vec3s point; // CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -684,8 +684,8 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { tf1 = Transform3f(); // z=0 is a singular points. Two normals could be returned. - tf2 = Transform3f(Vec3f(9.9, 0, 0.00001)); - normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) + tf2 = Transform3f(Vec3s(9.9, 0, 0.00001)); + normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); @@ -697,37 +697,37 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.00001)); - normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) + tf2 = transform * Transform3f(Vec3s(9.9, 0, 0.00001)); + normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, true, tol_gjk); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.1, 0, 0)); + tf2 = Transform3f(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.001, 0, 0)); + tf2 = Transform3f(Vec3s(10.001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.001, 0, 0)); + tf2 = transform * Transform3f(Vec3s(10.001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 9.9)); + tf2 = Transform3f(Vec3s(0, 0, 9.9)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); + tf2 = transform * Transform3f(Vec3s(0, 0, 9.9)); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); } @@ -742,9 +742,9 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { Transform3f transform; generateRandomTransform(extents, transform); - // Vec3f point; + // Vec3s point; // CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -761,81 +761,81 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0)); + tf2 = Transform3f(Vec3s(9.9, 0, 0)); normal = - Vec3f(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius)) + Vec3s(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius)) .normalized(); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); + tf2 = transform * Transform3f(Vec3s(9.9, 0, 0)); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(9.9, 0, 0.1)); + tf2 = Transform3f(Vec3s(9.9, 0, 0.1)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.1)); + tf2 = transform * Transform3f(Vec3s(9.9, 0, 0.1)); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.01, 0, 0)); + tf2 = Transform3f(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); + tf2 = transform * Transform3f(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10, 0, 0)); + tf2 = Transform3f(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10, 0, 0)); + tf2 = transform * Transform3f(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 9.9)); + tf2 = Transform3f(Vec3s(0, 0, 9.9)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); - normal = transform.getRotation() * Vec3f(0, 0, 1); + tf2 = transform * Transform3f(Vec3s(0, 0, 9.9)); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.01)); + tf2 = Transform3f(Vec3s(0, 0, 10.01)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.01)); + tf2 = transform * Transform3f(Vec3s(0, 0, 10.01)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10)); + tf2 = Transform3f(Vec3s(0, 0, 10)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -846,20 +846,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f normal; + Vec3s normal; // // Testing collision x, y, z // { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); // identity - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -868,7 +868,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal << 0, 0, -1; SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -879,14 +879,14 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0, 0; t[1] << 9.9, -20, 0; t[2] << 9.9, 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); normal << 9.9, 0, 0.001; normal.normalize(); SET_LINE; @@ -896,7 +896,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal << 9.9, 0, -0.001; normal.normalize(); SET_LINE; @@ -908,14 +908,14 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0, 0.001, 0)); + tf_tri.setTranslation(Vec3s(0, 0.001, 0)); normal << 0, 1, 0; SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -924,7 +924,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, -0.001, 0)); + tf_tri.setTranslation(Vec3s(0, -0.001, 0)); normal << 0, -1, 0; SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -935,14 +935,14 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0, 30, 0; t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0.001, 0, 0)); + tf_tri.setTranslation(Vec3s(0.001, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -951,7 +951,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(-0.001, 0, 0)); + tf_tri.setTranslation(Vec3s(-0.001, 0, 0)); normal << -1, 0, 0; testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); SET_LINE; @@ -965,20 +965,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { // Testing no collision x, y, z // { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0, 0, 10.1)); + tf_tri.setTranslation(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, 0, -10.1)); + tf_tri.setTranslation(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -986,20 +986,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0, 10.1, 0)); + tf_tri.setTranslation(Vec3s(0, 10.1, 0)); SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, -10.1, 0)); + tf_tri.setTranslation(Vec3s(0, -10.1, 0)); SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1007,20 +1007,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0, 20, 0; t[1] << 0, -20, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(10.1, 0, 0)); + tf_tri.setTranslation(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-10.1, 0, 0)); + tf_tri.setTranslation(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1029,24 +1029,24 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { } BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { - Halfspace hs(Vec3f(0, 0, 1), 0); + Halfspace hs(Vec3s(0, 0, 1), 0); Transform3f transform; generateRandomTransform(extents, transform); - Vec3f normal; + Vec3s normal; normal = hs.n; // with halfspaces, it's simple: normal is always // the normal of the halfspace. { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); // identity - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -1055,19 +1055,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(1, 1, 0.001)); + tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-1, -1, 0.001)); + tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1075,14 +1075,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0, 0; t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -1091,19 +1091,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(1, 1, 0.001)); + tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-1, -1, 0.001)); + tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1111,14 +1111,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0, 30, 0; t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); - tf_tri.setTranslation(Vec3f(0, 0, -0.001)); + tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -1127,19 +1127,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.001)); + tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(1, 1, 0.001)); + tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(-1, -1, 0.001)); + tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; testShapeCollide(hs, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1151,14 +1151,14 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f normal; + Vec3s normal; { - Vec3f t[3]; + Vec3s t[3]; t[0] << 20, 0, 0.05; t[1] << -20, 0, 0.05; t[2] << 0, 20, -0.1; - Plane pl(Vec3f(0, 0, 1), 0); + Plane pl(Vec3s(0, 0, 1), 0); TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); // identity @@ -1170,7 +1170,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, 0.05)); + tf_tri.setTranslation(Vec3s(0, 0, 0.05)); normal = pl.n; SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -1179,13 +1179,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0, -0.06)); + tf_tri.setTranslation(Vec3s(0, 0, -0.06)); SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, 0, 0.11)); + tf_tri.setTranslation(Vec3s(0, 0, 0.11)); SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1193,11 +1193,11 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 30, 0.05, 0; t[1] << -20, 0.05, 0; t[2] << 0, -0.1, 20; - Plane pl(Vec3f(0, 1, 0), 0); + Plane pl(Vec3s(0, 1, 0), 0); TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); // identity @@ -1209,7 +1209,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, 0.05, 0)); + tf_tri.setTranslation(Vec3s(0, 0.05, 0)); normal = pl.n; SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -1218,13 +1218,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0, -0.06, 0)); + tf_tri.setTranslation(Vec3s(0, -0.06, 0)); SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0, 0.11, 0)); + tf_tri.setTranslation(Vec3s(0, 0.11, 0)); SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1232,11 +1232,11 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { } { - Vec3f t[3]; + Vec3s t[3]; t[0] << 0.05, 30, 0; t[1] << 0.05, -10, 0; t[2] << -0.1, 0, 20; - Plane pl(Vec3f(1, 0, 0), 0); + Plane pl(Vec3s(1, 0, 0), 0); TriangleP tri(t[0], t[1], t[2]); Transform3f tf_tri = Transform3f(); // identity @@ -1248,7 +1248,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(0.05, 0, 0)); + tf_tri.setTranslation(Vec3s(0.05, 0, 0)); normal = pl.n; SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); @@ -1257,13 +1257,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, &normal); - tf_tri.setTranslation(Vec3f(-0.06, 0, 0)); + tf_tri.setTranslation(Vec3s(-0.06, 0, 0)); SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); - tf_tri.setTranslation(Vec3f(0.11, 0, 0)); + tf_tri.setTranslation(Vec3s(0.11, 0, 0)); SET_LINE; testShapeCollide(pl, Transform3f(), tri, tf_tri, false); SET_LINE; @@ -1273,7 +1273,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { Sphere s(10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -1281,9 +1281,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -1295,14 +1295,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-5, 0, 0)); + contact = transform.transform(Vec3s(-5, 0, 0)); depth = -10; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5, 0, 0)); + tf2 = Transform3f(Vec3s(5, 0, 0)); contact << -2.5, 0, 0; depth = -15; normal << -1, 0, 0; @@ -1310,15 +1310,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -15; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5, 0, 0)); + tf2 = Transform3f(Vec3s(-5, 0, 0)); contact << -7.5, 0, 0; depth = -5; normal << -1, 0, 0; @@ -1326,25 +1326,25 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); - contact = transform.transform(Vec3f(-7.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5, 0, 0)); + contact = transform.transform(Vec3s(-7.5, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-10.1, 0, 0)); + tf2 = Transform3f(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.1, 0, 0)); + tf2 = Transform3f(Vec3s(10.1, 0, 0)); contact << 0.05, 0, 0; depth = -20.1; normal << -1, 0, 0; @@ -1352,17 +1352,17 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); + tf2 = transform * Transform3f(Vec3s(10.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -20.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); } BOOST_AUTO_TEST_CASE(collide_planesphere) { Sphere s(10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -1370,13 +1370,13 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; - Vec3f p1, p2; + Vec3s normal; + Vec3s p1, p2; CoalScalar eps = 1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); + tf1 = Transform3f(Vec3s(eps, 0, 0)); tf2 = Transform3f(); depth = -10 + eps; p1 << -10 + eps, 0, 0; @@ -1390,12 +1390,12 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); + tf1 = Transform3f(Vec3s(eps, 0, 0)); tf2 = Transform3f(); depth = -10 - eps; p1 << 10 + eps, 0, 0; @@ -1408,12 +1408,12 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { tf1 = transform * tf1; tf2 = transform; contact = transform.transform((p1 + p2) / 2); - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5, 0, 0)); + tf2 = Transform3f(Vec3s(5, 0, 0)); p1 << 10, 0, 0; p2 << 5, 0, 0; contact << (p1 + p2) / 2; @@ -1423,15 +1423,15 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5, 0, 0)); + tf2 = Transform3f(Vec3s(-5, 0, 0)); p1 << -10, 0, 0; p2 << -5, 0, 0; contact << (p1 + p2) / 2; @@ -1441,37 +1441,37 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-10.1, 0, 0)); + tf2 = Transform3f(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(10.1, 0, 0)); + tf2 = Transform3f(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacebox) { Box s(5, 10, 20); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -1479,9 +1479,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -1493,14 +1493,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-1.25, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(1.25, 0, 0)); + tf2 = Transform3f(Vec3s(1.25, 0, 0)); contact << -0.625, 0, 0; depth = -3.75; normal << -1, 0, 0; @@ -1508,15 +1508,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); - contact = transform.transform(Vec3f(-0.625, 0, 0)); + tf2 = transform * Transform3f(Vec3s(1.25, 0, 0)); + contact = transform.transform(Vec3s(-0.625, 0, 0)); depth = -3.75; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-1.25, 0, 0)); + tf2 = Transform3f(Vec3s(-1.25, 0, 0)); contact << -1.875, 0, 0; depth = -1.25; normal << -1, 0, 0; @@ -1524,15 +1524,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); - contact = transform.transform(Vec3f(-1.875, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-1.25, 0, 0)); + contact = transform.transform(Vec3s(-1.875, 0, 0)); depth = -1.25; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.51, 0, 0)); + tf2 = Transform3f(Vec3s(2.51, 0, 0)); contact << 0.005, 0, 0; depth = -5.01; normal << -1, 0, 0; @@ -1540,20 +1540,20 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); - contact = transform.transform(Vec3f(0.005, 0, 0)); + tf2 = transform * Transform3f(Vec3s(2.51, 0, 0)); + contact = transform.transform(Vec3s(0.005, 0, 0)); depth = -5.01; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.51, 0, 0)); + tf2 = Transform3f(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); @@ -1565,7 +1565,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { BOOST_AUTO_TEST_CASE(collide_planebox) { Box s(5, 10, 20); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -1573,14 +1573,14 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); - Vec3f p1(2.5, 0, 0); - Vec3f p2(0, 0, 0); + Vec3s p1(2.5, 0, 0); + Vec3s p2(0, 0, 0); contact << (p1 + p2) / 2; depth = -2.5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) @@ -1591,12 +1591,12 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(1.25, 0, 0)); + tf2 = Transform3f(Vec3s(1.25, 0, 0)); p2 << 1.25, 0, 0; contact << (p1 + p2) / 2; depth = -1.25; @@ -1605,15 +1605,15 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); + tf2 = transform * Transform3f(Vec3s(1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-1.25, 0, 0)); + tf2 = Transform3f(Vec3s(-1.25, 0, 0)); p1 << -2.5, 0, 0; p2 << -1.25, 0, 0; contact << (p1 + p2) / 2; @@ -1623,30 +1623,30 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.51, 0, 0)); + tf2 = Transform3f(Vec3s(2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); + tf2 = transform * Transform3f(Vec3s(2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.51, 0, 0)); + tf2 = Transform3f(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); @@ -1658,7 +1658,7 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { Capsule s(5, 10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -1666,9 +1666,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -1680,14 +1680,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf2 = Transform3f(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; @@ -1695,15 +1695,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, 0)); + tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = Transform3f(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1711,15 +1711,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf2 = Transform3f(Vec3s(5.1, 0, 0)); contact << 0.05, 0, 0; depth = -10.1; normal << -1, 0, 0; @@ -1727,24 +1727,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); + tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 1, 0), 0); + hs = Halfspace(Vec3s(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -1756,14 +1756,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf2 = Transform3f(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; @@ -1771,15 +1771,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, 0)); + tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf2 = Transform3f(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; @@ -1787,15 +1787,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, 0)); + tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf2 = Transform3f(Vec3s(0, 5.1, 0)); contact << 0, 0.05, 0; depth = -10.1; normal << 0, -1, 0; @@ -1803,24 +1803,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, 0)); + tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + contact = transform.transform(Vec3s(0, 0.05, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf2 = Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 0, 1), 0); + hs = Halfspace(Vec3s(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -1832,14 +1832,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -5)); + contact = transform.transform(Vec3s(0, 0, -5)); depth = -10; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf2 = Transform3f(Vec3s(0, 0, 2.5)); contact << 0, 0, -3.75; depth = -12.5; normal << 0, 0, -1; @@ -1847,15 +1847,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); + tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -12.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf2 = Transform3f(Vec3s(0, 0, -2.5)); contact << 0, 0, -6.25; depth = -7.5; normal << 0, 0, -1; @@ -1863,15 +1863,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -6.25)); + tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -6.25)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf2 = Transform3f(Vec3s(0, 0, 10.1)); contact << 0, 0, 0.05; depth = -20.1; normal << 0, 0, -1; @@ -1879,27 +1879,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); + tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); + contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -20.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf2 = Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecapsule) { Capsule s(5, 10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -1907,9 +1907,9 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -1921,14 +1921,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); + contact = transform.transform(Vec3s(0, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf2 = Transform3f(Vec3s(2.5, 0, 0)); contact << 2.5, 0, 0; depth = -2.5; normal << 1, 0, 0; @@ -1936,15 +1936,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(2.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(2.5, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = Transform3f(Vec3s(-2.5, 0, 0)); contact << -2.5, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1952,34 +1952,34 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf2 = Transform3f(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 1, 0), 0); + hs = Plane(Vec3s(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -1991,14 +1991,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); + contact = transform.transform(Vec3s(0, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (0, 1, 0) or (0, -1, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf2 = Transform3f(Vec3s(0, 2.5, 0)); contact << 0, 2.5, 0; depth = -2.5; normal << 0, 1, 0; @@ -2006,15 +2006,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, 2.5, 0)); + tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, 2.5, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); + normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf2 = Transform3f(Vec3s(0, -2.5, 0)); contact << 0, -2.5, 0; depth = -2.5; normal << 0, -1, 0; @@ -2022,34 +2022,34 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -2.5, 0)); + tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf2 = Transform3f(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf2 = Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 0, 1), 0); + hs = Plane(Vec3s(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -2061,14 +2061,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, 0)); + contact = transform.transform(Vec3s(0, 0, 0)); depth = -10; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (0, 0, 1) or (0, 0, -1) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf2 = Transform3f(Vec3s(0, 0, 2.5)); contact << 0, 0, 2.5; depth = -7.5; normal << 0, 0, 1; @@ -2076,15 +2076,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, 2.5)); + tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, 2.5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf2 = Transform3f(Vec3s(0, 0, -2.5)); contact << 0, 0, -2.5; depth = -7.5; normal << 0, 0, -1; @@ -2092,37 +2092,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -2.5)); + tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf2 = Transform3f(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf2 = Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { Cylinder s(5, 10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -2130,9 +2130,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -2144,14 +2144,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf2 = Transform3f(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; @@ -2159,15 +2159,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, 0)); + tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = Transform3f(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -2175,15 +2175,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf2 = Transform3f(Vec3s(5.1, 0, 0)); contact << 0.05, 0, 0; depth = -10.1; normal << -1, 0, 0; @@ -2191,24 +2191,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, 0)); + tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 1, 0), 0); + hs = Halfspace(Vec3s(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -2220,14 +2220,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf2 = Transform3f(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; @@ -2235,15 +2235,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, 0)); + tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf2 = Transform3f(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; @@ -2251,15 +2251,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, 0)); + tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf2 = Transform3f(Vec3s(0, 5.1, 0)); contact << 0, 0.05, 0; depth = -10.1; normal << 0, -1, 0; @@ -2267,24 +2267,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, 0)); + tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + contact = transform.transform(Vec3s(0, 0.05, 0)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf2 = Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 0, 1), 0); + hs = Halfspace(Vec3s(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -2296,14 +2296,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf2 = Transform3f(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; @@ -2311,15 +2311,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -1.25)); + tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf2 = Transform3f(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; @@ -2327,15 +2327,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); + tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 5.1)); + tf2 = Transform3f(Vec3s(0, 0, 5.1)); contact << 0, 0, 0.05; depth = -10.1; normal << 0, 0, -1; @@ -2343,27 +2343,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); + tf2 = transform * Transform3f(Vec3s(0, 0, 5.1)); + contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -5.1)); + tf2 = Transform3f(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecylinder) { Cylinder s(5, 10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -2371,13 +2371,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; - Vec3f p1, p2; + Vec3s normal; + Vec3s p1, p2; CoalScalar eps = 1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); + tf1 = Transform3f(Vec3s(eps, 0, 0)); tf2 = Transform3f(); p1 << -5 + eps, 0, 0; p2 << 0, 0, 0; @@ -2392,12 +2392,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); + tf1 = Transform3f(Vec3s(eps, 0, 0)); tf2 = Transform3f(); p1 << 5 + eps, 0, 0; p2 << 0, 0, 0; @@ -2412,12 +2412,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf2 = Transform3f(Vec3s(2.5, 0, 0)); p1 << 5, 0, 0; p2 << 2.5, 0, 0; contact << (p1 + p2) / 2; @@ -2427,15 +2427,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = Transform3f(Vec3s(-2.5, 0, 0)); p1 << -5, 0, 0; p2 << -2.5, 0, 0; contact << (p1 + p2) / 2; @@ -2445,37 +2445,37 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf2 = Transform3f(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 1, 0), 0); + hs = Plane(Vec3s(0, 1, 0), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); + tf1 = Transform3f(Vec3s(0, eps, 0)); tf2 = Transform3f(); p1 << 0, -5 + eps, 0; p2 << 0, 0, 0; @@ -2489,12 +2489,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); + tf1 = Transform3f(Vec3s(0, eps, 0)); tf2 = Transform3f(); p1 << 0, 5 + eps, 0; p2 << 0, 0, 0; @@ -2508,12 +2508,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf2 = Transform3f(Vec3s(0, 2.5, 0)); p1 << 0, 5, 0; p2 << 0, 2.5, 0; contact << (p1 + p2) / 2; @@ -2523,15 +2523,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); + tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); + normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf2 = Transform3f(Vec3s(0, -2.5, 0)); p1 << 0, -5, 0; p2 << 0, -2.5, 0; contact << (p1 + p2) / 2; @@ -2541,37 +2541,37 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); + tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf2 = Transform3f(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf2 = Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 0, 1), 0); + hs = Plane(Vec3s(0, 0, 1), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); + tf1 = Transform3f(Vec3s(0, 0, eps)); tf2 = Transform3f(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; @@ -2585,12 +2585,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); + tf1 = Transform3f(Vec3s(0, 0, eps)); tf2 = Transform3f(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; @@ -2604,12 +2604,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf2 = Transform3f(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; @@ -2619,15 +2619,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); + tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf2 = Transform3f(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5.; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; @@ -2637,37 +2637,37 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); + tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf2 = Transform3f(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf2 = Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_halfspacecone) { Cone s(5, 10); - Halfspace hs(Vec3f(1, 0, 0), 0); + Halfspace hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -2675,9 +2675,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; + Vec3s normal; tf1 = Transform3f(); tf2 = Transform3f(); @@ -2689,14 +2689,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(-2.5, 0, -5)); + contact = transform.transform(Vec3s(-2.5, 0, -5)); depth = -5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf2 = Transform3f(Vec3s(2.5, 0, 0)); contact << -1.25, 0, -5; depth = -7.5; normal << -1, 0, 0; @@ -2704,15 +2704,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); - contact = transform.transform(Vec3f(-1.25, 0, -5)); + tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + contact = transform.transform(Vec3s(-1.25, 0, -5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = Transform3f(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, -5; depth = -2.5; normal << -1, 0, 0; @@ -2720,15 +2720,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); - contact = transform.transform(Vec3f(-3.75, 0, -5)); + tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + contact = transform.transform(Vec3s(-3.75, 0, -5)); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf2 = Transform3f(Vec3s(5.1, 0, 0)); contact << 0.05, 0, -5; depth = -10.1; normal << -1, 0, 0; @@ -2736,24 +2736,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); - contact = transform.transform(Vec3f(0.05, 0, -5)); + tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + contact = transform.transform(Vec3s(0.05, 0, -5)); depth = -10.1; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 1, 0), 0); + hs = Halfspace(Vec3s(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -2765,14 +2765,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, -2.5, -5)); + contact = transform.transform(Vec3s(0, -2.5, -5)); depth = -5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf2 = Transform3f(Vec3s(0, 2.5, 0)); contact << 0, -1.25, -5; depth = -7.5; normal << 0, -1, 0; @@ -2780,15 +2780,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); - contact = transform.transform(Vec3f(0, -1.25, -5)); + tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + contact = transform.transform(Vec3s(0, -1.25, -5)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf2 = Transform3f(Vec3s(0, -2.5, 0)); contact << 0, -3.75, -5; depth = -2.5; normal << 0, -1, 0; @@ -2796,15 +2796,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); - contact = transform.transform(Vec3f(0, -3.75, -5)); + tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + contact = transform.transform(Vec3s(0, -3.75, -5)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf2 = Transform3f(Vec3s(0, 5.1, 0)); contact << 0, 0.05, -5; depth = -10.1; normal << 0, -1, 0; @@ -2812,24 +2812,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); - contact = transform.transform(Vec3f(0, 0.05, -5)); + tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + contact = transform.transform(Vec3s(0, 0.05, -5)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf2 = Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Halfspace(Vec3f(0, 0, 1), 0); + hs = Halfspace(Vec3s(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); @@ -2841,14 +2841,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { tf1 = transform; tf2 = transform; - contact = transform.transform(Vec3f(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf2 = Transform3f(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; @@ -2856,15 +2856,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); - contact = transform.transform(Vec3f(0, 0, -1.25)); + tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf2 = Transform3f(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; @@ -2872,15 +2872,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); - contact = transform.transform(Vec3f(0, 0, -3.75)); + tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 5.1)); + tf2 = Transform3f(Vec3s(0, 0, 5.1)); contact << 0, 0, 0.05; depth = -10.1; normal << 0, 0, -1; @@ -2888,27 +2888,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); - contact = transform.transform(Vec3f(0, 0, 0.05)); + tf2 = transform * Transform3f(Vec3s(0, 0, 5.1)); + contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -10.1; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -5.1)); + tf2 = Transform3f(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planecone) { Cone s(5, 10); - Plane hs(Vec3f(1, 0, 0), 0); + Plane hs(Vec3s(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; @@ -2916,13 +2916,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { Transform3f transform; generateRandomTransform(extents, transform); - Vec3f contact; + Vec3s contact; CoalScalar depth; - Vec3f normal; - Vec3f p1, p2; + Vec3s normal; + Vec3s p1, p2; CoalScalar eps = 1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); + tf1 = Transform3f(Vec3s(eps, 0, 0)); tf2 = Transform3f(); p1 << -5 + eps, 0, -5; p2 << 0, 0, -5; @@ -2937,12 +2937,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(eps, 0, 0)); + tf1 = Transform3f(Vec3s(eps, 0, 0)); tf2 = Transform3f(); p1 << 5 + eps, 0, -5; p2 << 0, 0, -5; @@ -2957,12 +2957,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; normal = - transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) + transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(2.5, 0, 0)); + tf2 = Transform3f(Vec3s(2.5, 0, 0)); p1 << 5, 0, -5; p2 << 2.5, 0, -5; contact << (p1 + p2) / 2; @@ -2972,15 +2972,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(1, 0, 0); + normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = Transform3f(Vec3s(-2.5, 0, 0)); p1 << -5, 0, -5; p2 << -2.5, 0, -5; contact << (p1 + p2) / 2; @@ -2990,37 +2990,37 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(-1, 0, 0); + normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(5.1, 0, 0)); + tf2 = Transform3f(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); + tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 1, 0), 0); + hs = Plane(Vec3s(0, 1, 0), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); + tf1 = Transform3f(Vec3s(0, eps, 0)); tf2 = Transform3f(); p1 << 0, -5 + eps, -5; p2 << 0, 0, -5; @@ -3034,12 +3034,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, eps, 0)); + tf1 = Transform3f(Vec3s(0, eps, 0)); tf2 = Transform3f(); p1 << 0, 5 + eps, -5; p2 << 0, 0, -5; @@ -3053,12 +3053,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 2.5, 0)); + tf2 = Transform3f(Vec3s(0, 2.5, 0)); p1 << 0, 5, -5; p2 << 0, 2.5, -5; contact << (p1 + p2) / 2; @@ -3068,15 +3068,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); + tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 1, 0); + normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -2.5, 0)); + tf2 = Transform3f(Vec3s(0, -2.5, 0)); p1 << 0, -5, -5; p2 << 0, -2.5, -5; contact << (p1 + p2) / 2; @@ -3086,37 +3086,37 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); + tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, -1, 0); + normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 5.1, 0)); + tf2 = Transform3f(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, -5.1, 0)); + tf2 = Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); + tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - hs = Plane(Vec3f(0, 0, 1), 0); + hs = Plane(Vec3s(0, 0, 1), 0); eps = 1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); + tf1 = Transform3f(Vec3s(0, 0, eps)); tf2 = Transform3f(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; @@ -3130,12 +3130,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 + eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3f(0, 0, eps)); + tf1 = Transform3f(Vec3s(0, 0, eps)); tf2 = Transform3f(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; @@ -3149,12 +3149,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { tf2 = transform; contact = transform.transform((p1 + p2) / 2); depth = -5 - eps; - normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) + normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 2.5)); + tf2 = Transform3f(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; @@ -3164,15 +3164,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); + tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, 1); + normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -2.5)); + tf2 = Transform3f(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; @@ -3182,30 +3182,30 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); + tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; - normal = transform.getRotation() * Vec3f(0, 0, -1); + normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, 10.1)); + tf2 = Transform3f(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = Transform3f(); - tf2 = Transform3f(Vec3f(0, 0, -10.1)); + tf2 = Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); + tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } @@ -3214,15 +3214,15 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { Transform3f tf1; Transform3f tf2; - Vec3f normal; - Vec3f contact; + Vec3s normal; + Vec3s contact; CoalScalar distance; Transform3f transform; generateRandomTransform(extents, transform); { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset = 3.14; Plane plane1(n, offset); Plane plane2(n, offset); @@ -3249,7 +3249,7 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset1 = 3.14; CoalScalar offset2 = offset1 + 1.19841; Plane plane1(n, offset1); @@ -3267,7 +3267,7 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset1 = 3.14; CoalScalar offset2 = offset1 - 1.19841; Plane plane1(n, offset1); @@ -3285,10 +3285,10 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n1(1, 0, 0); + Vec3s n1(1, 0, 0); CoalScalar offset1 = 3.14; Plane plane1(n1, offset1); - Vec3f n2(0, 0, 1); + Vec3s n2(0, 0, 1); CoalScalar offset2 = -2.13; Plane plane2(n2, offset2); @@ -3307,10 +3307,10 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } { - Vec3f n1(1, 0, 0); + Vec3s n1(1, 0, 0); CoalScalar offset1 = 3.14; Plane plane1(n1, offset1); - Vec3f n2(1, 1, 1); + Vec3s n2(1, 1, 1); CoalScalar offset2 = -2.13; Plane plane2(n2, offset2); @@ -3334,15 +3334,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { Transform3f tf1; Transform3f tf2; - Vec3f normal; - Vec3f contact; + Vec3s normal; + Vec3s contact; CoalScalar distance; Transform3f transform; generateRandomTransform(extents, transform); { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset = 3.14; Halfspace hf1(n, offset); Halfspace hf2(n, offset); @@ -3361,7 +3361,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset1 = 3.14; CoalScalar offset2 = offset1 + 1.19841; Halfspace hf1(n, offset1); @@ -3381,7 +3381,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset1 = 3.14; CoalScalar offset2 = offset1 - 1.19841; Halfspace hf1(n, offset1); @@ -3402,10 +3402,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n1(1, 0, 0); + Vec3s n1(1, 0, 0); CoalScalar offset1 = 3.14; Halfspace hf1(n1, offset1); - Vec3f n2(0, 0, 1); + Vec3s n2(0, 0, 1); CoalScalar offset2 = -2.13; Halfspace hf2(n2, offset2); @@ -3423,10 +3423,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } { - Vec3f n1(1, 0, 0); + Vec3s n1(1, 0, 0); CoalScalar offset1 = 3.14; Halfspace hf1(n1, offset1); - Vec3f n2(1, 1, 1); + Vec3s n2(1, 1, 1); CoalScalar offset2 = -2.13; Halfspace hf2(n2, offset2); @@ -3450,15 +3450,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { Transform3f tf1; Transform3f tf2; - Vec3f normal; - Vec3f contact; + Vec3s normal; + Vec3s contact; CoalScalar distance; Transform3f transform; generateRandomTransform(extents, transform); { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset = 3.14; Halfspace hf(n, offset); Plane plane(n, offset); @@ -3478,7 +3478,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset1 = 3.14; CoalScalar offset2 = offset1 + 1.19841; Halfspace hf(n, offset1); @@ -3499,7 +3499,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n = Vec3f::Random().normalized(); + Vec3s n = Vec3s::Random().normalized(); CoalScalar offset1 = 3.14; CoalScalar offset2 = offset1 - 1.19841; Halfspace hf(n, offset1); @@ -3520,10 +3520,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n1(1, 0, 0); + Vec3s n1(1, 0, 0); CoalScalar offset1 = 3.14; Halfspace hf(n1, offset1); - Vec3f n2(0, 0, 1); + Vec3s n2(0, 0, 1); CoalScalar offset2 = -2.13; Plane plane(n2, offset2); @@ -3541,10 +3541,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { } { - Vec3f n1(1, 0, 0); + Vec3s n1(1, 0, 0); CoalScalar offset1 = 3.14; Halfspace hf(n1, offset1); - Vec3f n2(1, 1, 1); + Vec3s n2(1, 1, 1); CoalScalar offset2 = -2.13; Plane plane(n2, offset2); @@ -3567,7 +3567,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { Sphere s1(20); Sphere s2(10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3f transform; @@ -3576,61 +3576,61 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { CoalScalar dist = -1; dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), + s1, Transform3f(), s2, Transform3f(Vec3s(30.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), + s1, Transform3f(), s2, Transform3f(Vec3s(29.9, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, + dist = solver1.shapeDistance(s1, Transform3f(Vec3s(40, 0, 0)), s2, Transform3f(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, + dist = solver1.shapeDistance(s1, Transform3f(Vec3s(30.1, 0, 0)), s2, Transform3f(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, + dist = solver1.shapeDistance(s1, Transform3f(Vec3s(29.9, 0, 0)), s2, Transform3f(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(30.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(29.9, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, + dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(40, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), + dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(30.1, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), + dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(29.9, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); @@ -3639,7 +3639,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3f transform; @@ -3658,57 +3658,57 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(20.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(0, 20.2, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.2) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 10.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(20.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(0, 20.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), + s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 10.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(15.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), compute_penetration, + s1, Transform3f(), s2, Transform3f(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); } @@ -3720,22 +3720,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { Transform3f tf1( Quatf(0.5279170511703305, -0.50981118132505521, -0.67596178682051911, 0.0668715876735793), - Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015)); + Vec3s(0.041218354748013122, 1.2022554710435607, 0.77338855025700015)); Transform3f tf2( Quatf(0.70738826916719977, 0, 0, 0.70682518110536596), - Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003)); + Vec3s(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003)); GJKSolver solver; - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; bool compute_penetration = true; solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); // If objects are not colliding, p2 should be outside the cylinder and // p1 should be outside the box - Vec3f p2Loc(tf1.inverse().transform(p2)); + Vec3s p2Loc(tf1.inverse().transform(p2)); bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.halfLength) && (p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius)); - Vec3f p1Loc(tf2.inverse().transform(p1)); + Vec3s p1Loc(tf2.inverse().transform(p1)); bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all(); std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl; std::cout << "p1 in box = (" << p1Loc.transpose() << ")" << std::endl; @@ -3759,7 +3759,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { s1 = Cylinder(0.06, 0.1); tf1.setTranslation( - Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293)); + Vec3s(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293)); tf1.setQuatRotation(Quatf(0.52613359459338371, 0.32189408354839893, 0.70415587451837913, -0.35175580165512249)); solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal); @@ -3768,7 +3768,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { Sphere s1(20); Box s2(5, 5, 5); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3f transform; @@ -3779,17 +3779,17 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { int N = 10; for (int i = 0; i < N + 1; ++i) { CoalScalar dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N); - dist = solver1.shapeDistance(s1, Transform3f(Vec3f(dbox, 0., 0.)), s2, + dist = solver1.shapeDistance(s1, Transform3f(Vec3s(dbox, 0., 0.)), s2, Transform3f(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); - EIGEN_VECTOR_IS_APPROX(normal, -Vec3f(1, 0, 0), 1e-6); + EIGEN_VECTOR_IS_APPROX(normal, -Vec3s(1, 0, 0), 1e-6); dist = solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); dist = solver1.shapeDistance( - s1, transform * Transform3f(Vec3f(dbox, 0., 0.)), s2, transform, + s1, transform * Transform3f(Vec3s(dbox, 0., 0.)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), 1e-6); @@ -3806,22 +3806,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), + s1, Transform3f(), s2, Transform3f(Vec3s(22.6, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(22.6, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); } @@ -3829,7 +3829,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3f transform; @@ -3874,22 +3874,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); } @@ -3897,7 +3897,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { Cone s1(5, 10); Cone s2(5, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3f transform; @@ -3942,22 +3942,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), compute_penetration, + s1, Transform3f(), s2, Transform3f(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), + s1, transform, s2, transform * Transform3f(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); } @@ -3965,7 +3965,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); - Vec3f closest_p1, closest_p2, normal; + Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; Transform3f transform; @@ -4010,22 +4010,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), + s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.02); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration, + s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.01); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), + s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.1); } @@ -4033,16 +4033,16 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { template void testReversibleShapeDistance(const S1& s1, const S2& s2, CoalScalar distance) { - Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); - Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); + Transform3f tf1(Vec3s(-0.5 * distance, 0.0, 0.0)); + Transform3f tf2(Vec3s(+0.5 * distance, 0.0, 0.0)); CoalScalar distA; CoalScalar distB; - Vec3f p1A; - Vec3f p1B; - Vec3f p2A; - Vec3f p2B; - Vec3f normalA, normalB; + Vec3s p1A; + Vec3s p1B; + Vec3s p2A; + Vec3s p2B; + Vec3s normalA, normalB; bool compute_penetration = true; const double tol = 1e-6; @@ -4083,8 +4083,8 @@ BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) { Capsule capsule(5, 10); Cone cone(5, 10); Cylinder cylinder(5, 10); - Plane plane(Vec3f(0, 0, 0), 0.0); - Halfspace halfspace(Vec3f(0, 0, 0), 0.0); + Plane plane(Vec3s(0, 0, 0), 0.0); + Halfspace halfspace(Vec3s(0, 0, 0), 0.0); // Use sufficiently long distance so that all the primitive shapes CANNOT // intersect diff --git a/test/gjk.cpp b/test/gjk.cpp index cdfb4d5e7..bb4d8996d 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -49,11 +49,11 @@ using coal::CoalScalar; using coal::GJKSolver; using coal::GJKVariant; -using coal::Matrix3f; +using coal::Matrix3s; using coal::Quatf; using coal::Transform3f; using coal::TriangleP; -using coal::Vec3f; +using coal::Vec3s; typedef Eigen::Matrix vector_t; typedef Eigen::Matrix vector6_t; @@ -79,8 +79,8 @@ void test_gjk_distance_triangle_triangle( if (enable_gjk_nesterov_acceleration) solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration; Transform3f tf1, tf2; - Vec3f p1, p2, a1, a2; - Matrix3f M; + Vec3s p1, p2, a1, a2; + Matrix3s M; CoalScalar distance(sqrt(-1)); clock_t start, end; @@ -88,41 +88,41 @@ void test_gjk_distance_triangle_triangle( CoalScalar eps = 1e-7; Results_t results(N); for (std::size_t i = 0; i < N; ++i) { - Vec3f P1_loc(Vec3f::Random()), P2_loc(Vec3f::Random()), - P3_loc(Vec3f::Random()); - Vec3f Q1_loc(Vec3f::Random()), Q2_loc(Vec3f::Random()), - Q3_loc(Vec3f::Random()); + Vec3s P1_loc(Vec3s::Random()), P2_loc(Vec3s::Random()), + P3_loc(Vec3s::Random()); + Vec3s Q1_loc(Vec3s::Random()), Q2_loc(Vec3s::Random()), + Q3_loc(Vec3s::Random()); if (i == 0) { - P1_loc = Vec3f(0.063996093749999997, -0.15320971679687501, + P1_loc = Vec3s(0.063996093749999997, -0.15320971679687501, -0.42799999999999999); P2_loc = - Vec3f(0.069105957031249998, -0.150722900390625, -0.42999999999999999); - P3_loc = Vec3f(0.063996093749999997, -0.15320971679687501, + Vec3s(0.069105957031249998, -0.150722900390625, -0.42999999999999999); + P3_loc = Vec3s(0.063996093749999997, -0.15320971679687501, -0.42999999999999999); Q1_loc = - Vec3f(-25.655000000000001, -1.2858199462890625, 3.7249809570312502); - Q2_loc = Vec3f(-10.926, -1.284259033203125, 3.7281499023437501); - Q3_loc = Vec3f(-10.926, -1.2866180419921875, 3.72335400390625); + Vec3s(-25.655000000000001, -1.2858199462890625, 3.7249809570312502); + Q2_loc = Vec3s(-10.926, -1.284259033203125, 3.7281499023437501); + Q3_loc = Vec3s(-10.926, -1.2866180419921875, 3.72335400390625); Transform3f tf( Quatf(-0.42437287410898855, -0.26862477561450587, -0.46249645019513175, 0.73064726592483387), - Vec3f(-12.824601270753471, -1.6840516940066426, 3.8914453043793844)); + Vec3s(-12.824601270753471, -1.6840516940066426, 3.8914453043793844)); tf1 = tf; } else if (i == 1) { P1_loc = - Vec3f(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622); + Vec3s(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622); P2_loc = - Vec3f(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622); + Vec3s(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622); P3_loc = - Vec3f(0.8027043342590332, 0.30276307463645935, -0.4372950792312622); + Vec3s(0.8027043342590332, 0.30276307463645935, -0.4372950792312622); Q1_loc = - Vec3f(-0.224713996052742, -0.7417119741439819, 0.19999997317790985); + Vec3s(-0.224713996052742, -0.7417119741439819, 0.19999997317790985); Q2_loc = - Vec3f(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985); + Vec3s(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985); Q3_loc = - Vec3f(-0.224713996052742, -0.7417119741439819, 0.09999997168779373); - Matrix3f R; - Vec3f T; + Vec3s(-0.224713996052742, -0.7417119741439819, 0.09999997168779373); + Matrix3s R; + Vec3s T; R << 0.9657787025454787, 0.09400415350535746, 0.24173273843919627, -0.06713698817647556, 0.9908494114820345, -0.11709000206805695, -0.25052768814676646, 0.09685382227587608, 0.9632524147814993; @@ -136,7 +136,7 @@ void test_gjk_distance_triangle_triangle( TriangleP tri1(P1_loc, P2_loc, P3_loc); TriangleP tri2(Q1_loc, Q2_loc, Q3_loc); - Vec3f normal; + Vec3s normal; const bool compute_penetration = true; coal::DistanceRequest request(compute_penetration, compute_penetration); coal::DistanceResult result; @@ -155,7 +155,7 @@ void test_gjk_distance_triangle_triangle( results[i].timeGjk = end - start; results[i].collision = res; if (res) { - Vec3f c1, c2, normal2; + Vec3s c1, c2, normal2; ++nCol; // check that moving triangle 2 by the penetration depth in the // direction of the normal makes the triangles collision free. @@ -189,15 +189,15 @@ void test_gjk_distance_triangle_triangle( tf2.setIdentity(); } // Compute vectors between vertices - Vec3f P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)), + Vec3s P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)), P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)), Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc)); - Vec3f u1(P2 - P1); - Vec3f v1(P3 - P1); - Vec3f w1(u1.cross(v1)); - Vec3f u2(Q2 - Q1); - Vec3f v2(Q3 - Q1); - Vec3f w2(u2.cross(v2)); + Vec3s u1(P2 - P1); + Vec3s v1(P3 - P1); + Vec3s w1(u1.cross(v1)); + Vec3s u2(Q2 - Q1); + Vec3s v2(Q3 - Q1); + Vec3s w2(u2.cross(v2)); BOOST_CHECK(w1.squaredNorm() > eps * eps); M.col(0) = u1; M.col(1) = v1; @@ -334,7 +334,7 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) { test_gjk_distance_triangle_triangle(true); } -void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray, +void test_gjk_unit_sphere(CoalScalar center_distance, Vec3s ray, double swept_sphere_radius, bool use_gjk_nesterov_acceleration) { using namespace coal; @@ -343,7 +343,7 @@ void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray, sphere.setSweptSphereRadius(swept_sphere_radius); typedef Eigen::Matrix Vec4f; - Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3f::Zero()); + Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3s::Zero()); Transform3f tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray); bool expect_collision = center_distance <= 2 * (r + swept_sphere_radius); @@ -359,7 +359,7 @@ void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray, details::GJK gjk(2, 1e-6); if (use_gjk_nesterov_acceleration) gjk.gjk_variant = GJKVariant::NesterovAcceleration; - details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); + details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0)); if (expect_collision) { BOOST_CHECK((status == details::GJK::Collision) || @@ -372,12 +372,12 @@ void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray, BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); } - Vec3f w0, w1, normal; + Vec3s w0, w1, normal; gjk.getWitnessPointsAndNormal(shape, w0, w1, normal); - Vec3f w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) + + Vec3s w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) + swept_sphere_radius * normal); - Vec3f w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) - + Vec3s w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) - swept_sphere_radius * normal); EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10); @@ -389,36 +389,36 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { std::array swept_sphere_radius = {0., 0.1, 1., 10., 100.}; for (bool nesterov_acceleration : use_nesterov_acceleration) { for (double ssr : swept_sphere_radius) { - test_gjk_unit_sphere(3, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(3, Vec3s(1, 0, 0), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(2.01, Vec3s(1, 0, 0), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(2.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration); - test_gjk_unit_sphere(1.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration); + test_gjk_unit_sphere(1.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration); // Random rotation - test_gjk_unit_sphere(3, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(3, Vec3s::Random().normalized(), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(2.01, Vec3s::Random().normalized(), ssr, nesterov_acceleration); - test_gjk_unit_sphere(2.0, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(2.0, Vec3s::Random().normalized(), ssr, nesterov_acceleration); - test_gjk_unit_sphere(1.0, Vec3f::Random().normalized(), ssr, + test_gjk_unit_sphere(1.0, Vec3s::Random().normalized(), ssr, nesterov_acceleration); } } } -void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, +void test_gjk_triangle_capsule(Vec3s T, bool expect_collision, bool use_gjk_nesterov_acceleration, - Vec3f w0_expected, Vec3f w1_expected) { + Vec3s w0_expected, Vec3s w1_expected) { using namespace coal; Capsule capsule(1., 2.); // Radius 1 and length 2 - TriangleP triangle(Vec3f(0., 0., 0.), Vec3f(1., 0., 0.), Vec3f(1., 1., 0.)); + TriangleP triangle(Vec3s(0., 0., 0.), Vec3s(1., 0., 0.), Vec3s(1., 1., 0.)); Transform3f tf0, tf1; tf1.setTranslation(T); @@ -436,7 +436,7 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, details::GJK gjk(10, 1e-6); if (use_gjk_nesterov_acceleration) gjk.gjk_variant = GJKVariant::NesterovAcceleration; - details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); + details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0)); if (expect_collision) { BOOST_CHECK((status == details::GJK::Collision) || @@ -445,19 +445,19 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); // Check that guess works as expected - Vec3f guess = gjk.getGuessFromSimplex(); + Vec3s guess = gjk.getGuessFromSimplex(); details::GJK gjk2(3, 1e-6); details::GJK::Status status2 = gjk2.evaluate(shape, guess); BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision); } - Vec3f w0, w1, normal; + Vec3s w0, w1, normal; if (status == details::GJK::NoCollision || status == details::GJK::CollisionWithPenetrationInformation) { gjk.getWitnessPointsAndNormal(shape, w0, w1, normal); } else { details::EPA epa(64, 1e-6); - details::EPA::Status epa_status = epa.evaluate(gjk, Vec3f(1, 0, 0)); + details::EPA::Status epa_status = epa.evaluate(gjk, Vec3s(1, 0, 0)); BOOST_CHECK_EQUAL(epa_status, details::EPA::AccuracyReached); epa.getWitnessPointsAndNormal(shape, w0, w1, normal); } @@ -468,23 +468,23 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, BOOST_AUTO_TEST_CASE(triangle_capsule) { // GJK -> no collision - test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, false, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, false, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK + Nesterov acceleration -> no collision - test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, true, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, true, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK -> collision - test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, false, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, false, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK + Nesterov acceleration -> collision - test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, true, Vec3f(1., 0, 0), - Vec3f(0., 0, 0)); + test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, true, Vec3s(1., 0, 0), + Vec3s(0., 0, 0)); // GJK + EPA -> collision - test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, false, Vec3f(0, 1, 0), - Vec3f(0.5, 0, 0)); + test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, false, Vec3s(0, 1, 0), + Vec3s(0.5, 0, 0)); // GJK + Nesterov accleration + EPA -> collision - test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, true, Vec3f(0, 1, 0), - Vec3f(0.5, 0, 0)); + test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, true, Vec3s(0, 1, 0), + Vec3s(0.5, 0, 0)); } diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp index 4028b8690..d18a17f7c 100644 --- a/test/gjk_asserts.cpp +++ b/test/gjk_asserts.cpp @@ -13,8 +13,8 @@ double DegToRad(const double& deg) { static double degToRad = pi / 180.; return deg * degToRad; } -std::vector dirs{Vec3f::UnitZ(), -Vec3f::UnitZ(), Vec3f::UnitY(), - -Vec3f::UnitY(), Vec3f::UnitX(), -Vec3f::UnitX()}; +std::vector dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(), + -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()}; void CreateSphereMesh(BVHModel& model, const double& radius) { size_t polarSteps{32}; @@ -24,7 +24,7 @@ void CreateSphereMesh(BVHModel& model, const double& radius) { const float polarStep = PI / (float)(polarSteps - 1); const float azimuthStep = 2.0f * PI / (float)(azimuthSteps - 1); - std::vector vertices; + std::vector vertices; std::vector triangles; for (size_t p = 0; p < polarSteps; ++p) { @@ -77,9 +77,9 @@ BOOST_AUTO_TEST_CASE(TestSpheres) { (i == 86 && j == 52) || (i == 89 && j == 17) || (i == 89 && j == 58) || (i == 89 && j == 145)) { sphere2Tf.setQuatRotation( - Eigen::AngleAxis(DegToRad(i), Vec3f::UnitZ()) * - Eigen::AngleAxis(DegToRad(j), Vec3f::UnitY())); - for (const Vec3f& dir : dirs) { + Eigen::AngleAxis(DegToRad(i), Vec3s::UnitZ()) * + Eigen::AngleAxis(DegToRad(j), Vec3s::UnitY())); + for (const Vec3s& dir : dirs) { sphere2Tf.setTranslation(dir); CollisionResult result; diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index 5283e4514..139df1ada 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -54,7 +54,7 @@ using coal::GJKSolver; using coal::ShapeBase; using coal::support_func_guess_t; using coal::Transform3f; -using coal::Vec3f; +using coal::Vec3s; using coal::details::GJK; using coal::details::MinkowskiDiff; using std::size_t; @@ -128,7 +128,7 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, Transform3f identity = Transform3f::Identity(); // Same init for both solvers - Vec3f init_guess = Vec3f(1, 0, 0); + Vec3s init_guess = Vec3s(1, 0, 0); support_func_guess_t init_support_guess; init_support_guess.setZero(); @@ -138,21 +138,21 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(gjk1.getNumIterations() <= max_iterations); - Vec3f ray1 = gjk1.ray; + Vec3s ray1 = gjk1.ray; res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res1 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, 1e-8); GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(gjk2.getNumIterations() <= max_iterations); - Vec3f ray2 = gjk2.ray; + Vec3s ray2 = gjk2.ray; res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res2 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, 1e-8); GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(gjk3.getNumIterations() <= max_iterations); - Vec3f ray3 = gjk3.ray; + Vec3s ray3 = gjk3.ray; res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess); BOOST_CHECK(res3 != GJK::Status::Failed); EIGEN_VECTOR_IS_APPROX(gjk3.ray, ray3, 1e-8); diff --git a/test/hfields.cpp b/test/hfields.cpp index 0042cd802..a9fcc4c09 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -61,18 +61,18 @@ void test_constant_hfields(const Eigen::DenseIndex nx, const CoalScalar min_altitude, const CoalScalar max_altitude) { const CoalScalar x_dim = 1., y_dim = 2.; - const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); BOOST_CHECK(hfield.getXDim() == x_dim); BOOST_CHECK(hfield.getYDim() == y_dim); - const VecXf& x_grid = hfield.getXGrid(); + const VecXs& x_grid = hfield.getXGrid(); BOOST_CHECK(x_grid[0] == -x_dim / 2.); BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.); - const VecXf& y_grid = hfield.getYGrid(); + const VecXs& y_grid = hfield.getYGrid(); BOOST_CHECK(y_grid[0] == y_dim / 2.); BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.); @@ -81,7 +81,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx, for (Eigen::DenseIndex i = 0; i < nx; ++i) { for (Eigen::DenseIndex j = 0; j < ny; ++j) { - Vec3f point(x_grid[i], y_grid[j], heights(j, i)); + Vec3s point(x_grid[i], y_grid[j], heights(j, i)); BOOST_CHECK(hfield.aabb_local.contain(point)); } } @@ -98,13 +98,13 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); const Transform3f box_placement( - Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); + Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.)); // Test collision const Sphere sphere(1.); static const Transform3f IdTransform; - const Box box(Vec3f::Ones()); + const Box box(Vec3s::Ones()); Transform3f M_sphere, M_box; @@ -112,9 +112,9 @@ void test_constant_hfields(const Eigen::DenseIndex nx, { const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -139,9 +139,9 @@ void test_constant_hfields(const Eigen::DenseIndex nx, { const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); @@ -165,15 +165,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Update height hfield.updateHeights( - MatrixXf::Constant(ny, nx, max_altitude / 2.)); // We change nothing + MatrixXs::Constant(ny, nx, max_altitude / 2.)); // We change nothing // No collision case { const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -198,9 +198,9 @@ void test_constant_hfields(const Eigen::DenseIndex nx, { const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); @@ -224,15 +224,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Restore height hfield.updateHeights( - MatrixXf::Constant(ny, nx, max_altitude)); // We change nothing + MatrixXs::Constant(ny, nx, max_altitude)); // We change nothing // Collision case { const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); @@ -275,20 +275,20 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, const CoalScalar min_altitude, const CoalScalar max_altitude) { const CoalScalar x_dim = 1., y_dim = 2.; - const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); const Transform3f box_placement( - Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); + Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.)); // Test collision const Sphere sphere(1.); static const Transform3f IdTransform; - const Box box(Vec3f::Ones()); + const Box box(Vec3s::Ones()); Transform3f M_sphere, M_box; @@ -296,9 +296,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, { const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -323,9 +323,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, { const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; request.security_margin = eps_no_collision + 1e-6; @@ -351,9 +351,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, { const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; CollisionResult result; @@ -378,9 +378,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, { const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude); M_sphere.setTranslation( - Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); M_box.setTranslation( - Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); CollisionRequest request; request.security_margin = eps_no_collision - 1e-4; @@ -415,22 +415,22 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { const Eigen::DenseIndex nx = 100, ny = 100; typedef AABB BV; - const MatrixXf X = + const MatrixXs X = Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); - const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); + const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); const CoalScalar dim_square = 0.5; const Eigen::Array hole = (X.array().abs() < dim_square) && (Y.array().abs() < dim_square); - const MatrixXf heights = - MatrixXf::Ones(ny, nx) - hole.cast().matrix(); + const MatrixXs heights = + MatrixXs::Ones(ny, nx) - hole.cast().matrix(); const HeightField hfield(2., 2., heights, -10.); Sphere sphere(0.48); - const Transform3f sphere_pos(Vec3f(0., 0., 0.5)); + const Transform3f sphere_pos(Vec3s(0., 0., 0.5)); const Transform3f hfield_pos; const CollisionRequest request; @@ -459,17 +459,17 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { // typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug // mode), as the overlap of OBBRSS is not satisfactory yet. typedef AABB BV; - const MatrixXf X = + const MatrixXs X = Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); - const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); + const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); const CoalScalar dim_hole = 1; const Eigen::Array hole = (X.array().square() + Y.array().square() <= dim_hole); - const MatrixXf heights = - MatrixXf::Ones(ny, nx) - hole.cast().matrix(); + const MatrixXs heights = + MatrixXs::Ones(ny, nx) - hole.cast().matrix(); const HeightField hfield(2., 2., heights, -10.); @@ -480,7 +480,7 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); Sphere sphere(0.975); - const Transform3f sphere_pos(Vec3f(0., 0., 1.)); + const Transform3f sphere_pos(Vec3s(0., 0., 1.)); const Transform3f hfield_pos; const CoalScalar thresholds[3] = {0., 0.01, -0.005}; @@ -520,11 +520,11 @@ bool isApprox(const CoalScalar v1, const CoalScalar v2, return std::fabs(v1 - v2) <= tol; } -Vec3f computeFaceNormal(const Triangle& triangle, - const std::vector& points) { - const Vec3f pointA = points[triangle[0]]; - const Vec3f pointB = points[triangle[1]]; - const Vec3f pointC = points[triangle[2]]; +Vec3s computeFaceNormal(const Triangle& triangle, + const std::vector& points) { + const Vec3s pointA = points[triangle[0]]; + const Vec3s pointB = points[triangle[1]]; + const Vec3s pointC = points[triangle[2]]; return (pointB - pointA).cross(pointC - pointA).normalized(); } @@ -532,7 +532,7 @@ Vec3f computeFaceNormal(const Triangle& triangle, BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); - MatrixXf altitutes(2, 2); + MatrixXs altitutes(2, 2); CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); @@ -564,20 +564,20 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { // Check face normals for convex1 { - const std::vector& points = *(convex1.points); + const std::vector& points = *(convex1.points); // BOTTOM { const Triangle& triangle = (*(convex1.polygons))[0]; BOOST_CHECK( - computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ())); + computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); } // TOP { const Triangle& triangle = (*(convex1.polygons))[1]; - BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ())); + BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); } // WEST sides @@ -586,14 +586,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const Triangle& triangle2 = (*(convex1.polygons))[3]; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitX())); + computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitX())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitX())); + computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitX())); } // SOUTH-EAST sides { - const Vec3f south_east_normal = Vec3f(1., -1., 0).normalized(); + const Vec3s south_east_normal = Vec3s(1., -1., 0).normalized(); const Triangle& triangle1 = (*(convex1.polygons))[4]; const Triangle& triangle2 = (*(convex1.polygons))[5]; @@ -613,29 +613,29 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { << computeFaceNormal(triangle1, points).transpose() << std::endl; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitY())); + computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitY())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitY())); + computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitY())); } } // Check face normals for convex2 { - const std::vector& points = *(convex2.points); + const std::vector& points = *(convex2.points); // BOTTOM { const Triangle& triangle = (*(convex2.polygons))[0]; BOOST_CHECK( - computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ())); + computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); } // TOP { const Triangle& triangle = (*(convex2.polygons))[1]; - BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ())); + BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); } // SOUTH sides @@ -644,14 +644,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const Triangle& triangle2 = (*(convex2.polygons))[3]; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitY())); + computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitY())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitY())); + computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitY())); } // NORTH-WEST sides { - const Vec3f north_west_normal = Vec3f(-1., 1., 0).normalized(); + const Vec3s north_west_normal = Vec3s(-1., 1., 0).normalized(); const Triangle& triangle1 = (*(convex2.polygons))[4]; const Triangle& triangle2 = (*(convex2.polygons))[5]; @@ -668,9 +668,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { const Triangle& triangle2 = (*(convex2.polygons))[7]; BOOST_CHECK( - computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitX())); + computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitX())); BOOST_CHECK( - computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitX())); + computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitX())); } } } @@ -679,7 +679,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { typedef HFNodeBase::FaceOrientation FaceOrientation; const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); - MatrixXf altitutes(3, 3); + MatrixXs altitutes(3, 3); CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); @@ -716,7 +716,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { const CoalScalar sphere_radius = 1.; Sphere sphere(sphere_radius); - MatrixXf altitutes(2, 2); + MatrixXs altitutes(2, 2); CoalScalar altitude_value = 1.; altitutes.fill(altitude_value); @@ -728,7 +728,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the TOP { - const Transform3f sphere_pos(Vec3f(0., 0., 2.)); + const Transform3f sphere_pos(Vec3s(0., 0., 2.)); const Transform3f hfield_pos; CollisionResult result; @@ -743,7 +743,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Same, but with a positive margin. { - const Transform3f sphere_pos(Vec3f(0., 0., 2.)); + const Transform3f sphere_pos(Vec3s(0., 0., 2.)); const Transform3f hfield_pos; CollisionResult result; @@ -754,7 +754,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitZ())); + BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitZ())); std::cout << "contact.penetration_depth: " << contact.penetration_depth << std::endl; BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); @@ -763,7 +763,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the BOTTOM { - const Transform3f sphere_pos(Vec3f(0., 0., -1.)); + const Transform3f sphere_pos(Vec3s(0., 0., -1.)); const Transform3f hfield_pos; CollisionResult result; @@ -775,7 +775,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos(Vec3f(0., 0., -1.)); + const Transform3f sphere_pos(Vec3s(0., 0., -1.)); const Transform3f hfield_pos; CollisionResult result; @@ -786,7 +786,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { BOOST_CHECK(result.isCollision()); { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitZ())); + BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitZ())); std::cout << "contact.penetration_depth: " << contact.penetration_depth << std::endl; BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); @@ -796,7 +796,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the WEST { const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); + Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -809,7 +809,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { { const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); + Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -820,7 +820,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { BOOST_CHECK(result.isCollision()); if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitX())); + BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitX())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } @@ -828,7 +828,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the EAST { const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); + Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -841,7 +841,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { { const Transform3f sphere_pos( - Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); + Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -853,7 +853,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitX())); + BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitX())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } @@ -861,7 +861,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the NORTH { const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); + Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -874,7 +874,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { { const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); + Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -886,7 +886,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitY())); + BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitY())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } @@ -894,7 +894,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the SOUTH { const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); + Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -907,7 +907,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { { const Transform3f sphere_pos( - Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); + Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); const Transform3f hfield_pos; CollisionResult result; @@ -919,7 +919,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { if (result.isCollision()) { const Contact& contact = result.getContact(0); - BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitY())); + BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitY())); BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); } } diff --git a/test/math.cpp b/test/math.cpp index ad44b43f1..231739030 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -48,13 +48,13 @@ using namespace coal; BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { - Vec3f v1(1.0, 2.0, 3.0); + Vec3s v1(1.0, 2.0, 3.0); BOOST_CHECK(v1[0] == 1.0); BOOST_CHECK(v1[1] == 2.0); BOOST_CHECK(v1[2] == 3.0); - Vec3f v2 = v1; - Vec3f v3(3.3, 4.3, 5.3); + Vec3s v2 = v1; + Vec3s v3(3.3, 4.3, 5.3); v1 += v3; BOOST_CHECK(isEqual(v1, v2 + v3)); v1 -= v3; @@ -87,53 +87,53 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) { BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix())); v1.array() += 2.0; - BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-1.0, -2.0, -3.0))); + BOOST_CHECK(isEqual((-Vec3s(1.0, 2.0, 3.0)), Vec3s(-1.0, -2.0, -3.0))); - v1 = Vec3f(1.0, 2.0, 3.0); - v2 = Vec3f(3.0, 4.0, 5.0); - BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0))); + v1 = Vec3s(1.0, 2.0, 3.0); + v2 = Vec3s(3.0, 4.0, 5.0); + BOOST_CHECK(isEqual((v1.cross(v2)), Vec3s(-2.0, 4.0, -2.0))); BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5); - v1 = Vec3f(3.0, 4.0, 5.0); + v1 = Vec3s(3.0, 4.0, 5.0); BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5); BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5); BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm())); - v1 = Vec3f(1.0, 2.0, 3.0); - v2 = Vec3f(3.0, 4.0, 5.0); - BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0))); + v1 = Vec3s(1.0, 2.0, 3.0); + v2 = Vec3s(3.0, 4.0, 5.0); + BOOST_CHECK(isEqual(v1.cross(v2), Vec3s(-2.0, 4.0, -2.0))); BOOST_CHECK(v1.dot(v2) == 26); } -Vec3f rotate(Vec3f input, CoalScalar w, Vec3f vec) { +Vec3s rotate(Vec3s input, CoalScalar w, Vec3s vec) { return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input + 2 * w * vec.cross(input); } BOOST_AUTO_TEST_CASE(quaternion) { Quatf q1(Quatf::Identity()), q2, q3; - q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2); + q2 = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2); q3 = q2.inverse(); - Vec3f v(1, -1, 0); + Vec3s v(1, -1, 0); BOOST_CHECK(isEqual(v, q1 * v)); - BOOST_CHECK(isEqual(Vec3f(1, 1, 0), q2 * v)); + BOOST_CHECK(isEqual(Vec3s(1, 1, 0), q2 * v)); BOOST_CHECK( - isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v)); + isEqual(rotate(v, q3.w(), Vec3s(q3.x(), q3.y(), q3.z())), q3 * v)); } BOOST_AUTO_TEST_CASE(transform) { - Quatf q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2); - Vec3f T(0, 1, 2); + Quatf q = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2); + Vec3s T(0, 1, 2); Transform3f tf(q, T); - Vec3f v(1, -1, 0); + Vec3s v(1, -1, 0); BOOST_CHECK(isEqual(q * v + T, q * v + T)); - Vec3f rv(q * v); - // typename Transform3f::transform_return_type::type output = + Vec3s rv(q * v); + // typename Transform3f::transform_return_type::type output = // tf * v; // std::cout << rv << std::endl; // std::cout << output.lhs() << std::endl; diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp index e440afdc7..76e62af0f 100644 --- a/test/normal_and_nearest_points.cpp +++ b/test/normal_and_nearest_points.cpp @@ -128,16 +128,16 @@ void test_normal_and_nearest_points( Contact contact = colres.getContact(0); BOOST_CHECK_CLOSE(dist, contact.penetration_depth, dummy_precision); - Vec3f cp1 = contact.nearest_points[0]; + Vec3s cp1 = contact.nearest_points[0]; EIGEN_VECTOR_IS_APPROX(cp1, distres.nearest_points[0], dummy_precision); - Vec3f cp2 = contact.nearest_points[1]; + Vec3s cp2 = contact.nearest_points[1]; EIGEN_VECTOR_IS_APPROX(cp2, distres.nearest_points[1], dummy_precision); BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), epa_tolerance); EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, epa_tolerance); - Vec3f separation_vector = contact.penetration_depth * contact.normal; + Vec3s separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, epa_tolerance); if (dist < 0) { @@ -160,13 +160,13 @@ void test_normal_and_nearest_points( BOOST_CHECK(!new_colres.isCollision()); BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist, epa_tolerance); - Vec3f new_cp1 = new_distres.nearest_points[0]; - Vec3f new_cp2 = new_distres.nearest_points[1]; + Vec3s new_cp1 = new_distres.nearest_points[0]; + Vec3s new_cp2 = new_distres.nearest_points[1]; BOOST_CHECK_CLOSE(new_dist, (new_cp1 - new_cp2).norm(), epa_tolerance); EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal, epa_tolerance); - Vec3f new_separation_vector = new_dist * new_distres.normal; + Vec3s new_separation_vector = new_dist * new_distres.normal; EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1, epa_tolerance); @@ -179,12 +179,12 @@ void test_normal_and_nearest_points( BOOST_CHECK_CLOSE(distres.min_distance, dist, dummy_precision); BOOST_CHECK_CLOSE(dist, colres.distance_lower_bound, dummy_precision); - Vec3f cp1 = distres.nearest_points[0]; - Vec3f cp2 = distres.nearest_points[1]; + Vec3s cp1 = distres.nearest_points[0]; + Vec3s cp2 = distres.nearest_points[1]; BOOST_CHECK_CLOSE(dist, (cp1 - cp2).norm(), gjk_tolerance); EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, gjk_tolerance); - Vec3f separation_vector = dist * distres.normal; + Vec3s separation_vector = dist * distres.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, gjk_tolerance); if (dist > 0) { @@ -214,11 +214,11 @@ void test_normal_and_nearest_points( // tolerance if (new_colres.isCollision()) { Contact contact = new_colres.getContact(0); - Vec3f new_cp1 = contact.nearest_points[0]; + Vec3s new_cp1 = contact.nearest_points[0]; EIGEN_VECTOR_IS_APPROX(new_cp1, new_distres.nearest_points[0], dummy_precision); - Vec3f new_cp2 = contact.nearest_points[1]; + Vec3s new_cp2 = contact.nearest_points[1]; EIGEN_VECTOR_IS_APPROX(new_cp2, new_distres.nearest_points[1], dummy_precision); BOOST_CHECK_CLOSE(contact.penetration_depth, @@ -226,7 +226,7 @@ void test_normal_and_nearest_points( EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal, epa_tolerance); - Vec3f new_separation_vector = + Vec3s new_separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1, epa_tolerance); @@ -372,7 +372,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(0.05, 1.0))); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -385,7 +385,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Box(generateRandomVector<3>(0.05, 1.0))); CoalScalar offset = 0.1; - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -400,7 +400,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) { CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Capsule(r, h)); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -413,7 +413,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(0.05, 1.0))); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -426,7 +426,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_plane) { for (size_t i = 0; i < 10; ++i) { shared_ptr o1(new Sphere(generateRandomNumber(0.05, 1.0))); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -442,7 +442,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_halfspace) { shared_ptr> o1(new Convex( o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons)); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -497,7 +497,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) { CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cone(r, h)); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -512,7 +512,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) { CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cylinder(r, h)); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); @@ -527,7 +527,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) { CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cone(r, h)); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -542,7 +542,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) { CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Cylinder(r, h)); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -557,7 +557,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_plane) { CoalScalar h = generateRandomNumber(0.15, 1.0); shared_ptr o1(new Capsule(r, h)); CoalScalar offset = generateRandomNumber(-0.5, 0.5); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Plane(n, offset)); @@ -593,7 +593,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) { for (size_t i = 0; i < 10; ++i) { CoalScalar offset = generateRandomNumber(0.15, 1.0); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); shared_ptr o2(new Halfspace(n, offset)); @@ -606,7 +606,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) { BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_plane) { for (size_t i = 0; i < 10; ++i) { CoalScalar offset = generateRandomNumber(0.15, 1.0); - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0))); shared_ptr o2(new Plane(n, offset)); @@ -652,13 +652,13 @@ void test_normal_and_nearest_points(const BVHModel& o1, BOOST_CHECK(contact.penetration_depth <= 0); BOOST_CHECK(contact.penetration_depth >= colres.distance_lower_bound); - Vec3f cp1 = contact.nearest_points[0]; - Vec3f cp2 = contact.nearest_points[1]; + Vec3s cp1 = contact.nearest_points[0]; + Vec3s cp2 = contact.nearest_points[1]; BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), 1e-6); EIGEN_VECTOR_IS_APPROX( cp1, cp2 - contact.penetration_depth * contact.normal, 1e-6); - Vec3f separation_vector = contact.penetration_depth * contact.normal; + Vec3s separation_vector = contact.penetration_depth * contact.normal; EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6); if (dist < 0) { @@ -687,7 +687,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) { o1.buildConvexRepresentation(false); CoalScalar offset = 0.1; - Vec3f n = Vec3f::Random(); + Vec3s n = Vec3s::Random(); n.normalize(); shared_ptr o2(new Halfspace(n, offset)); diff --git a/test/obb.cpp b/test/obb.cpp index d8db1dd85..77c16fe3c 100644 --- a/test/obb.cpp +++ b/test/obb.cpp @@ -48,16 +48,16 @@ using namespace coal; -void randomOBBs(Vec3f& a, Vec3f& b, CoalScalar extentNorm) { +void randomOBBs(Vec3s& a, Vec3s& b, CoalScalar extentNorm) { // Extent norm is between 0 and extentNorm on each axis - // a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); - // b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3)); + // a = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); + // b = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); - a = extentNorm * Vec3f::Random().cwiseAbs().normalized(); - b = extentNorm * Vec3f::Random().cwiseAbs().normalized(); + a = extentNorm * Vec3s::Random().cwiseAbs().normalized(); + b = extentNorm * Vec3s::Random().cwiseAbs().normalized(); } -void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b, +void randomTransform(Matrix3s& B, Vec3s& T, const Vec3s& a, const Vec3s& b, const CoalScalar extentNorm) { // TODO Should we scale T to a and b norm ? (void)a; @@ -67,7 +67,7 @@ void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b, CoalScalar N = a.norm() + b.norm(); // A translation of norm N ensures there is no collision. // Set translation to be between 0 and 2 * N; - T = (Vec3f::Random() / sqrt(3)) * 1.5 * N; + T = (Vec3s::Random() / sqrt(3)) * 1.5 * N; // T.setZero(); Quatf q; @@ -103,22 +103,22 @@ const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, namespace obbDisjoint_impls { /// @return true if OBB are disjoint. -bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b, +bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, CoalScalar& distance) { GJKSolver gjk; Box ba(2 * a), bb(2 * b); Transform3f tfa, tfb(B, T); - Vec3f p1, p2, normal; + Vec3s p1, p2, normal; bool compute_penetration = true; distance = gjk.shapeDistance(ba, tfa, bb, tfb, compute_penetration, p1, p2, normal); return (distance > gjk.getDistancePrecision(compute_penetration)); } -inline CoalScalar _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf) { - Vec3f AABB_corner; +inline CoalScalar _computeDistanceForCase1(const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf) { + Vec3s AABB_corner; /* This seems to be slower AABB_corner.noalias() = T.cwiseAbs () - a; AABB_corner.noalias() -= PRODUCT(Bf,b); @@ -135,11 +135,11 @@ inline CoalScalar _computeDistanceForCase1(const Vec3f& T, const Vec3f& a, return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); } -inline CoalScalar _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf) { +inline CoalScalar _computeDistanceForCase2(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, + const Matrix3s& Bf) { /* - Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); + Vec3s AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a); return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm (); /*/ @@ -153,18 +153,18 @@ inline CoalScalar _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T, if (s > 0) t += s * s; return t; #else - Vec3f AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); + Vec3s AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm(); #endif // */ } -int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const CoalScalar& breakDistance2, +int separatingAxisId(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { int id = 0; - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -216,10 +216,10 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } // ------------------------ 0 -------------------------------------- -bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const CoalScalar& breakDistance2, +bool withRuntimeLoop(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -268,30 +268,30 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } // ------------------------ 1 -------------------------------------- -bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +bool withManualLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { CoalScalar t, s; CoalScalar diff; - // Matrix3f Bf = abs(B); + // Matrix3s Bf = abs(B); // Bf += reps; - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin - Vec3f AABB_corner(T.cwiseAbs() - Bf * b); - Vec3f diff3(AABB_corner - a); - diff3 = diff3.cwiseMax(Vec3f::Zero()); + Vec3s AABB_corner(T.cwiseAbs() - Bf * b); + Vec3s diff3(AABB_corner - a); + diff3 = diff3.cwiseMax(Vec3s::Zero()); - // for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); + // for (Vec3s::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); squaredLowerBoundDistance = diff3.squaredNorm(); if (squaredLowerBoundDistance > breakDistance2) return true; AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a; // diff3 = | B^T T| - b - Bf^T a diff3 = AABB_corner - b; - diff3 = diff3.cwiseMax(Vec3f::Zero()); + diff3 = diff3.cwiseMax(Vec3s::Zero()); squaredLowerBoundDistance = diff3.squaredNorm(); if (squaredLowerBoundDistance > breakDistance2) return true; @@ -458,11 +458,11 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, } // ------------------------ 2 -------------------------------------- -bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +bool withManualLoopUnrolling_2(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -638,8 +638,8 @@ bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T, template struct loop_case_1 { - static inline bool run(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const Matrix3f& Bf, + static inline bool run(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); @@ -669,11 +669,11 @@ struct loop_case_1 { } }; -bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +bool withTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -718,9 +718,9 @@ bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, template struct loop_case_2 { - static inline bool run(int ia, int ja, int ka, const Matrix3f& B, - const Vec3f& T, const Vec3f& a, const Vec3f& b, - const Matrix3f& Bf, const CoalScalar& breakDistance2, + static inline bool run(int ia, int ja, int ka, const Matrix3s& B, + const Vec3s& T, const Vec3s& a, const Vec3s& b, + const Matrix3s& Bf, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); @@ -749,11 +749,11 @@ struct loop_case_2 { } }; -bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +bool withPartialTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); @@ -782,13 +782,13 @@ bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T, } // ------------------------ 5 -------------------------------------- -bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const CoalScalar& breakDistance2, +bool originalWithLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar& breakDistance2, CoalScalar& squaredLowerBoundDistance) { CoalScalar t, s; CoalScalar diff; - Matrix3f Bf(B.cwiseAbs()); + Matrix3s Bf(B.cwiseAbs()); squaredLowerBoundDistance = 0; // if any of these tests are one-sided, then the polyhedra are disjoint @@ -1003,15 +1003,15 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, } // ------------------------ 6 -------------------------------------- -bool originalWithNoLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a, - const Vec3f& b, const CoalScalar&, +bool originalWithNoLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const CoalScalar&, CoalScalar& squaredLowerBoundDistance) { CoalScalar t, s; const CoalScalar reps = 1e-6; squaredLowerBoundDistance = 0; - Matrix3f Bf(B.array().abs() + reps); + Matrix3s Bf(B.array().abs() + reps); // Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint @@ -1173,8 +1173,8 @@ std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) { return br.print(os); } -BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, +BenchmarkResult benchmark_obb_case(const Matrix3s& B, const Vec3s& T, + const Vec3s& a, const Vec3s& b, const CollisionRequest& request, std::size_t N) { const CoalScalar breakDistance(request.break_distance + @@ -1283,9 +1283,9 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) { std::size_t nbFailure = 0; // Create two OBBs axis - Vec3f a, b; - Matrix3f B; - Vec3f T; + Vec3s a, b; + Matrix3s B; + Vec3s T; CollisionRequest request; #ifndef NDEBUG // if debug mode diff --git a/test/octree.cpp b/test/octree.cpp index 894c28031..9cedd2e6c 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -55,7 +55,7 @@ namespace utf = boost::unit_test::framework; using namespace coal; -void makeMesh(const std::vector& vertices, +void makeMesh(const std::vector& vertices, const std::vector& triangles, BVHModel& model) { coal::SplitMethodType split_method(coal::SPLIT_METHOD_MEAN); model.bv_splitter.reset(new BVSplitter(split_method)); @@ -68,8 +68,8 @@ void makeMesh(const std::vector& vertices, coal::OcTree makeOctree(const BVHModel& mesh, const CoalScalar& resolution) { - Vec3f m(mesh.aabb_local.min_); - Vec3f M(mesh.aabb_local.max_); + Vec3s m(mesh.aabb_local.min_); + Vec3s M(mesh.aabb_local.max_); coal::Box box(resolution, resolution, resolution); CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); CollisionResult result; @@ -82,7 +82,7 @@ coal::OcTree makeOctree(const BVHModel& mesh, y += resolution) { for (CoalScalar z = resolution * floor(m[2] / resolution); z <= M[2]; z += resolution) { - Vec3f center(x + .5 * resolution, y + .5 * resolution, + Vec3s center(x + .5 * resolution, y + .5 * resolution, z + .5 * resolution); tfBox.setTranslation(center); coal::collide(&box, tfBox, &mesh, Transform3f(), request, result); @@ -105,7 +105,7 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); CoalScalar resolution(10.); - std::vector pRob, pEnv; + std::vector pRob, pEnv; std::vector tRob, tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob); @@ -179,7 +179,7 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ", "", "", "(", ")"); CoalScalar resolution(10.); - std::vector pEnv; + std::vector pEnv; std::vector tEnv; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv); @@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { const CoalScalar x_dim = 10, y_dim = 20; const int nx = 100, ny = 100; const CoalScalar max_altitude = 1., min_altitude = 0.; - const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); HeightField hfield(x_dim, y_dim, heights, min_altitude); hfield.computeLocalAABB(); diff --git a/test/profiling.cpp b/test/profiling.cpp index bdc5eb152..5319e422c 100644 --- a/test/profiling.cpp +++ b/test/profiling.cpp @@ -44,7 +44,7 @@ bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) { template CollisionGeometryPtr_t objToGeom(const std::string& filename) { - std::vector pt; + std::vector pt; std::vector tri; loadOBJFile(filename.c_str(), pt, tri); @@ -60,7 +60,7 @@ CollisionGeometryPtr_t objToGeom(const std::string& filename) { template CollisionGeometryPtr_t meshToGeom(const std::string& filename, - const Vec3f& scale = Vec3f(1, 1, 1)) { + const Vec3s& scale = Vec3s(1, 1, 1)) { shared_ptr > model(new BVHModel()); loadPolyhedronFromResource(filename, scale, model); return model; @@ -190,9 +190,9 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { iarg += 3; if (iarg < argc && strcmp(argv[iarg], "crop") == 0) { CHECK_PARAM_NB(6, Crop); - coal::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]), + coal::AABB aabb(Vec3s(atof(argv[iarg + 1]), atof(argv[iarg + 2]), atof(argv[iarg + 3])), - Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]), + Vec3s(atof(argv[iarg + 4]), atof(argv[iarg + 5]), atof(argv[iarg + 6]))); OUT("Cropping " << aabb.min_.transpose() << " ---- " << aabb.max_.transpose() << " ..."); @@ -249,11 +249,11 @@ int main(int argc, char** argv) { geoms.push_back(Geometry("Cone", new Cone(2, 1))); geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1))); // geoms.push_back(Geometry ("Plane" , new Plane - // (Vec3f(1,1,1).normalized(), 0) )); + // (Vec3s(1,1,1).normalized(), 0) )); // geoms.push_back(Geometry ("Halfspace" , new Halfspace - // (Vec3f(1,1,1).normalized(), 0) )); + // (Vec3s(1,1,1).normalized(), 0) )); // not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP - // (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0)) )); + // (Vec3s(0,1,0), Vec3s(0,0,1), Vec3s(-1,0,0)) )); geoms.push_back(Geometry("rob BVHModel", objToGeom((path / "rob.obj").string()))); diff --git a/test/python_unit/collision.py b/test/python_unit/collision.py index 61cdcd243..fbf543bce 100644 --- a/test/python_unit/collision.py +++ b/test/python_unit/collision.py @@ -6,7 +6,7 @@ def tetahedron(): - pts = coal.StdVec_Vec3f() + pts = coal.StdVec_Vec3s() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py index 57e03e21c..c7774e316 100644 --- a/test/python_unit/geometric_shapes.py +++ b/test/python_unit/geometric_shapes.py @@ -160,7 +160,7 @@ def test_BVH(self): self.assertEqual(bvh.vertices().shape, (0, 3)) def test_convex(self): - verts = coal.StdVec_Vec3f() + verts = coal.StdVec_Vec3s() faces = coal.StdVec_Triangle() verts.extend( [ diff --git a/test/python_unit/pickling.py b/test/python_unit/pickling.py index 8a94bde07..c6143d879 100644 --- a/test/python_unit/pickling.py +++ b/test/python_unit/pickling.py @@ -7,7 +7,7 @@ def tetahedron(): - pts = coal.StdVec_Vec3f() + pts = coal.StdVec_Vec3s() pts.append(np.array((0, 0, 0))) pts.append(np.array((0, 1, 0))) pts.append(np.array((1, 0, 0))) diff --git a/test/security_margin.cpp b/test/security_margin.cpp index 1f05503f2..c56dba7c1 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -55,7 +55,7 @@ using coal::CollisionResult; using coal::DistanceRequest; using coal::DistanceResult; using coal::Transform3f; -using coal::Vec3f; +using coal::Vec3s; #define MATH_SQUARED(x) (x * x) @@ -64,7 +64,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 1, 1)); + const Transform3f tf2_collision(Vec3s(0, 1, 1)); coal::Box s1(1, 1, 1); coal::Box s2(1, 1, 1); const double tol = 1e-8; @@ -90,7 +90,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); CoalScalar sqrDistLowerBound; @@ -106,7 +106,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { const double distance = 0.01; collisionRequest.security_margin = distance; Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); CoalScalar sqrDistLowerBound; @@ -122,7 +122,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { const double distance = -0.01; collisionRequest.security_margin = distance; const Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, distance))); AABB bv2_transformed; computeBV(s2, tf2, bv2_transformed); CoalScalar sqrDistLowerBound; @@ -154,7 +154,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 0)); + const Transform3f tf2_collision(Vec3s(0, 0, 0)); coal::Box s1(1, 1, 1); coal::Box s2(1, 1, 1); @@ -184,7 +184,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionGeometryPtr_t s2(new coal::Sphere(2)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 3)); + const Transform3f tf2_collision(Vec3s(0, 0, 3)); // NOTE: when comparing a result to zero, **do not use BOOST_CHECK_CLOSE**! // Zero is not close to any other number, so the test will always fail. @@ -207,7 +207,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionResult collisionResult; const double distance = 0.01; Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); @@ -221,7 +221,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { const double distance = 0.01; collisionRequest.security_margin = distance; Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); @@ -236,7 +236,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { const double distance = -0.01; collisionRequest.security_margin = distance; Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); @@ -262,7 +262,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionGeometryPtr_t c2(new coal::Capsule(0.5, 1.)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 1., 0)); + const Transform3f tf2_collision(Vec3s(0, 1., 0)); // No security margin - collision { @@ -281,7 +281,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionResult collisionResult; const double distance = 0.01; Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); @@ -295,7 +295,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { const double distance = 0.01; collisionRequest.security_margin = distance; Transform3f tf2_no_collision( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); @@ -311,7 +311,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { const double distance = -0.01; collisionRequest.security_margin = distance; Transform3f tf2( - Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0))); + Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8); @@ -336,7 +336,7 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 1, 1)); + const Transform3f tf2_collision(Vec3s(0, 1, 1)); const double tol = 1e-3; @@ -356,7 +356,7 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; const Transform3f tf2_no_collision( - (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); + (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; collide(b1.get(), tf1, b2.get(), tf2_no_collision, collisionRequest, @@ -399,7 +399,7 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, collisionRequest.security_margin, + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); @@ -430,7 +430,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; const Transform3f tf2_no_collision( - (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); + (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; collide(&shape1, tf1, &shape2, tf2_no_collision, collisionRequest, @@ -476,7 +476,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, collisionRequest.security_margin, + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); @@ -497,7 +497,7 @@ BOOST_AUTO_TEST_CASE(sphere_box) { CollisionGeometryPtr_t s2(new coal::Sphere(0.5)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 1)); + const Transform3f tf2_collision(Vec3s(0, 0, 1)); const double tol = 1e-6; diff --git a/test/serialization.cpp b/test/serialization.cpp index 3d0769f8c..c1dc66fbd 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -250,12 +250,12 @@ void test_serialization(const T& value, } BOOST_AUTO_TEST_CASE(test_aabb) { - AABB aabb(-Vec3f::Ones(), Vec3f::Ones()); + AABB aabb(-Vec3s::Ones(), Vec3s::Ones()); test_serialization(aabb); } BOOST_AUTO_TEST_CASE(test_collision_data) { - Contact contact(NULL, NULL, 1, 2, Vec3f::Ones(), Vec3f::Zero(), -10.); + Contact contact(NULL, NULL, 1, 2, Vec3s::Ones(), Vec3s::Zero(), -10.); test_serialization(contact); CollisionRequest collision_request(CONTACT, 10); @@ -290,7 +290,7 @@ BOOST_AUTO_TEST_CASE(test_collision_data) { Transform3f tf2; // set translation to have a collision const CoalScalar offset = 0.001; - tf2.setTranslation(Vec3f(0, 0, height / 2 - offset)); + tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -326,7 +326,7 @@ void checkEqualStdVector(const std::vector& v1, const std::vector& v2) { } BOOST_AUTO_TEST_CASE(test_BVHModel) { - std::vector p1, p2; + std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -367,7 +367,7 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) { #ifdef COAL_HAS_QHULL BOOST_AUTO_TEST_CASE(test_Convex) { - std::vector p1; + std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); @@ -420,7 +420,7 @@ BOOST_AUTO_TEST_CASE(test_HeightField) { const CoalScalar min_altitude = -1.; const CoalScalar x_dim = 1., y_dim = 2.; const Eigen::DenseIndex nx = 100, ny = 200; - const MatrixXf heights = MatrixXf::Random(ny, nx); + const MatrixXs heights = MatrixXs::Random(ny, nx); HeightField hfield(x_dim, y_dim, heights, min_altitude); @@ -438,7 +438,7 @@ BOOST_AUTO_TEST_CASE(test_HeightField) { BOOST_AUTO_TEST_CASE(test_transform) { Transform3f T; T.setQuatRotation(Quaternion3f::UnitRandom()); - T.setTranslation(Vec3f::Random()); + T.setTranslation(Vec3s::Random()); Transform3f T_copy; test_serialization(T, T_copy); @@ -446,15 +446,15 @@ BOOST_AUTO_TEST_CASE(test_transform) { BOOST_AUTO_TEST_CASE(test_shapes) { { - TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), Vec3f::UnitZ()); + TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), Vec3s::UnitZ()); triangle.setSweptSphereRadius(1.); triangle.computeLocalAABB(); - TriangleP triangle_copy(Vec3f::Random(), Vec3f::Random(), Vec3f::Random()); + TriangleP triangle_copy(Vec3s::Random(), Vec3s::Random(), Vec3s::Random()); test_serialization(triangle, triangle_copy); } { - Box box(Vec3f::UnitX()), box_copy(Vec3f::Random()); + Box box(Vec3s::UnitX()), box_copy(Vec3s::Random()); box.setSweptSphereRadius(1.); box.computeLocalAABB(); test_serialization(box, box_copy); @@ -496,14 +496,14 @@ BOOST_AUTO_TEST_CASE(test_shapes) { } { - Halfspace hs(Vec3f::Random(), 1.), hs_copy(Vec3f::Zero(), 0.); + Halfspace hs(Vec3s::Random(), 1.), hs_copy(Vec3s::Zero(), 0.); hs.setSweptSphereRadius(1.); hs.computeLocalAABB(); test_serialization(hs, hs_copy); } { - Plane plane(Vec3f::Random(), 1.), plane_copy(Vec3f::Zero(), 0.); + Plane plane(Vec3s::Random(), 1.), plane_copy(Vec3s::Zero(), 0.); plane.setSweptSphereRadius(1.); plane.computeLocalAABB(); test_serialization(plane, plane_copy); @@ -511,11 +511,11 @@ BOOST_AUTO_TEST_CASE(test_shapes) { { const size_t num_points = 500; - std::shared_ptr> points = - std::make_shared>(); + std::shared_ptr> points = + std::make_shared>(); points->reserve(num_points); for (size_t i = 0; i < num_points; i++) { - points->emplace_back(Vec3f::Random()); + points->emplace_back(Vec3s::Random()); } using Convex = Convex; std::unique_ptr convex = @@ -532,7 +532,7 @@ BOOST_AUTO_TEST_CASE(test_shapes) { #ifdef COAL_HAS_OCTOMAP BOOST_AUTO_TEST_CASE(test_octree) { const CoalScalar resolution = 1e-2; - const Matrixx3f points = Matrixx3f::Random(1000, 3); + const MatrixX3s points = MatrixX3s::Random(1000, 3); OcTreePtr_t octree_ptr = makeOctree(points, resolution); const OcTree& octree = *octree_ptr.get(); @@ -572,7 +572,7 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) { Sphere sphere(1.); BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere)); - std::vector p1; + std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index 3a32cb6ce..a571c380a 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -54,7 +54,7 @@ using coal::CollisionResult; using coal::DistanceRequest; using coal::DistanceResult; using coal::Transform3f; -using coal::Vec3f; +using coal::Vec3s; #define MATH_SQUARED(x) (x * x) @@ -190,10 +190,10 @@ BOOST_AUTO_TEST_CASE(test_inflate) { test(cone, 0.01, 1e-8); test_throw(cone, -1.1); - const coal::Halfspace halfspace(Vec3f::UnitZ(), 0.); + const coal::Halfspace halfspace(Vec3s::UnitZ(), 0.); test(halfspace, 0.01, 1e-8); - // const coal::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), - // Vec3f::UnitZ()); + // const coal::TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), + // Vec3s::UnitZ()); // test(triangle, 0.01, 1e-8); } diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index 9dfd5db84..d234602ef 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -64,7 +64,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { Transform3f pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); @@ -108,7 +108,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { 0.05); } - pose.setTranslation(Vec3f(40.1, 0, 0)); + pose.setTranslation(Vec3s(40.1, 0, 0)); res.clear(), res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); @@ -167,7 +167,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { Transform3f pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); @@ -211,7 +211,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { 0.01); } - pose.setTranslation(Vec3f(15.1, 0, 0)); + pose.setTranslation(Vec3s(15.1, 0, 0)); res.clear(); res1.clear(); @@ -271,7 +271,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { Transform3f pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); @@ -316,7 +316,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { } pose.setTranslation( - Vec3f(15, 0, 0)); // libccd cannot use small value here :( + Vec3s(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); @@ -376,7 +376,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { Transform3f pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); @@ -420,7 +420,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { } pose.setTranslation( - Vec3f(15, 0, 0)); // libccd cannot use small value here :( + Vec3s(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); @@ -479,7 +479,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { Transform3f pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); @@ -523,7 +523,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { 0.05); } - pose.setTranslation(Vec3f(40.1, 0, 0)); + pose.setTranslation(Vec3s(40.1, 0, 0)); res.clear(); res1.clear(); @@ -583,7 +583,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { Transform3f pose; - pose.setTranslation(Vec3f(50, 0, 0)); + pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); @@ -627,7 +627,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { 0.01); } - pose.setTranslation(Vec3f(15.1, 0, 0)); + pose.setTranslation(Vec3s(15.1, 0, 0)); res.clear(); res1.clear(); @@ -687,7 +687,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { Transform3f pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); @@ -743,7 +743,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } - pose.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); res.clear(); res1.clear(); @@ -803,7 +803,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { Transform3f pose; - pose.setTranslation(Vec3f(20, 0, 0)); + pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); @@ -847,7 +847,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { 0.05); } - pose.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); res.clear(); res1.clear(); @@ -945,9 +945,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(40, 0, 0)); - pose_aabb.setTranslation(Vec3f(40, 0, 0)); - pose_obb.setTranslation(Vec3f(40, 0, 0)); + pose.setTranslation(Vec3s(40, 0, 0)); + pose_aabb.setTranslation(Vec3s(40, 0, 0)); + pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -977,9 +977,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(30, 0, 0)); - pose_aabb.setTranslation(Vec3f(30, 0, 0)); - pose_obb.setTranslation(Vec3f(30, 0, 0)); + pose.setTranslation(Vec3s(30, 0, 0)); + pose_aabb.setTranslation(Vec3s(30, 0, 0)); + pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1009,11 +1009,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(29.9, 0, 0)); + pose.setTranslation(Vec3s(29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1043,11 +1043,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-29.9, 0, 0)); + pose.setTranslation(Vec3s(-29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1077,9 +1077,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-30, 0, 0)); - pose_aabb.setTranslation(Vec3f(-30, 0, 0)); - pose_obb.setTranslation(Vec3f(-30, 0, 0)); + pose.setTranslation(Vec3s(-30, 0, 0)); + pose_aabb.setTranslation(Vec3s(-30, 0, 0)); + pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1164,9 +1164,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(15.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(15.01, 0, 0)); - pose_obb.setTranslation(Vec3f(15.01, 0, 0)); + pose.setTranslation(Vec3s(15.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(15.01, 0, 0)); + pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1196,9 +1196,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(14.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(14.99, 0, 0)); - pose_obb.setTranslation(Vec3f(14.99, 0, 0)); + pose.setTranslation(Vec3s(14.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(14.99, 0, 0)); + pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1283,9 +1283,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(22.4, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.4, 0, 0)); - pose_obb.setTranslation(Vec3f(22.4, 0, 0)); + pose.setTranslation(Vec3s(22.4, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.4, 0, 0)); + pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1315,9 +1315,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(22.51, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.51, 0, 0)); - pose_obb.setTranslation(Vec3f(22.51, 0, 0)); + pose.setTranslation(Vec3s(22.51, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.51, 0, 0)); + pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1369,9 +1369,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) { Transform3f pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.99, 0, 0)); - pose_obb.setTranslation(Vec3f(9.99, 0, 0)); + pose.setTranslation(Vec3s(9.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); + pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1401,9 +1401,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.01, 0, 0)); - pose_obb.setTranslation(Vec3f(10.01, 0, 0)); + pose.setTranslation(Vec3s(10.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.01, 0, 0)); + pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1455,9 +1455,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { Transform3f pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.9, 0, 0)); - pose_obb.setTranslation(Vec3f(9.9, 0, 0)); + pose.setTranslation(Vec3s(9.9, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); + pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1487,9 +1487,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.1, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.1, 0, 0)); - pose_obb.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.1, 0, 0)); + pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1519,9 +1519,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(0, 0, 9.9)); - pose_aabb.setTranslation(Vec3f(0, 0, 9.9)); - pose_obb.setTranslation(Vec3f(0, 0, 9.9)); + pose.setTranslation(Vec3s(0, 0, 9.9)); + pose_aabb.setTranslation(Vec3s(0, 0, 9.9)); + pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1551,9 +1551,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(0, 0, 10.1)); - pose_aabb.setTranslation(Vec3f(0, 0, 10.1)); - pose_obb.setTranslation(Vec3f(0, 0, 10.1)); + pose.setTranslation(Vec3s(0, 0, 10.1)); + pose_aabb.setTranslation(Vec3s(0, 0, 10.1)); + pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1638,9 +1638,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(40, 0, 0)); - pose_aabb.setTranslation(Vec3f(40, 0, 0)); - pose_obb.setTranslation(Vec3f(40, 0, 0)); + pose.setTranslation(Vec3s(40, 0, 0)); + pose_aabb.setTranslation(Vec3s(40, 0, 0)); + pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1670,9 +1670,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(30, 0, 0)); - pose_aabb.setTranslation(Vec3f(30, 0, 0)); - pose_obb.setTranslation(Vec3f(30, 0, 0)); + pose.setTranslation(Vec3s(30, 0, 0)); + pose_aabb.setTranslation(Vec3s(30, 0, 0)); + pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1702,11 +1702,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(29.9, 0, 0)); + pose.setTranslation(Vec3s(29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1736,11 +1736,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-29.9, 0, 0)); + pose.setTranslation(Vec3s(-29.9, 0, 0)); pose_aabb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation( - Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision + Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1770,9 +1770,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(-30, 0, 0)); - pose_aabb.setTranslation(Vec3f(-30, 0, 0)); - pose_obb.setTranslation(Vec3f(-30, 0, 0)); + pose.setTranslation(Vec3s(-30, 0, 0)); + pose_aabb.setTranslation(Vec3s(-30, 0, 0)); + pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1858,9 +1858,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(15.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(15.01, 0, 0)); - pose_obb.setTranslation(Vec3f(15.01, 0, 0)); + pose.setTranslation(Vec3s(15.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(15.01, 0, 0)); + pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1890,9 +1890,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(14.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(14.99, 0, 0)); - pose_obb.setTranslation(Vec3f(14.99, 0, 0)); + pose.setTranslation(Vec3s(14.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(14.99, 0, 0)); + pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -1978,9 +1978,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(22.4, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.4, 0, 0)); - pose_obb.setTranslation(Vec3f(22.4, 0, 0)); + pose.setTranslation(Vec3s(22.4, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.4, 0, 0)); + pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -2010,9 +2010,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(22.51, 0, 0)); - pose_aabb.setTranslation(Vec3f(22.51, 0, 0)); - pose_obb.setTranslation(Vec3f(22.51, 0, 0)); + pose.setTranslation(Vec3s(22.51, 0, 0)); + pose_aabb.setTranslation(Vec3s(22.51, 0, 0)); + pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -2065,9 +2065,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { Transform3f pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.99, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.99, 0, 0)); - pose_obb.setTranslation(Vec3f(9.99, 0, 0)); + pose.setTranslation(Vec3s(9.99, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); + pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -2097,9 +2097,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.01, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.01, 0, 0)); - pose_obb.setTranslation(Vec3f(10.01, 0, 0)); + pose.setTranslation(Vec3s(10.01, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.01, 0, 0)); + pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -2152,9 +2152,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { Transform3f pose, pose_aabb, pose_obb; - pose.setTranslation(Vec3f(9.9, 0, 0)); - pose_aabb.setTranslation(Vec3f(9.9, 0, 0)); - pose_obb.setTranslation(Vec3f(9.9, 0, 0)); + pose.setTranslation(Vec3s(9.9, 0, 0)); + pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); + pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -2184,9 +2184,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(10.1, 0, 0)); - pose_aabb.setTranslation(Vec3f(10.1, 0, 0)); - pose_obb.setTranslation(Vec3f(10.1, 0, 0)); + pose.setTranslation(Vec3s(10.1, 0, 0)); + pose_aabb.setTranslation(Vec3s(10.1, 0, 0)); + pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -2216,9 +2216,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); - pose.setTranslation(Vec3f(0, 0, 9.9)); - pose_aabb.setTranslation(Vec3f(0, 0, 9.9)); - pose_obb.setTranslation(Vec3f(0, 0, 9.9)); + pose.setTranslation(Vec3s(0, 0, 9.9)); + pose_aabb.setTranslation(Vec3s(0, 0, 9.9)); + pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); @@ -2248,9 +2248,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); - pose.setTranslation(Vec3f(0, 0, 10.1)); - pose_aabb.setTranslation(Vec3f(0, 0, 10.1)); - pose_obb.setTranslation(Vec3f(0, 0, 10.1)); + pose.setTranslation(Vec3s(0, 0, 10.1)); + pose_aabb.setTranslation(Vec3s(0, 0, 10.1)); + pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); diff --git a/test/simple.cpp b/test/simple.cpp index cbeceab0b..fd1f7a399 100644 --- a/test/simple.cpp +++ b/test/simple.cpp @@ -16,24 +16,24 @@ static bool approx(CoalScalar x, CoalScalar y) { } BOOST_AUTO_TEST_CASE(projection_test_line) { - Vec3f v1(0, 0, 0); - Vec3f v2(2, 0, 0); + Vec3s v1(0, 0, 0); + Vec3s v2(2, 0, 0); - Vec3f p(1, 0, 0); + Vec3s p(1, 0, 0); Project::ProjectResult res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); - p = Vec3f(-1, 0, 0); + p = Vec3s(-1, 0, 0); res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 1)); BOOST_CHECK(approx(res.parameterization[0], 1)); BOOST_CHECK(approx(res.parameterization[1], 0)); - p = Vec3f(3, 0, 0); + p = Vec3s(3, 0, 0); res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 1)); @@ -42,11 +42,11 @@ BOOST_AUTO_TEST_CASE(projection_test_line) { } BOOST_AUTO_TEST_CASE(projection_test_triangle) { - Vec3f v1(0, 0, 1); - Vec3f v2(0, 1, 0); - Vec3f v3(1, 0, 0); + Vec3s v1(0, 0, 1); + Vec3s v2(0, 1, 0); + Vec3s v3(1, 0, 0); - Vec3f p(1, 1, 1); + Vec3s p(1, 1, 1); Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 7); BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0)); @@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) { BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); - p = Vec3f(0, 0, 1.5); + p = Vec3s(0, 0, 1.5); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -62,7 +62,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) { BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); - p = Vec3f(1.5, 0, 0); + p = Vec3s(1.5, 0, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 4); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -70,7 +70,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) { BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 1)); - p = Vec3f(0, 1.5, 0); + p = Vec3s(0, 1.5, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) { BOOST_CHECK(approx(res.parameterization[1], 1)); BOOST_CHECK(approx(res.parameterization[2], 0)); - p = Vec3f(1, 1, 0); + p = Vec3s(1, 1, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 6); BOOST_CHECK(approx(res.sqr_distance, 0.5)); @@ -86,7 +86,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) { BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); - p = Vec3f(1, 0, 1); + p = Vec3s(1, 0, 1); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 5); BOOST_CHECK(approx(res.sqr_distance, 0.5)); @@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) { BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); - p = Vec3f(0, 1, 1); + p = Vec3s(0, 1, 1); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0.5)); @@ -104,12 +104,12 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) { } BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { - Vec3f v1(0, 0, 1); - Vec3f v2(0, 1, 0); - Vec3f v3(1, 0, 0); - Vec3f v4(1, 1, 1); + Vec3s v1(0, 0, 1); + Vec3s v2(0, 1, 0); + Vec3s v3(1, 0, 0); + Vec3s v4(1, 1, 1); - Vec3f p(0.5, 0.5, 0.5); + Vec3s p(0.5, 0.5, 0.5); Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 15); BOOST_CHECK(approx(res.sqr_distance, 0)); @@ -118,7 +118,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0.25)); BOOST_CHECK(approx(res.parameterization[3], 0.25)); - p = Vec3f(0, 0, 0); + p = Vec3s(0, 0, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 7); BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); @@ -127,7 +127,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[3], 0)); - p = Vec3f(0, 1, 1); + p = Vec3s(0, 1, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 11); BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); @@ -136,7 +136,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - p = Vec3f(1, 1, 0); + p = Vec3s(1, 1, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 14); BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); @@ -145,7 +145,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - p = Vec3f(1, 0, 1); + p = Vec3s(1, 0, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 13); BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0)); @@ -154,7 +154,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0)); BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0)); - p = Vec3f(1.5, 1.5, 1.5); + p = Vec3s(1.5, 1.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 8); BOOST_CHECK(approx(res.sqr_distance, 0.75)); @@ -163,7 +163,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 1)); - p = Vec3f(1.5, -0.5, -0.5); + p = Vec3s(1.5, -0.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 4); BOOST_CHECK(approx(res.sqr_distance, 0.75)); @@ -172,7 +172,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 1)); BOOST_CHECK(approx(res.parameterization[3], 0)); - p = Vec3f(-0.5, -0.5, 1.5); + p = Vec3s(-0.5, -0.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 0.75)); @@ -181,7 +181,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); - p = Vec3f(-0.5, 1.5, -0.5); + p = Vec3s(-0.5, 1.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 0.75)); @@ -190,7 +190,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); - p = Vec3f(0.5, -0.5, 0.5); + p = Vec3s(0.5, -0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 5); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0)); - p = Vec3f(0.5, 1.5, 0.5); + p = Vec3s(0.5, 1.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 10); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -208,7 +208,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); - p = Vec3f(1.5, 0.5, 0.5); + p = Vec3s(1.5, 0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 12); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); - p = Vec3f(-0.5, 0.5, 0.5); + p = Vec3s(-0.5, 0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -226,7 +226,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); - p = Vec3f(0.5, 0.5, 1.5); + p = Vec3s(0.5, 0.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 9); BOOST_CHECK(approx(res.sqr_distance, 0.25)); @@ -235,7 +235,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); - p = Vec3f(0.5, 0.5, -0.5); + p = Vec3s(0.5, 0.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 6); BOOST_CHECK(approx(res.sqr_distance, 0.25)); diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index a89c89374..b1a687a91 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -115,8 +115,8 @@ struct SweptSphereGJKSolver : public GJKSolver { template CoalScalar shapeDistance( const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, bool compute_penetration, Vec3f& p1, Vec3f& p2, - Vec3f& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const { + const Transform3f& tf2, bool compute_penetration, Vec3s& p1, Vec3s& p2, + Vec3s& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const { if (use_swept_sphere_radius_in_gjk_epa_iterations) { CoalScalar distance; this->runGJKAndEPA( @@ -163,9 +163,9 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { SET_LINE; std::array distance; - std::array p1; - std::array p2; - std::array normal; + std::array p1; + std::array p2; + std::array normal; // Default hppfcl behavior - Don't take swept sphere radius into account // during GJK/EPA iterations. Correct the solution afterwards. diff --git a/test/utility.cpp b/test/utility.cpp index bddb9b4da..80efceba4 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -85,16 +85,16 @@ const Eigen::IOFormat vfmt = Eigen::IOFormat( const Eigen::IOFormat pyfmt = Eigen::IOFormat( Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); -const Vec3f UnitX = Vec3f(1, 0, 0); -const Vec3f UnitY = Vec3f(0, 1, 0); -const Vec3f UnitZ = Vec3f(0, 0, 1); +const Vec3s UnitX = Vec3s(1, 0, 0); +const Vec3s UnitY = Vec3s(0, 1, 0); +const Vec3s UnitZ = Vec3s(0, 0, 1); CoalScalar rand_interval(CoalScalar rmin, CoalScalar rmax) { CoalScalar t = rand() / ((CoalScalar)RAND_MAX + 1); return (t * (rmax - rmin) + rmin); } -void loadOBJFile(const char* filename, std::vector& points, +void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles) { FILE* file = fopen(filename, "rb"); if (!file) { @@ -124,7 +124,7 @@ void loadOBJFile(const char* filename, std::vector& points, CoalScalar x = (CoalScalar)atof(strtok(NULL, "\t ")); CoalScalar y = (CoalScalar)atof(strtok(NULL, "\t ")); CoalScalar z = (CoalScalar)atof(strtok(NULL, "\t ")); - Vec3f p(x, y, z); + Vec3s p(x, y, z); points.push_back(p); } } break; @@ -160,7 +160,7 @@ void loadOBJFile(const char* filename, std::vector& points, } } -void saveOBJFile(const char* filename, std::vector& points, +void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles) { std::ofstream os(filename); if (!os) { @@ -195,7 +195,7 @@ OcTree loadOctreeFile(const std::string& filename, } #endif -void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3f& R) { +void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3s& R) { CoalScalar c1 = cos(a); CoalScalar c2 = cos(b); CoalScalar c3 = cos(c); @@ -217,9 +217,9 @@ void generateRandomTransform(CoalScalar extents[6], Transform3f& transform) { CoalScalar b = rand_interval(0, 2 * pi); CoalScalar c = rand_interval(0, 2 * pi); - Matrix3f R; + Matrix3s R; eulerToMatrix(a, b, c, R); - Vec3f T(x, y, z); + Vec3s T(x, y, z); transform.setTransform(R, T); } @@ -238,9 +238,9 @@ void generateRandomTransforms(CoalScalar extents[6], CoalScalar c = rand_interval(0, 2 * pi); { - Matrix3f R; + Matrix3s R; eulerToMatrix(a, b, c, R); - Vec3f T(x, y, z); + Vec3s T(x, y, z); transforms[i].setTransform(R, T); } } @@ -264,9 +264,9 @@ void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], CoalScalar c = rand_interval(0, 2 * pi); { - Matrix3f R; + Matrix3s R; eulerToMatrix(a, b, c, R); - Vec3f T(x, y, z); + Vec3s T(x, y, z); transforms[i].setTransform(R, T); } @@ -279,9 +279,9 @@ void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], CoalScalar deltac = rand_interval(-delta_rot, delta_rot); { - Matrix3f R; + Matrix3s R; eulerToMatrix(a + deltaa, b + deltab, c + deltac, R); - Vec3f T(x + deltax, y + deltay, z + deltaz); + Vec3s T(x + deltax, y + deltay, z + deltaz); transforms2[i].setTransform(R, T); } } @@ -458,10 +458,10 @@ void generateEnvironmentsMesh(std::vector& env, } Convex buildBox(CoalScalar l, CoalScalar w, CoalScalar d) { - std::shared_ptr> pts(new std::vector( - {Vec3f(l, w, d), Vec3f(l, w, -d), Vec3f(l, -w, d), Vec3f(l, -w, -d), - Vec3f(-l, w, d), Vec3f(-l, w, -d), Vec3f(-l, -w, d), - Vec3f(-l, -w, -d)})); + std::shared_ptr> pts(new std::vector( + {Vec3s(l, w, d), Vec3s(l, w, -d), Vec3s(l, -w, d), Vec3s(l, -w, -d), + Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d), + Vec3s(-l, -w, -d)})); std::shared_ptr> polygons( new std::vector(6)); @@ -480,7 +480,7 @@ Convex buildBox(CoalScalar l, CoalScalar w, CoalScalar d) { } /// Takes a point and projects it onto the surface of the unit sphere -void toSphere(Vec3f& point) { +void toSphere(Vec3s& point) { assert(point.norm() > 1e-8); point /= point.norm(); } @@ -491,7 +491,7 @@ void toSphere(Vec3f& point) { /// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y * /// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) = /// diag(r). -void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) { +void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) { toSphere(point); point[0] *= ellipsoid.radii[0]; point[1] *= ellipsoid.radii[1]; @@ -502,24 +502,24 @@ Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) { CoalScalar PHI = (1 + std::sqrt(5)) / 2; // vertices - std::shared_ptr> pts(new std::vector({ - Vec3f(-1, PHI, 0), - Vec3f(1, PHI, 0), - Vec3f(-1, -PHI, 0), - Vec3f(1, -PHI, 0), - - Vec3f(0, -1, PHI), - Vec3f(0, 1, PHI), - Vec3f(0, -1, -PHI), - Vec3f(0, 1, -PHI), - - Vec3f(PHI, 0, -1), - Vec3f(PHI, 0, 1), - Vec3f(-PHI, 0, -1), - Vec3f(-PHI, 0, 1), + std::shared_ptr> pts(new std::vector({ + Vec3s(-1, PHI, 0), + Vec3s(1, PHI, 0), + Vec3s(-1, -PHI, 0), + Vec3s(1, -PHI, 0), + + Vec3s(0, -1, PHI), + Vec3s(0, 1, PHI), + Vec3s(0, -1, -PHI), + Vec3s(0, 1, -PHI), + + Vec3s(PHI, 0, -1), + Vec3s(PHI, 0, 1), + Vec3s(-PHI, 0, -1), + Vec3s(-PHI, 0, 1), })); - std::vector& pts_ = *pts; + std::vector& pts_ = *pts; for (size_t i = 0; i < 12; ++i) { toEllipsoid(pts_[i], ellipsoid); } @@ -557,7 +557,7 @@ Convex constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) { } Box makeRandomBox(CoalScalar min_size, CoalScalar max_size) { - return Box(Vec3f(rand_interval(min_size, max_size), + return Box(Vec3s(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } @@ -567,7 +567,7 @@ Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size) { } Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size) { - return Ellipsoid(Vec3f(rand_interval(min_size, max_size), + return Ellipsoid(Vec3s(rand_interval(min_size, max_size), rand_interval(min_size, max_size), rand_interval(min_size, max_size))); } @@ -596,11 +596,11 @@ Convex makeRandomConvex(CoalScalar min_size, CoalScalar max_size) { } Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size) { - return Plane(Vec3f::Random().normalized(), rand_interval(min_size, max_size)); + return Plane(Vec3s::Random().normalized(), rand_interval(min_size, max_size)); } Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size) { - return Halfspace(Vec3f::Random().normalized(), + return Halfspace(Vec3s::Random().normalized(), rand_interval(min_size, max_size)); } diff --git a/test/utility.h b/test/utility.h index 70c868092..2969bb288 100644 --- a/test/utility.h +++ b/test/utility.h @@ -126,15 +126,15 @@ struct TStruct { extern const Eigen::IOFormat vfmt; extern const Eigen::IOFormat pyfmt; typedef Eigen::AngleAxis AngleAxis; -extern const Vec3f UnitX; -extern const Vec3f UnitY; -extern const Vec3f UnitZ; +extern const Vec3s UnitX; +extern const Vec3s UnitY; +extern const Vec3s UnitZ; /// @brief Load an obj mesh file -void loadOBJFile(const char* filename, std::vector& points, +void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles); -void saveOBJFile(const char* filename, std::vector& points, +void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); #ifdef COAL_HAS_OCTOMAP @@ -167,8 +167,8 @@ void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], /// corresponding nearest point pair struct DistanceRes { double distance; - Vec3f p1; - Vec3f p2; + Vec3s p1; + Vec3s p2; }; /// @brief Default collision callback for two objects o1 and o2 in broad phase. From 770369b632bf9dfc64e8fef0ebdb9d123c8f1fe6 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 17:32:57 +0200 Subject: [PATCH 16/57] all: rename `Transform3f` to `Transform3s` --- README.md | 8 +- doc/mesh-mesh-collision-call.dot | 4 +- doc/shape-mesh-collision-call.dot | 16 +- doc/shape-shape-collision-call.dot | 12 +- include/coal/BV/BV.h | 30 +- include/coal/BVH/BVH_utility.h | 18 +- .../broadphase_dynamic_AABB_tree-inl.h | 12 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 12 +- include/coal/collision.h | 8 +- include/coal/collision_data.h | 4 +- include/coal/collision_func_matrix.h | 4 +- include/coal/collision_object.h | 8 +- include/coal/collision_utility.h | 2 +- include/coal/contact_patch.h | 12 +- .../coal/contact_patch/contact_patch_solver.h | 8 +- .../contact_patch/contact_patch_solver.hxx | 14 +- include/coal/contact_patch_func_matrix.h | 4 +- include/coal/distance.h | 8 +- include/coal/distance_func_matrix.h | 4 +- include/coal/fwd.hh | 2 +- include/coal/internal/intersect.h | 4 +- .../internal/shape_shape_contact_patch_func.h | 24 +- include/coal/internal/shape_shape_func.h | 48 +- include/coal/internal/traversal_node_base.h | 4 +- .../coal/internal/traversal_node_bvh_shape.h | 8 +- .../internal/traversal_node_hfield_shape.h | 8 +- include/coal/internal/traversal_node_octree.h | 94 +- include/coal/internal/traversal_node_setup.h | 110 +- include/coal/math/transform.h | 46 +- .../coal/narrowphase/minkowski_difference.h | 2 +- include/coal/narrowphase/narrowphase.h | 28 +- include/coal/serialization/transform.h | 2 +- .../coal/shape/geometric_shape_to_BVH_model.h | 14 +- include/coal/shape/geometric_shapes.h | 34 +- include/coal/shape/geometric_shapes_utility.h | 132 +- python/collision-geometries.cc | 4 +- python/collision.cc | 6 +- python/contact_patch.cc | 6 +- python/distance.cc | 6 +- python/gjk.cc | 12 +- python/math.cc | 60 +- src/BVH/BVH_utility.cpp | 22 +- .../broadphase_dynamic_AABB_tree.cpp | 20 +- .../broadphase_dynamic_AABB_tree_array.cpp | 20 +- src/collision.cpp | 12 +- src/collision_func_matrix.cpp | 50 +- src/collision_utility.cpp | 6 +- src/contact_patch.cpp | 10 +- src/contact_patch_func_matrix.cpp | 16 +- src/distance.cpp | 10 +- src/distance/box_halfspace.cpp | 8 +- src/distance/box_plane.cpp | 8 +- src/distance/box_sphere.cpp | 8 +- src/distance/capsule_capsule.cpp | 4 +- src/distance/capsule_halfspace.cpp | 8 +- src/distance/capsule_plane.cpp | 8 +- src/distance/cone_halfspace.cpp | 8 +- src/distance/cone_plane.cpp | 8 +- src/distance/convex_halfspace.cpp | 8 +- src/distance/convex_plane.cpp | 8 +- src/distance/cylinder_halfspace.cpp | 8 +- src/distance/cylinder_plane.cpp | 8 +- src/distance/ellipsoid_halfspace.cpp | 8 +- src/distance/ellipsoid_plane.cpp | 8 +- src/distance/halfspace_halfspace.cpp | 4 +- src/distance/halfspace_plane.cpp | 8 +- src/distance/plane_plane.cpp | 4 +- src/distance/sphere_capsule.cpp | 8 +- src/distance/sphere_cylinder.cpp | 8 +- src/distance/sphere_halfspace.cpp | 8 +- src/distance/sphere_plane.cpp | 8 +- src/distance/sphere_sphere.cpp | 4 +- src/distance/triangle_halfspace.cpp | 8 +- src/distance/triangle_plane.cpp | 8 +- src/distance/triangle_sphere.cpp | 8 +- src/distance/triangle_triangle.cpp | 4 +- src/distance_func_matrix.cpp | 62 +- src/intersect.cpp | 4 +- src/math/transform.cpp | 10 +- src/narrowphase/details.h | 44 +- src/narrowphase/minkowski_difference.cpp | 6 +- src/narrowphase/support_functions.cpp | 8 +- src/shape/geometric_shapes.cpp | 20 +- src/shape/geometric_shapes_utility.cpp | 162 +-- test/accelerated_gjk.cpp | 6 +- test/benchmark.cpp | 20 +- test/box_box_collision.cpp | 6 +- test/box_box_distance.cpp | 20 +- test/broadphase.cpp | 24 +- test/bvh_models.cpp | 6 +- test/capsule_box_1.cpp | 6 +- test/capsule_box_2.cpp | 4 +- test/capsule_capsule.cpp | 40 +- test/collision.cpp | 42 +- test/collision_node_asserts.cpp | 4 +- test/contact_patch.cpp | 44 +- test/convex.cpp | 10 +- test/distance.cpp | 20 +- test/distance_lower_bound.cpp | 30 +- test/frontlist.cpp | 26 +- test/geometric_shapes.cpp | 1242 ++++++++--------- test/gjk.cpp | 14 +- test/gjk_asserts.cpp | 4 +- test/gjk_convergence_criterion.cpp | 6 +- test/hfields.cpp | 68 +- test/math.cpp | 6 +- test/normal_and_nearest_points.cpp | 20 +- test/obb.cpp | 2 +- test/octree.cpp | 18 +- test/profiling.cpp | 8 +- test/python_unit/api.py | 8 +- test/python_unit/collision.py | 12 +- test/python_unit/collision_manager.py | 4 +- test/security_margin.cpp | 66 +- test/serialization.cpp | 8 +- test/shape_inflation.cpp | 12 +- test/shape_mesh_consistency.cpp | 1020 +++++++------- test/swept_sphere_radius.cpp | 20 +- test/utility.cpp | 20 +- test/utility.h | 10 +- 120 files changed, 2194 insertions(+), 2194 deletions(-) diff --git a/README.md b/README.md index 7cb262f3d..d731a1727 100644 --- a/README.md +++ b/README.md @@ -111,10 +111,10 @@ int main() { std::shared_ptr shape2 = loadConvexMesh("../path/to/mesh/file.obj"); // Define the shapes' placement in 3D space - hpp::fcl::Transform3f T1; + hpp::fcl::Transform3s T1; T1.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); T1.setTranslation(hpp::fcl::Vec3s::Random()); - hpp::fcl::Transform3f T2 = hpp::fcl::Transform3f::Identity(); + hpp::fcl::Transform3s T2 = hpp::fcl::Transform3s::Identity(); T2.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); T2.setTranslation(hpp::fcl::Vec3s::Random()); @@ -180,10 +180,10 @@ if __name__ == "__main__": shape2 = loadConvexMesh("../path/to/mesh/file.obj") # Define the shapes' placement in 3D space - T1 = hppfcl.Transform3f() + T1 = hppfcl.Transform3s() T1.setTranslation(pin.SE3.Random().translation) T1.setRotation(pin.SE3.Random().rotation) - T2 = hppfcl.Transform3f(); + T2 = hppfcl.Transform3s(); # Using np arrays also works T1.setTranslation(np.random.rand(3)) T2.setRotation(pin.SE3.Random().rotation) diff --git a/doc/mesh-mesh-collision-call.dot b/doc/mesh-mesh-collision-call.dot index 5069bbc49..e70206145 100644 --- a/doc/mesh-mesh-collision-call.dot +++ b/doc/mesh-mesh-collision-call.dot @@ -4,7 +4,7 @@ digraph CD { compound=true size = 11.7 - "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box] + "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box] "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] "bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box] "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box] @@ -19,7 +19,7 @@ digraph CD { "MeshCollisionTraversalNode::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box] "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box] - "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" + "std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3s& tf1, const CollisionGeometry* o2,\nconst Transform3s& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" diff --git a/doc/shape-mesh-collision-call.dot b/doc/shape-mesh-collision-call.dot index 3eb293642..2a16e9f98 100644 --- a/doc/shape-mesh-collision-call.dot +++ b/doc/shape-mesh-collision-call.dot @@ -4,8 +4,8 @@ digraph CD { compound=true size = 11.7 - "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] - "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] + "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] + "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" [shape = box] "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" [shape = box] "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" [shape = box] "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" [shape = box] @@ -16,11 +16,11 @@ digraph CD { "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box] "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box] "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box] - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] - "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box] + "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box] - "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" - "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" + "BVHShapeCollider::collide\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,\nconst CollisionRequest& request, CollisionResult& result)\ncollision_func_matrix.cpp" -> "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" + "details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>\n(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s&tf2,\nNarrowPhaseSolver* nsolver, const CollisionRequest& request,\nCollisionResult& result)\ncollision_func_matrix.cpp" -> "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" "void collide(CollisionTraversalNodeBase* node,\nconst CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)\ncollision_node.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" "void propagateBVHFrontListCollisionRecurse\n(CollisionTraversalNodeBase* node, const CollisionRequest& request,\nCollisionResult& result, BVHFrontList* front_list)\ntraversal/traversal_recurse.cpp" -> "void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" @@ -31,6 +31,6 @@ digraph CD { "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" - "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" - "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3f& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" + "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" + "void details::meshShapeCollisionOrientedNodeLeafTesting \n(int b1, int b2, const BVHModel* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3s& tf1,\nconst Transform3s& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect\n(const S& s, const Transform3s& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" } diff --git a/doc/shape-shape-collision-call.dot b/doc/shape-shape-collision-call.dot index 59e8467ed..58d666331 100644 --- a/doc/shape-shape-collision-call.dot +++ b/doc/shape-shape-collision-call.dot @@ -4,16 +4,16 @@ digraph CD { compound=true size = 11.7 - "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box] - "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box] + "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" [shape = box] + "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" [shape = box] "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box] "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box] "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box] - "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box] + "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box] - "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" - "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" + "template\nstd::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver,const CollisionRequest& request,\nCollisionResult& result)" -> "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" + "template\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3s& tf1,\nconst CollisionGeometry* o2, const Transform3s& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)" -> "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" "void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" -> "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" "void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" -> "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" - "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" + "void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" -> "template bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3s& tf1, const S2& s2, const Transform3s& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" } diff --git a/include/coal/BV/BV.h b/include/coal/BV/BV.h index aba2d1d49..2224f7918 100644 --- a/include/coal/BV/BV.h +++ b/include/coal/BV/BV.h @@ -56,14 +56,14 @@ namespace details { /// bounding volume of type BV2 in I configuration. template struct Converter { - static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2); + static void convert(const BV1& bv1, const Transform3s& tf1, BV2& bv2); static void convert(const BV1& bv1, BV2& bv2); }; /// @brief Convert from AABB to AABB, not very tight but is fast. template <> struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) { + static void convert(const AABB& bv1, const Transform3s& tf1, AABB& bv2) { const Vec3s& center = bv1.center(); CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5; const Vec3s center2 = tf1.transform(center); @@ -76,7 +76,7 @@ struct Converter { template <> struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) { + static void convert(const AABB& bv1, const Transform3s& tf1, OBB& bv2) { bv2.To = tf1.transform(bv1.center()); bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; bv2.axes = tf1.getRotation(); @@ -91,7 +91,7 @@ struct Converter { template <> struct Converter { - static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) { + static void convert(const OBB& bv1, const Transform3s& tf1, OBB& bv2) { bv2.extent = bv1.extent; bv2.To = tf1.transform(bv1.To); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; @@ -102,7 +102,7 @@ struct Converter { template <> struct Converter { - static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) { + static void convert(const OBBRSS& bv1, const Transform3s& tf1, OBB& bv2) { Converter::convert(bv1.obb, tf1, bv2); } @@ -113,7 +113,7 @@ struct Converter { template <> struct Converter { - static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) { + static void convert(const RSS& bv1, const Transform3s& tf1, OBB& bv2) { bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius); bv2.To = tf1.transform(bv1.Tr); @@ -130,7 +130,7 @@ struct Converter { template struct Converter { - static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) { + static void convert(const BV1& bv1, const Transform3s& tf1, AABB& bv2) { const Vec3s& center = bv1.center(); CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; const Vec3s center2 = tf1.transform(center); @@ -148,7 +148,7 @@ struct Converter { template struct Converter { - static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) { + static void convert(const BV1& bv1, const Transform3s& tf1, OBB& bv2) { AABB bv; Converter::convert(bv1, bv); Converter::convert(bv, tf1, bv2); @@ -163,7 +163,7 @@ struct Converter { template <> struct Converter { - static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) { + static void convert(const OBB& bv1, const Transform3s& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.To); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; @@ -184,7 +184,7 @@ struct Converter { template <> struct Converter { - static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) { + static void convert(const RSS& bv1, const Transform3s& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.Tr); bv2.axes.noalias() = tf1.getRotation() * bv1.axes; @@ -198,7 +198,7 @@ struct Converter { template <> struct Converter { - static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) { + static void convert(const OBBRSS& bv1, const Transform3s& tf1, RSS& bv2) { Converter::convert(bv1.rss, tf1, bv2); } @@ -209,7 +209,7 @@ struct Converter { template <> struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) { + static void convert(const AABB& bv1, const Transform3s& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.center()); /// Sort the AABB edges so that AABB extents are ordered. @@ -249,13 +249,13 @@ struct Converter { } static void convert(const AABB& bv1, RSS& bv2) { - convert(bv1, Transform3f(), bv2); + convert(bv1, Transform3s(), bv2); } }; template <> struct Converter { - static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2) { + static void convert(const AABB& bv1, const Transform3s& tf1, OBBRSS& bv2) { Converter::convert(bv1, tf1, bv2.obb); Converter::convert(bv1, tf1, bv2.rss); } @@ -273,7 +273,7 @@ struct Converter { /// @brief Convert a bounding volume of type BV1 in configuration tf1 to /// bounding volume of type BV2 in identity configuration. template -static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) { +static inline void convertBV(const BV1& bv1, const Transform3s& tf1, BV2& bv2) { details::Converter::convert(bv1, tf1, bv2); } diff --git a/include/coal/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h index ef24f8845..daf0e4781 100644 --- a/include/coal/BVH/BVH_utility.h +++ b/include/coal/BVH/BVH_utility.h @@ -45,39 +45,39 @@ namespace coal { /// A triangle in collision with the AABB is considered inside. template COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, const AABB& aabb); + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); template <> COAL_DLLAPI BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); /// @brief Compute the covariance matrix for a set or subset of points. if ts = diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h index 870e39b06..6e1314f6e 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -71,8 +71,8 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const AABB& root2_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); @@ -100,8 +100,8 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const AABB& root2_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); @@ -159,8 +159,8 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); diff --git a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 296555166..730db5782 100644 --- a/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/coal/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -68,8 +68,8 @@ bool collisionRecurse_( const AABB& root_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root_bv_t)) { Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); @@ -95,8 +95,8 @@ bool collisionRecurse_( const AABB& root_bv_t = translate(root2_bv, translation2); if (root1->bv.overlap(root_bv_t)) { Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); @@ -157,8 +157,8 @@ bool distanceRecurse_( if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); - Transform3f box_tf; - Transform3f tf2 = Transform3f::Identity(); + Transform3s box_tf; + Transform3s tf2 = Transform3s::Identity(); tf2.translation() = translation2; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); diff --git a/include/coal/collision.h b/include/coal/collision.h index 39ae20b63..cf9b9a1c0 100644 --- a/include/coal/collision.h +++ b/include/coal/collision.h @@ -62,9 +62,9 @@ COAL_DLLAPI std::size_t collide(const CollisionObject* o1, /// @copydoc collide(const CollisionObject*, const CollisionObject*, const /// CollisionRequest&, CollisionResult&) COAL_DLLAPI std::size_t collide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result); @@ -80,7 +80,7 @@ class COAL_DLLAPI ComputeCollision { /// @brief Default constructor from two Collision Geometries. ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2); - std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2, + std::size_t operator()(const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const; @@ -107,7 +107,7 @@ class COAL_DLLAPI ComputeCollision { CollisionFunctionMatrix::CollisionFunc func; bool swap_geoms; - virtual std::size_t run(const Transform3f& tf1, const Transform3f& tf2, + virtual std::size_t run(const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const; diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index 7be9827ac..6e17f0485 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -515,7 +515,7 @@ struct COAL_DLLAPI ContactPatch { /// @brief Frame of the set, expressed in the world coordinates. /// The z-axis of the frame's rotation is the contact patch normal. - Transform3f tf; + Transform3s tf; /// @brief Direction of ContactPatch. /// When doing collision detection, the convention of Coal is that the @@ -559,7 +559,7 @@ struct COAL_DLLAPI ContactPatch { /// of the patch is known in advance. Coal will automatically expand/shrink /// the contact patch if needed. explicit ContactPatch(size_t preallocated_size = default_preallocated_size) - : tf(Transform3f::Identity()), + : tf(Transform3s::Identity()), direction(PatchDirection::DEFAULT), penetration_depth(0) { this->m_points.reserve(preallocated_size); diff --git a/include/coal/collision_func_matrix.h b/include/coal/collision_func_matrix.h index 4311d4613..e9aeca48f 100644 --- a/include/coal/collision_func_matrix.h +++ b/include/coal/collision_func_matrix.h @@ -58,9 +58,9 @@ struct COAL_DLLAPI CollisionFunctionMatrix { /// information, whether need to compute cost); /// 4. the structure to return collision result typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result); diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h index 503ca6016..ccc5cf136 100644 --- a/include/coal/collision_object.h +++ b/include/coal/collision_object.h @@ -220,7 +220,7 @@ class COAL_DLLAPI CollisionObject { } CollisionObject(const shared_ptr& cgeom_, - const Transform3f& tf, bool compute_local_aabb = true) + const Transform3s& tf, bool compute_local_aabb = true) : cgeom(cgeom_), t(tf), user_data(nullptr) { init(compute_local_aabb); } @@ -287,7 +287,7 @@ class COAL_DLLAPI CollisionObject { inline const Matrix3s& getRotation() const { return t.getRotation(); } /// @brief get object's transform - inline const Transform3f& getTransform() const { return t; } + inline const Transform3s& getTransform() const { return t; } /// @brief set object's rotation matrix void setRotation(const Matrix3s& R) { t.setRotation(R); } @@ -299,7 +299,7 @@ class COAL_DLLAPI CollisionObject { void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); } /// @brief set object's transform - void setTransform(const Transform3f& tf) { t = tf; } + void setTransform(const Transform3s& tf) { t = tf; } /// @brief whether the object is in local coordinate bool isIdentityTransform() const { return t.isIdentity(); } @@ -346,7 +346,7 @@ class COAL_DLLAPI CollisionObject { shared_ptr cgeom; - Transform3f t; + Transform3s t; /// @brief AABB in global coordinate mutable AABB aabb; diff --git a/include/coal/collision_utility.h b/include/coal/collision_utility.h index 19aa534a8..44a597d26 100644 --- a/include/coal/collision_utility.h +++ b/include/coal/collision_utility.h @@ -23,7 +23,7 @@ namespace coal { COAL_DLLAPI CollisionGeometry* extract(const CollisionGeometry* model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb); /** diff --git a/include/coal/contact_patch.h b/include/coal/contact_patch.h index edea02d2c..a942fa0b9 100644 --- a/include/coal/contact_patch.h +++ b/include/coal/contact_patch.h @@ -50,15 +50,15 @@ namespace coal { /// read @ref ContactPatch if you want to fully understand what is meant by /// "contact patch". COAL_DLLAPI void computeContactPatch(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result); -/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3f&, -// const CollisionGeometry*, const Transform3f&, const CollisionResult&, const +/// @copydoc computeContactPatch(const CollisionGeometry*, const Transform3s&, +// const CollisionGeometry*, const Transform3s&, const CollisionResult&, const // ContactPatchRequest&, ContactPatchResult&); COAL_DLLAPI void computeContactPatch(const CollisionObject* o1, const CollisionObject* o2, @@ -79,7 +79,7 @@ class COAL_DLLAPI ComputeContactPatch { /// @brief Default constructor from two Collision Geometries. ComputeContactPatch(const CollisionGeometry* o1, const CollisionGeometry* o2); - void operator()(const Transform3f& tf1, const Transform3f& tf2, + void operator()(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const; @@ -108,7 +108,7 @@ class COAL_DLLAPI ComputeContactPatch { ContactPatchFunctionMatrix::ContactPatchFunc func; bool swap_geoms; - virtual void run(const Transform3f& tf1, const Transform3f& tf2, + virtual void run(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const; diff --git a/include/coal/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h index 63c7fd342..cce436ea1 100644 --- a/include/coal/contact_patch/contact_patch_solver.h +++ b/include/coal/contact_patch/contact_patch_solver.h @@ -160,14 +160,14 @@ struct COAL_DLLAPI ContactPatchSolver { /// plane passing (by `contact.pos` and supported by `contact.normal`) and the /// shapes s1 and s2. template - void computePatch(const ShapeType1& s1, const Transform3f& tf1, - const ShapeType2& s2, const Transform3f& tf2, + void computePatch(const ShapeType1& s1, const Transform3s& tf1, + const ShapeType2& s2, const Transform3s& tf2, const Contact& contact, ContactPatch& contact_patch) const; /// @brief Reset the internal quantities of the solver. template - void reset(const ShapeType1& shape1, const Transform3f& tf1, - const ShapeType2& shape2, const Transform3f& tf2, + void reset(const ShapeType1& shape1, const Transform3s& tf1, + const ShapeType2& shape2, const Transform3s& tf2, const ContactPatch& contact_patch) const; /// @brief Retrieve result, adds a post-processing step if result has bigger diff --git a/include/coal/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx index 403612f37..f4f97b02c 100644 --- a/include/coal/contact_patch/contact_patch_solver.hxx +++ b/include/coal/contact_patch/contact_patch_solver.hxx @@ -74,9 +74,9 @@ inline void ContactPatchSolver::set(const ContactPatchRequest& request) { // ============================================================================ template void ContactPatchSolver::computePatch(const ShapeType1& s1, - const Transform3f& tf1, + const Transform3s& tf1, const ShapeType2& s2, - const Transform3f& tf2, + const Transform3s& tf2, const Contact& contact, ContactPatch& contact_patch) const { // Note: `ContactPatch` is an alias for `SupportSet`. @@ -372,9 +372,9 @@ inline void ContactPatchSolver::getResult( // ============================================================================ template inline void ContactPatchSolver::reset(const ShapeType1& shape1, - const Transform3f& tf1, + const Transform3s& tf1, const ShapeType2& shape2, - const Transform3f& tf2, + const Transform3s& tf2, const ContactPatch& contact_patch) const { // Reset internal quantities this->support_set_shape1.clear(); @@ -382,12 +382,12 @@ inline void ContactPatchSolver::reset(const ShapeType1& shape1, this->support_set_buffer.clear(); // Get the support function of each shape - const Transform3f& tfc = contact_patch.tf; + const Transform3s& tfc = contact_patch.tf; this->support_set_shape1.direction = SupportSetDirection::DEFAULT; // Set the reference frame of the support set of the first shape to be the // local frame of shape 1. - Transform3f& tf1c = this->support_set_shape1.tf; + Transform3s& tf1c = this->support_set_shape1.tf; tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation(); tf1c.translation().noalias() = tf1.rotation().transpose() * (tfc.translation() - tf1.translation()); @@ -397,7 +397,7 @@ inline void ContactPatchSolver::reset(const ShapeType1& shape1, this->support_set_shape2.direction = SupportSetDirection::INVERTED; // Set the reference frame of the support set of the second shape to be the // local frame of shape 2. - Transform3f& tf2c = this->support_set_shape2.tf; + Transform3s& tf2c = this->support_set_shape2.tf; tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation(); tf2c.translation().noalias() = tf2.rotation().transpose() * (tfc.translation() - tf2.translation()); diff --git a/include/coal/contact_patch_func_matrix.h b/include/coal/contact_patch_func_matrix.h index ec2aeb496..f95d73c4e 100644 --- a/include/coal/contact_patch_func_matrix.h +++ b/include/coal/contact_patch_func_matrix.h @@ -65,9 +65,9 @@ struct COAL_DLLAPI ContactPatchFunctionMatrix { /// tf1 and tf2 and make multiple calls to the GJKSolver (although this is not /// the approach done by default). typedef void (*ContactPatchFunc)(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, diff --git a/include/coal/distance.h b/include/coal/distance.h index 21da64876..7dd4a3e7d 100644 --- a/include/coal/distance.h +++ b/include/coal/distance.h @@ -57,9 +57,9 @@ COAL_DLLAPI CoalScalar distance(const CollisionObject* o1, /// @copydoc distance(const CollisionObject*, const CollisionObject*, const /// DistanceRequest&, DistanceResult&) COAL_DLLAPI CoalScalar distance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result); @@ -74,7 +74,7 @@ class COAL_DLLAPI ComputeDistance { public: ComputeDistance(const CollisionGeometry* o1, const CollisionGeometry* o2); - CoalScalar operator()(const Transform3f& tf1, const Transform3f& tf2, + CoalScalar operator()(const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const; @@ -102,7 +102,7 @@ class COAL_DLLAPI ComputeDistance { DistanceFunctionMatrix::DistanceFunc func; bool swap_geoms; - virtual CoalScalar run(const Transform3f& tf1, const Transform3f& tf2, + virtual CoalScalar run(const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const; diff --git a/include/coal/distance_func_matrix.h b/include/coal/distance_func_matrix.h index febcc663d..a37e1df51 100644 --- a/include/coal/distance_func_matrix.h +++ b/include/coal/distance_func_matrix.h @@ -55,9 +55,9 @@ struct COAL_DLLAPI DistanceFunctionMatrix { /// 3. the request setting for distance (e.g., whether need to return nearest /// points); typedef CoalScalar (*DistanceFunc)(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result); diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh index 196a207cf..8be5e49a8 100644 --- a/include/coal/fwd.hh +++ b/include/coal/fwd.hh @@ -134,7 +134,7 @@ typedef shared_ptr CollisionObjectConstPtr_t; class CollisionGeometry; typedef shared_ptr CollisionGeometryPtr_t; typedef shared_ptr CollisionGeometryConstPtr_t; -class Transform3f; +class Transform3s; class AABB; diff --git a/include/coal/internal/intersect.h b/include/coal/internal/intersect.h index 4b5ef8d70..350101cff 100644 --- a/include/coal/internal/intersect.h +++ b/include/coal/internal/intersect.h @@ -146,7 +146,7 @@ class COAL_DLLAPI TriangleDistance { /// from the triangles, not coincident points on the intersection of the /// triangles, as might be expected. static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], - const Transform3f& tf, Vec3s& P, Vec3s& Q); + const Transform3s& tf, Vec3s& P, Vec3s& Q); /// Compute squared distance between triangles /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices @@ -177,7 +177,7 @@ class COAL_DLLAPI TriangleDistance { static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, - const Transform3f& tf, Vec3s& P, Vec3s& Q); + const Transform3s& tf, Vec3s& P, Vec3s& Q); }; } // namespace coal diff --git a/include/coal/internal/shape_shape_contact_patch_func.h b/include/coal/internal/shape_shape_contact_patch_func.h index b18b6bbc3..3eb28b539 100644 --- a/include/coal/internal/shape_shape_contact_patch_func.h +++ b/include/coal/internal/shape_shape_contact_patch_func.h @@ -50,8 +50,8 @@ namespace coal { /// by the `ContactPatchRequest`. template struct ComputeShapeShapeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, @@ -88,9 +88,9 @@ struct ComputeShapeShapeContactPatch { /// this function). template void computePatchPlaneOrHalfspace(const OtherShapeType& s1, - const Transform3f& tf1, + const Transform3s& tf1, const PlaneOrHalfspace& s2, - const Transform3f& tf2, + const Transform3s& tf2, const ContactPatchSolver* csolver, const Contact& contact, ContactPatch& contact_patch) { @@ -134,8 +134,8 @@ void computePatchPlaneOrHalfspace(const OtherShapeType& s1, #define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace) \ template \ struct ComputeShapeShapeContactPatch { \ - static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const CollisionResult& collision_result, \ const ContactPatchSolver* csolver, \ const ContactPatchRequest& request, \ @@ -168,8 +168,8 @@ void computePatchPlaneOrHalfspace(const OtherShapeType& s1, \ template \ struct ComputeShapeShapeContactPatch { \ - static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const CollisionResult& collision_result, \ const ContactPatchSolver* csolver, \ const ContactPatchRequest& request, \ @@ -206,8 +206,8 @@ PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace) #define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2) \ template <> \ struct ComputeShapeShapeContactPatch { \ - static void run(const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const CollisionResult& collision_result, \ const ContactPatchSolver* csolver, \ const ContactPatchRequest& request, \ @@ -249,8 +249,8 @@ PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace) #undef PLANE_HSPACE_CONTACT_PATCH template -void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, diff --git a/include/coal/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h index 1d518e7f0..eb18b3ae9 100644 --- a/include/coal/internal/shape_shape_func.h +++ b/include/coal/internal/shape_shape_func.h @@ -49,8 +49,8 @@ namespace coal { template struct ShapeShapeDistancer { - static CoalScalar run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { @@ -70,8 +70,8 @@ struct ShapeShapeDistancer { return distance; } - static CoalScalar run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static CoalScalar run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) { @@ -91,9 +91,9 @@ struct ShapeShapeDistancer { /// @note This function might be specialized for some pairs of shapes. template CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return ShapeShapeDistancer::run( @@ -113,9 +113,9 @@ namespace internal { /// @note This function might be specialized for some pairs of shapes. template CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) { return ::coal::ShapeShapeDistancer::run( @@ -134,8 +134,8 @@ CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, /// internal collision detection checks. template struct ShapeShapeCollider { - static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static std::size_t run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { @@ -168,9 +168,9 @@ struct ShapeShapeCollider { template std::size_t ShapeShapeCollide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return ShapeShapeCollider::run( @@ -217,20 +217,20 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ template <> \ COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ Vec3s& p2, Vec3s& normal); \ template <> \ COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ Vec3s& p2, Vec3s& normal); \ template <> \ inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ DistanceResult& result) { \ result.o1 = o1; \ @@ -244,8 +244,8 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, } \ template <> \ inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ DistanceResult& result) { \ result.o1 = o1; \ @@ -261,14 +261,14 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, #define SHAPE_SELF_DISTANCE_SPECIALIZATION(T) \ template <> \ COAL_DLLAPI CoalScalar internal::ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \ Vec3s& p2, Vec3s& normal); \ template <> \ inline COAL_DLLAPI CoalScalar ShapeShapeDistance( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ + const CollisionGeometry* o1, const Transform3s& tf1, \ + const CollisionGeometry* o2, const Transform3s& tf2, \ const GJKSolver* nsolver, const DistanceRequest& request, \ DistanceResult& result) { \ result.o1 = o1; \ diff --git a/include/coal/internal/traversal_node_base.h b/include/coal/internal/traversal_node_base.h index 238dfc1a3..edd252985 100644 --- a/include/coal/internal/traversal_node_base.h +++ b/include/coal/internal/traversal_node_base.h @@ -85,10 +85,10 @@ class TraversalNodeBase { void enableStatistics(bool enable) { enable_statistics = enable; } /// @brief configuation of first object - Transform3f tf1; + Transform3s tf1; /// @brief configuration of second object - Transform3f tf2; + Transform3s tf2; /// @brief Whether stores statistics bool enable_statistics; diff --git a/include/coal/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h index cff2a801b..5b85879a5 100644 --- a/include/coal/internal/traversal_node_bvh_shape.h +++ b/include/coal/internal/traversal_node_bvh_shape.h @@ -157,7 +157,7 @@ class MeshShapeCollisionTraversalNode CoalScalar distance; if (RTIsIdentity) { - static const Transform3f Id; + static const Transform3s Id; distance = internal::ShapeShapeDistance( &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration, c1, c2, normal); @@ -341,7 +341,7 @@ template void meshShapeDistanceOrientedNodeleafComputeDistance( unsigned int b1, unsigned int /* b2 */, const BVHModel* model1, const S& model2, Vec3s* vertices, Triangle* tri_indices, - const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver, bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request, DistanceResult& result) { if (enable_statistics) num_leaf_tests++; @@ -365,8 +365,8 @@ void meshShapeDistanceOrientedNodeleafComputeDistance( template static inline void distancePreprocessOrientedNode( const BVHModel* model1, Vec3s* vertices, Triangle* tri_indices, - int init_tri_id, const S& model2, const Transform3f& tf1, - const Transform3f& tf2, const GJKSolver* nsolver, + int init_tri_id, const S& model2, const Transform3s& tf1, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { const Triangle& tri_id = tri_indices[init_tri_id]; const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]], diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h index 438f5c8cc..20e6202c2 100644 --- a/include/coal/internal/traversal_node_hfield_shape.h +++ b/include/coal/internal/traversal_node_hfield_shape.h @@ -320,7 +320,7 @@ inline CoalScalar distanceContactPointToFace(const size_t face_id, template bool binCorrection(const Convex& convex, const int convex_active_faces, const Shape& shape, - const Transform3f& shape_pose, CoalScalar& distance, + const Transform3s& shape_pose, CoalScalar& distance, Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal, Vec3s& face_normal, const bool is_collision) { const CoalScalar prec = 1e-12; @@ -403,13 +403,13 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request, const Convex& convex1, const int convex1_active_faces, const Convex& convex2, - const int convex2_active_faces, const Transform3f& tf1, - const Shape& shape, const Transform3f& tf2, + const int convex2_active_faces, const Transform3s& tf1, + const Shape& shape, const Transform3s& tf2, CoalScalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal, Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) { enum { RTIsIdentity = Options & RelativeTransformationIsIdentity }; - const Transform3f Id; + const Transform3s Id; // The solver `nsolver` has already been set up by the collision request // `request`. If GJK early stopping is enabled through `request`, it will be // used. diff --git a/include/coal/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h index f9b49722e..2f9da5f07 100644 --- a/include/coal/internal/traversal_node_octree.h +++ b/include/coal/internal/traversal_node_octree.h @@ -74,7 +74,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief collision between two octrees void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; @@ -86,7 +86,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief distance between two octrees void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; @@ -99,7 +99,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief collision between octree and mesh template void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; @@ -112,7 +112,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief distance between octree and mesh template void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; @@ -125,7 +125,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief collision between mesh and octree template void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const @@ -140,7 +140,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief distance between mesh and octree template void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; @@ -152,8 +152,8 @@ class COAL_DLLAPI OcTreeSolver { template void OcTreeHeightFieldIntersect( - const OcTree* tree1, const HeightField* tree2, const Transform3f& tf1, - const Transform3f& tf2, const CollisionRequest& request_, + const OcTree* tree1, const HeightField* tree2, const Transform3s& tf1, + const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_, CoalScalar& sqrDistLowerBound) const { crequest = &request_; cresult = &result_; @@ -165,8 +165,8 @@ class COAL_DLLAPI OcTreeSolver { template void HeightFieldOcTreeIntersect(const HeightField* tree1, - const OcTree* tree2, const Transform3f& tf1, - const Transform3f& tf2, + const OcTree* tree2, const Transform3s& tf1, + const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_, CoalScalar& sqrDistLowerBound) const { @@ -181,14 +181,14 @@ class COAL_DLLAPI OcTreeSolver { /// @brief collision between octree and shape template void OcTreeShapeIntersect(const OcTree* tree, const S& s, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; AABB bv2; - computeBV(s, Transform3f(), bv2); + computeBV(s, Transform3s(), bv2); OBB obb2; convertBV(bv2, tf2, obb2); OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, @@ -198,14 +198,14 @@ class COAL_DLLAPI OcTreeSolver { /// @brief collision between shape and octree template void ShapeOcTreeIntersect(const S& s, const OcTree* tree, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; AABB bv1; - computeBV(s, Transform3f(), bv1); + computeBV(s, Transform3s(), bv1); OBB obb1; convertBV(bv1, tf1, obb1); OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, @@ -215,7 +215,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief distance between octree and shape template void OcTreeShapeDistance(const OcTree* tree, const S& s, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; @@ -230,7 +230,7 @@ class COAL_DLLAPI OcTreeSolver { /// @brief distance between shape and octree template void ShapeOcTreeDistance(const S& s, const OcTree* tree, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; @@ -247,12 +247,12 @@ class COAL_DLLAPI OcTreeSolver { bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const S& s, - const AABB& aabb2, const Transform3f& tf1, - const Transform3f& tf2) const { + const AABB& aabb2, const Transform3s& tf1, + const Transform3s& tf2) const { if (!tree1->nodeHasChildren(root1)) { if (tree1->isNodeOccupied(root1)) { Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { @@ -299,8 +299,8 @@ class COAL_DLLAPI OcTreeSolver { bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const S& s, const OBB& obb2, - const Transform3f& tf1, - const Transform3f& tf2) const { + const Transform3s& tf1, + const Transform3s& tf2) const { // Empty OcTree is considered free. if (!root1) return false; @@ -327,7 +327,7 @@ class COAL_DLLAPI OcTreeSolver { assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); @@ -371,12 +371,12 @@ class COAL_DLLAPI OcTreeSolver { bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const BVHModel* tree2, - unsigned int root2, const Transform3f& tf1, - const Transform3f& tf2) const { + unsigned int root2, const Transform3s& tf1, + const Transform3s& tf2) const { if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { if (tree1->isNodeOccupied(root1)) { Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { @@ -462,8 +462,8 @@ class COAL_DLLAPI OcTreeSolver { bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const BVHModel* tree2, - unsigned int root2, const Transform3f& tf1, - const Transform3f& tf2) const { + unsigned int root2, const Transform3s& tf1, + const Transform3s& tf2) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? @@ -496,7 +496,7 @@ class COAL_DLLAPI OcTreeSolver { if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); @@ -566,8 +566,8 @@ class COAL_DLLAPI OcTreeSolver { template bool OcTreeHeightFieldIntersectRecurse( const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, - const HeightField* tree2, unsigned int root2, const Transform3f& tf1, - const Transform3f& tf2, CoalScalar& sqrDistLowerBound) const { + const HeightField* tree2, unsigned int root2, const Transform3s& tf1, + const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? @@ -602,7 +602,7 @@ class COAL_DLLAPI OcTreeSolver { if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) { assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain. Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(bv1, tf1, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); @@ -686,8 +686,8 @@ class COAL_DLLAPI OcTreeSolver { template bool HeightFieldOcTreeIntersectRecurse( const HeightField* tree1, unsigned int root1, const OcTree* tree2, - const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1, - const Transform3f& tf2, CoalScalar& sqrDistLowerBound) const { + const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1, + const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const { // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The // code in this if(!root1) did not output anything so the empty OcTree is // considered free. Should an empty OcTree be considered free ? @@ -722,7 +722,7 @@ class COAL_DLLAPI OcTreeSolver { if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) { assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain. Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(bv2, tf2, box, box_tf); if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) { box.computeLocalAABB(); @@ -806,12 +806,12 @@ class COAL_DLLAPI OcTreeSolver { const OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3f& tf1, - const Transform3f& tf2) const { + const Transform3s& tf1, + const Transform3s& tf2) const { if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { Box box1, box2; - Transform3f box1_tf, box2_tf; + Transform3s box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); @@ -887,8 +887,8 @@ class COAL_DLLAPI OcTreeSolver { const OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, - const Transform3f& tf1, - const Transform3f& tf2) const { + const Transform3s& tf1, + const Transform3s& tf2) const { // Empty OcTree is considered free. if (!root1) return false; if (!root2) return false; @@ -931,7 +931,7 @@ class COAL_DLLAPI OcTreeSolver { assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)); Box box1, box2; - Transform3f box1_tf, box2_tf; + Transform3s box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); @@ -1028,7 +1028,7 @@ class COAL_DLLAPI OcTreeCollisionTraversalNode const OcTree* model1; const OcTree* model2; - Transform3f tf1, tf2; + Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; @@ -1060,7 +1060,7 @@ class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode const S* model1; const OcTree* model2; - Transform3f tf1, tf2; + Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; @@ -1093,7 +1093,7 @@ class COAL_DLLAPI OcTreeShapeCollisionTraversalNode const OcTree* model1; const S* model2; - Transform3f tf1, tf2; + Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; @@ -1125,7 +1125,7 @@ class COAL_DLLAPI MeshOcTreeCollisionTraversalNode const BVHModel* model1; const OcTree* model2; - Transform3f tf1, tf2; + Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; @@ -1157,7 +1157,7 @@ class COAL_DLLAPI OcTreeMeshCollisionTraversalNode const OcTree* model1; const BVHModel* model2; - Transform3f tf1, tf2; + Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; @@ -1188,7 +1188,7 @@ class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode const OcTree* model1; const HeightField* model2; - Transform3f tf1, tf2; + Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; @@ -1219,7 +1219,7 @@ class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode const HeightField* model1; const OcTree* model2; - Transform3f tf1, tf2; + Transform3s tf1, tf2; const OcTreeSolver* otsolver; }; diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h index 6f60609e8..4f892a02a 100644 --- a/include/coal/internal/traversal_node_setup.h +++ b/include/coal/internal/traversal_node_setup.h @@ -61,8 +61,8 @@ namespace coal { /// @brief Initialize traversal node for collision between two octrees, given /// current object transform inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; @@ -80,8 +80,8 @@ inline bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, /// @brief Initialize traversal node for distance between two octrees, given /// current object transform inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { @@ -103,8 +103,8 @@ inline bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1, /// octree, given current object transform template bool initialize(ShapeOcTreeCollisionTraversalNode& node, const S& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; @@ -123,8 +123,8 @@ bool initialize(ShapeOcTreeCollisionTraversalNode& node, const S& model1, /// shape, given current object transform template bool initialize(OcTreeShapeCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, const S& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, + const OcTree& model1, const Transform3s& tf1, const S& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; @@ -143,8 +143,8 @@ bool initialize(OcTreeShapeCollisionTraversalNode& node, /// octree, given current object transform template bool initialize(ShapeOcTreeDistanceTraversalNode& node, const S& model1, - const Transform3f& tf1, const OcTree& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, + const Transform3s& tf1, const OcTree& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; @@ -164,7 +164,7 @@ bool initialize(ShapeOcTreeDistanceTraversalNode& node, const S& model1, /// shape, given current object transform template bool initialize(OcTreeShapeDistanceTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const S& model2, const Transform3f& tf2, + const Transform3s& tf1, const S& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; @@ -185,8 +185,8 @@ bool initialize(OcTreeShapeDistanceTraversalNode& node, const OcTree& model1, /// octree, given current object transform template bool initialize(MeshOcTreeCollisionTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; @@ -205,8 +205,8 @@ bool initialize(MeshOcTreeCollisionTraversalNode& node, /// mesh, given current object transform template bool initialize(OcTreeMeshCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, + const OcTree& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; @@ -225,8 +225,8 @@ bool initialize(OcTreeMeshCollisionTraversalNode& node, /// height field, given current object transform template bool initialize(OcTreeHeightFieldCollisionTraversalNode& node, - const OcTree& model1, const Transform3f& tf1, - const HeightField& model2, const Transform3f& tf2, + const OcTree& model1, const Transform3s& tf1, + const HeightField& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; @@ -245,8 +245,8 @@ bool initialize(OcTreeHeightFieldCollisionTraversalNode& node, /// one octree, given current object transform template bool initialize(HeightFieldOcTreeCollisionTraversalNode& node, - const HeightField& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, + const HeightField& model1, const Transform3s& tf1, + const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, CollisionResult& result) { node.result = &result; @@ -265,8 +265,8 @@ bool initialize(HeightFieldOcTreeCollisionTraversalNode& node, /// octree, given current object transform template bool initialize(MeshOcTreeDistanceTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const OcTree& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const OcTree& model2, const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; @@ -287,8 +287,8 @@ bool initialize(MeshOcTreeDistanceTraversalNode& node, /// mesh, given current object transform template bool initialize(OcTreeMeshDistanceTraversalNode& node, const OcTree& model1, - const Transform3f& tf1, const BVHModel& model2, - const Transform3f& tf2, const OcTreeSolver* otsolver, + const Transform3s& tf1, const BVHModel& model2, + const Transform3s& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; @@ -310,8 +310,8 @@ bool initialize(OcTreeMeshDistanceTraversalNode& node, const OcTree& model1, /// given current object transform template bool initialize(ShapeCollisionTraversalNode& node, const S1& shape1, - const Transform3f& tf1, const S2& shape2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf1, const S2& shape2, + const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { node.model1 = &shape1; node.tf1 = tf1; @@ -328,8 +328,8 @@ bool initialize(ShapeCollisionTraversalNode& node, const S1& shape1, /// shape, given current object transform template bool initialize(MeshShapeCollisionTraversalNode& node, - BVHModel& model1, Transform3f& tf1, const S& model2, - const Transform3f& tf2, const GJKSolver* nsolver, + BVHModel& model1, Transform3s& tf1, const S& model2, + const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) @@ -376,8 +376,8 @@ bool initialize(MeshShapeCollisionTraversalNode& node, /// shape template bool initialize(MeshShapeCollisionTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( @@ -405,8 +405,8 @@ bool initialize(MeshShapeCollisionTraversalNode& node, /// shape, given current object transform template bool initialize(HeightFieldShapeCollisionTraversalNode& node, - HeightField& model1, Transform3f& tf1, const S& model2, - const Transform3f& tf2, const GJKSolver* nsolver, + HeightField& model1, Transform3s& tf1, const S& model2, + const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false); @@ -414,8 +414,8 @@ bool initialize(HeightFieldShapeCollisionTraversalNode& node, /// shape template bool initialize(HeightFieldShapeCollisionTraversalNode& node, - const HeightField& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, + const HeightField& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { node.model1 = &model1; node.tf1 = tf1; @@ -434,8 +434,8 @@ bool initialize(HeightFieldShapeCollisionTraversalNode& node, namespace details { template class OrientedNode> static inline bool setupShapeMeshCollisionOrientedNode( - OrientedNode& node, const S& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, + OrientedNode& node, const S& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, const GJKSolver* nsolver, CollisionResult& result) { if (model2.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( @@ -466,8 +466,8 @@ static inline bool setupShapeMeshCollisionOrientedNode( template bool initialize( MeshCollisionTraversalNode& node, - BVHModel& model1, Transform3f& tf1, BVHModel& model2, - Transform3f& tf2, CollisionResult& result, bool use_refit = false, + BVHModel& model1, Transform3s& tf1, BVHModel& model2, + Transform3s& tf2, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( @@ -530,8 +530,8 @@ bool initialize( template bool initialize(MeshCollisionTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, CollisionResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( @@ -567,8 +567,8 @@ bool initialize(MeshCollisionTraversalNode& node, /// @brief Initialize traversal node for distance between two geometric shapes template bool initialize(ShapeDistanceTraversalNode& node, const S1& shape1, - const Transform3f& tf1, const S2& shape2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf1, const S2& shape2, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; @@ -587,8 +587,8 @@ bool initialize(ShapeDistanceTraversalNode& node, const S1& shape1, template bool initialize( MeshDistanceTraversalNode& node, - BVHModel& model1, Transform3f& tf1, BVHModel& model2, - Transform3f& tf2, const DistanceRequest& request, DistanceResult& result, + BVHModel& model1, Transform3s& tf1, BVHModel& model2, + Transform3s& tf2, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( @@ -653,8 +653,8 @@ bool initialize( /// @brief Initialize traversal node for distance computation between two meshes template bool initialize(MeshDistanceTraversalNode& node, - const BVHModel& model1, const Transform3f& tf1, - const BVHModel& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const BVHModel& model2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( @@ -696,8 +696,8 @@ bool initialize(MeshDistanceTraversalNode& node, /// and one shape, given the current transforms template bool initialize(MeshShapeDistanceTraversalNode& node, - BVHModel& model1, Transform3f& tf1, const S& model2, - const Transform3f& tf2, const GJKSolver* nsolver, + BVHModel& model1, Transform3s& tf1, const S& model2, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) @@ -744,8 +744,8 @@ namespace details { template class OrientedNode> static inline bool setupMeshShapeDistanceOrientedNode( - OrientedNode& node, const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, const GJKSolver* nsolver, + OrientedNode& node, const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (model1.getModelType() != BVH_MODEL_TRIANGLES) COAL_THROW_PRETTY( @@ -776,8 +776,8 @@ static inline bool setupMeshShapeDistanceOrientedNode( /// and one shape, specialized for RSS type template bool initialize(MeshShapeDistanceTraversalNodeRSS& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode( @@ -788,8 +788,8 @@ bool initialize(MeshShapeDistanceTraversalNodeRSS& node, /// and one shape, specialized for kIOS type template bool initialize(MeshShapeDistanceTraversalNodekIOS& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode( @@ -800,8 +800,8 @@ bool initialize(MeshShapeDistanceTraversalNodekIOS& node, /// and one shape, specialized for OBBRSS type template bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, - const BVHModel& model1, const Transform3f& tf1, - const S& model2, const Transform3f& tf2, + const BVHModel& model1, const Transform3s& tf1, + const S& model2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode( diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h index ff96bac1f..d2cf8ed41 100644 --- a/include/coal/math/transform.h +++ b/include/coal/math/transform.h @@ -52,7 +52,7 @@ static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) { } /// @brief Simple transform class used locally by InterpMotion -class COAL_DLLAPI Transform3f { +class COAL_DLLAPI Transform3s { /// @brief Matrix cache Matrix3s R; @@ -61,37 +61,37 @@ class COAL_DLLAPI Transform3f { public: /// @brief Default transform is no movement - Transform3f() { + Transform3s() { setIdentity(); // set matrix_set true } - static Transform3f Identity() { return Transform3f(); } + static Transform3s Identity() { return Transform3s(); } /// @brief Construct transform from rotation and translation template - Transform3f(const Eigen::MatrixBase& R_, + Transform3s(const Eigen::MatrixBase& R_, const Eigen::MatrixBase& T_) : R(R_), T(T_) {} /// @brief Construct transform from rotation and translation template - Transform3f(const Quatf& q_, const Eigen::MatrixBase& T_) + Transform3s(const Quatf& q_, const Eigen::MatrixBase& T_) : R(q_.toRotationMatrix()), T(T_) {} /// @brief Construct transform from rotation - Transform3f(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {} + Transform3s(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {} /// @brief Construct transform from rotation - Transform3f(const Quatf& q_) : R(q_), T(Vec3s::Zero()) {} + Transform3s(const Quatf& q_) : R(q_), T(Vec3s::Zero()) {} /// @brief Construct transform from translation - Transform3f(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {} + Transform3s(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {} /// @brief Construct transform from other transform - Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {} + Transform3s(const Transform3s& tf) : R(tf.R), T(tf.T) {} /// @brief operator = - Transform3f& operator=(const Transform3f& tf) { + Transform3s& operator=(const Transform3s& tf) { R = tf.R; T = tf.T; return *this; @@ -160,32 +160,32 @@ class COAL_DLLAPI Transform3f { } /// @brief inverse transform - inline Transform3f& inverseInPlace() { + inline Transform3s& inverseInPlace() { R.transposeInPlace(); T = -R * T; return *this; } /// @brief inverse transform - inline Transform3f inverse() { - return Transform3f(R.transpose(), -R.transpose() * T); + inline Transform3s inverse() { + return Transform3s(R.transpose(), -R.transpose() * T); } /// @brief inverse the transform and multiply with another - inline Transform3f inverseTimes(const Transform3f& other) const { - return Transform3f(R.transpose() * other.R, R.transpose() * (other.T - T)); + inline Transform3s inverseTimes(const Transform3s& other) const { + return Transform3s(R.transpose() * other.R, R.transpose() * (other.T - T)); } /// @brief multiply with another transform - inline const Transform3f& operator*=(const Transform3f& other) { + inline const Transform3s& operator*=(const Transform3s& other) { T += R * other.T; R *= other.R; return *this; } /// @brief multiply with another transform - inline Transform3f operator*(const Transform3f& other) const { - return Transform3f(R * other.R, R * other.T + T); + inline Transform3s operator*(const Transform3s& other) const { + return Transform3s(R * other.R, R * other.T + T); } /// @brief check whether the transform is identity @@ -202,16 +202,16 @@ class COAL_DLLAPI Transform3f { } /// @brief return a random transform - static Transform3f Random() { return Transform3f().setRandom(); } + static Transform3s Random() { return Transform3s().setRandom(); } /// @brief set the transform to a random transform - Transform3f& setRandom(); + Transform3s& setRandom(); - bool operator==(const Transform3f& other) const { + bool operator==(const Transform3s& other) const { return (R == other.getRotation()) && (T == other.getTranslation()); } - bool operator!=(const Transform3f& other) const { return !(*this == other); } + bool operator!=(const Transform3s& other) const { return !(*this == other); } EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -247,7 +247,7 @@ inline Quatf uniformRandomQuaternion() { return q; } -inline Transform3f& Transform3f::setRandom() { +inline Transform3s& Transform3s::setRandom() { const Quatf q = uniformRandomQuaternion(); this->rotation() = q.matrix(); this->translation().setRandom(); diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h index 3449301ab..1d9e36bfa 100644 --- a/include/coal/narrowphase/minkowski_difference.h +++ b/include/coal/narrowphase/minkowski_difference.h @@ -127,7 +127,7 @@ struct COAL_DLLAPI MinkowskiDiff { /// ShapeBase*)` for more details. template void set(const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1); + const Transform3s& tf0, const Transform3s& tf1); /// @brief support function for shape0. /// The output vector is expressed in the local frame of shape0. diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h index 03046b45e..65cd31b13 100644 --- a/include/coal/narrowphase/narrowphase.h +++ b/include/coal/narrowphase/narrowphase.h @@ -305,8 +305,8 @@ struct COAL_DLLAPI GJKSolver { /// It's up to the user to decide whether the shapes are in collision or not, /// based on that estimate. template - CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, + CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, const bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { constexpr bool relative_transformation_already_computed = false; @@ -320,11 +320,11 @@ struct COAL_DLLAPI GJKSolver { /// second shape is a triangle. It is more efficient to pre-compute the /// relative transformation between the two shapes before calling GJK/EPA. template - CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1, - const TriangleP& s2, const Transform3f& tf2, + CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1, + const TriangleP& s2, const Transform3s& tf2, const bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { - const Transform3f tf_1M2(tf1.inverseTimes(tf2)); + const Transform3s tf_1M2(tf1.inverseTimes(tf2)); TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b), tf_1M2.transform(s2.c)); @@ -337,8 +337,8 @@ struct COAL_DLLAPI GJKSolver { /// @brief See other partial template specialization of shapeDistance above. template - CoalScalar shapeDistance(const TriangleP& s1, const Transform3f& tf1, - const S2& s2, const Transform3f& tf2, + CoalScalar shapeDistance(const TriangleP& s1, const Transform3s& tf1, + const S2& s2, const Transform3s& tf2, const bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { CoalScalar distance = this->shapeDistance( @@ -420,8 +420,8 @@ struct COAL_DLLAPI GJKSolver { template void runGJKAndEPA( - const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, const bool compute_penetration, + const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, const bool compute_penetration, CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal, const bool relative_transformation_already_computed = false) const { // Reset internal state of GJK algorithm @@ -586,7 +586,7 @@ struct COAL_DLLAPI GJKSolver { } } - void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3f& tf1, + void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3s& tf1, CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { @@ -607,7 +607,7 @@ struct COAL_DLLAPI GJKSolver { // normal = tf1.getRotation() * normal; } - void GJKExtractWitnessPointsAndNormal(const Transform3f& tf1, + void GJKExtractWitnessPointsAndNormal(const Transform3s& tf1, CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { // Apart from early stopping, there are two cases where GJK says there is no @@ -634,7 +634,7 @@ struct COAL_DLLAPI GJKSolver { p2.noalias() = p + 0.5 * distance * normal; } - void GJKCollisionExtractWitnessPointsAndNormal(const Transform3f& tf1, + void GJKCollisionExtractWitnessPointsAndNormal(const Transform3s& tf1, CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { @@ -654,7 +654,7 @@ struct COAL_DLLAPI GJKSolver { Vec3s::Constant(std::numeric_limits::quiet_NaN()); } - void EPAExtractWitnessPointsAndNormal(const Transform3f& tf1, + void EPAExtractWitnessPointsAndNormal(const Transform3s& tf1, CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { // Cache EPA result for potential future call to this GJKSolver. @@ -709,7 +709,7 @@ struct COAL_DLLAPI GJKSolver { p2.noalias() = p + 0.5 * distance * normal; } - void EPAFailedExtractWitnessPointsAndNormal(const Transform3f& tf1, + void EPAFailedExtractWitnessPointsAndNormal(const Transform3s& tf1, CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal) const { this->cached_guess = Vec3s(1, 0, 0); diff --git a/include/coal/serialization/transform.h b/include/coal/serialization/transform.h index e2905e999..c43542d7a 100644 --- a/include/coal/serialization/transform.h +++ b/include/coal/serialization/transform.h @@ -12,7 +12,7 @@ namespace boost { namespace serialization { template -void serialize(Archive& ar, coal::Transform3f& tf, +void serialize(Archive& ar, coal::Transform3s& tf, const unsigned int /*version*/) { ar& make_nvp("R", tf.rotation()); ar& make_nvp("T", tf.translation()); diff --git a/include/coal/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h index f9a3aca8b..6f43e9d7d 100644 --- a/include/coal/shape/geometric_shape_to_BVH_model.h +++ b/include/coal/shape/geometric_shape_to_BVH_model.h @@ -47,7 +47,7 @@ namespace coal { /// @brief Generate BVH model from box template void generateBVHModel(BVHModel& model, const Box& shape, - const Transform3f& pose) { + const Transform3s& pose) { CoalScalar a = shape.halfSide[0]; CoalScalar b = shape.halfSide[1]; CoalScalar c = shape.halfSide[2]; @@ -89,7 +89,7 @@ void generateBVHModel(BVHModel& model, const Box& shape, /// longitude and number of rings along latitude. template void generateBVHModel(BVHModel& model, const Sphere& shape, - const Transform3f& pose, unsigned int seg, + const Transform3s& pose, unsigned int seg, unsigned int ring) { std::vector points; std::vector tri_indices; @@ -155,7 +155,7 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, /// single triangle is approximately the same.s template void generateBVHModel(BVHModel& model, const Sphere& shape, - const Transform3f& pose, + const Transform3s& pose, unsigned int n_faces_for_unit_sphere) { CoalScalar r = shape.radius; CoalScalar n_low_bound = @@ -170,7 +170,7 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, /// circle and the number of segments along axis. template void generateBVHModel(BVHModel& model, const Cylinder& shape, - const Transform3f& pose, unsigned int tot, + const Transform3s& pose, unsigned int tot, unsigned int h_num) { std::vector points; std::vector tri_indices; @@ -243,7 +243,7 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, /// number of circle split number is r * tot. template void generateBVHModel(BVHModel& model, const Cylinder& shape, - const Transform3f& pose, + const Transform3s& pose, unsigned int tot_for_unit_cylinder) { CoalScalar r = shape.radius; CoalScalar h = 2 * shape.halfLength; @@ -262,7 +262,7 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, /// circle and the number of segments along axis. template void generateBVHModel(BVHModel& model, const Cone& shape, - const Transform3f& pose, unsigned int tot, + const Transform3s& pose, unsigned int tot, unsigned int h_num) { std::vector points; std::vector tri_indices; @@ -335,7 +335,7 @@ void generateBVHModel(BVHModel& model, const Cone& shape, /// of circle split number is r * tot. template void generateBVHModel(BVHModel& model, const Cone& shape, - const Transform3f& pose, unsigned int tot_for_unit_cone) { + const Transform3s& pose, unsigned int tot_for_unit_cone) { CoalScalar r = shape.radius; CoalScalar h = 2 * shape.halfLength; diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h index e88a737e9..1050f692a 100644 --- a/include/coal/shape/geometric_shapes.h +++ b/include/coal/shape/geometric_shapes.h @@ -125,10 +125,10 @@ class COAL_DLLAPI TriangleP : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } - // std::pair inflated(const CoalScalar value) const + // std::pair inflated(const CoalScalar value) const // { // if (value == 0) return std::make_pair(new TriangleP(*this), - // Transform3f()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize(); + // Transform3s()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize(); // BC.normalize(); // CA.normalize(); // @@ -137,7 +137,7 @@ class COAL_DLLAPI TriangleP : public ShapeBase { // Vec3s new_c(c + value * Vec3s(-CA + BC).normalized()); // // return std::make_pair(new TriangleP(new_a, new_b, new_c), - // Transform3f()); + // Transform3s()); // } // // CoalScalar minInflationValue() const @@ -212,14 +212,14 @@ class COAL_DLLAPI Box : public ShapeBase { /// /// \returns a new inflated box and the related transform to account for the /// change of shape frame - std::pair inflated(const CoalScalar value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") " << "is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))), - Transform3f()); + Transform3s()); } private: @@ -278,13 +278,13 @@ class COAL_DLLAPI Sphere : public ShapeBase { /// /// \returns a new inflated sphere and the related transform to account for /// the change of shape frame - std::pair inflated(const CoalScalar value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair(Sphere(radius + value), Transform3f()); + return std::make_pair(Sphere(radius + value), Transform3s()); } private: @@ -352,14 +352,14 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase { /// /// \returns a new inflated ellipsoid and the related transform to account for /// the change of shape frame - std::pair inflated(const CoalScalar value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)), - Transform3f()); + Transform3s()); } private: @@ -437,14 +437,14 @@ class COAL_DLLAPI Capsule : public ShapeBase { /// /// \returns a new inflated capsule and the related transform to account for /// the change of shape frame - std::pair inflated(const CoalScalar value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Capsule(radius + value, 2 * halfLength), - Transform3f()); + Transform3s()); } private: @@ -519,7 +519,7 @@ class COAL_DLLAPI Cone : public ShapeBase { /// /// \returns a new inflated cone and the related transform to account for the /// change of shape frame - std::pair inflated(const CoalScalar value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " @@ -538,7 +538,7 @@ class COAL_DLLAPI Cone : public ShapeBase { const CoalScalar new_radius = new_lz / tan_alpha; return std::make_pair(Cone(new_radius, new_lz), - Transform3f(Vec3s(0., 0., new_cz))); + Transform3s(Vec3s(0., 0., new_cz))); } private: @@ -616,14 +616,14 @@ class COAL_DLLAPI Cylinder : public ShapeBase { /// /// \returns a new inflated cylinder and the related transform to account for /// the change of shape frame - std::pair inflated(const CoalScalar value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), - Transform3f()); + Transform3s()); } private: @@ -943,13 +943,13 @@ class COAL_DLLAPI Halfspace : public ShapeBase { /// /// \returns a new inflated halfspace and the related transform to account for /// the change of shape frame - std::pair inflated(const CoalScalar value) const { + std::pair inflated(const CoalScalar value) const { if (value <= minInflationValue()) COAL_THROW_PRETTY("value (" << value << ") is two small. It should be at least: " << minInflationValue(), std::invalid_argument); - return std::make_pair(Halfspace(n, d + value), Transform3f()); + return std::make_pair(Halfspace(n, d + value), Transform3s()); } /// @brief Plane normal diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h index 305008870..76ac78ae8 100644 --- a/include/coal/shape/geometric_shapes_utility.h +++ b/include/coal/shape/geometric_shapes_utility.h @@ -50,27 +50,27 @@ namespace details { /// @brief get the vertices of some convex shape which can bound the given shape /// in a specific configuration COAL_DLLAPI std::vector getBoundVertices(const Box& box, - const Transform3f& tf); + const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Sphere& sphere, - const Transform3f& tf); + const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Ellipsoid& ellipsoid, - const Transform3f& tf); + const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Capsule& capsule, - const Transform3f& tf); + const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Cone& cone, - const Transform3f& tf); + const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const Cylinder& cylinder, - const Transform3f& tf); + const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const ConvexBase& convex, - const Transform3f& tf); + const Transform3s& tf); COAL_DLLAPI std::vector getBoundVertices(const TriangleP& triangle, - const Transform3f& tf); + const Transform3s& tf); } // namespace details /// @endcond /// @brief calculate a bounding volume for a shape in a specific configuration template -inline void computeBV(const S& s, const Transform3f& tf, BV& bv) { +inline void computeBV(const S& s, const Transform3s& tf, BV& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -81,180 +81,180 @@ inline void computeBV(const S& s, const Transform3f& tf, BV& bv) { } template <> -COAL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Box& s, const Transform3s& tf, AABB& bv); template <> -COAL_DLLAPI void computeBV(const Sphere& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Sphere& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Ellipsoid& e, - const Transform3f& tf, AABB& bv); + const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Capsule& s, - const Transform3f& tf, AABB& bv); + const Transform3s& tf, AABB& bv); template <> -COAL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Cone& s, const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Cylinder& s, - const Transform3f& tf, AABB& bv); + const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const ConvexBase& s, - const Transform3f& tf, AABB& bv); + const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const TriangleP& s, - const Transform3f& tf, AABB& bv); + const Transform3s& tf, AABB& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, AABB& bv); + const Transform3s& tf, AABB& bv); template <> -COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, AABB& bv); template <> -COAL_DLLAPI void computeBV(const Box& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Box& s, const Transform3s& tf, OBB& bv); template <> -COAL_DLLAPI void computeBV(const Sphere& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Sphere& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Capsule& s, - const Transform3f& tf, OBB& bv); + const Transform3s& tf, OBB& bv); template <> -COAL_DLLAPI void computeBV(const Cone& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Cone& s, const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Cylinder& s, - const Transform3f& tf, OBB& bv); + const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const ConvexBase& s, - const Transform3f& tf, OBB& bv); + const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, OBB& bv); + const Transform3s& tf, OBB& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, RSS& bv); + const Transform3s& tf, RSS& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, + const Transform3s& tf, OBBRSS& bv); template <> COAL_DLLAPI void computeBV(const Halfspace& s, - const Transform3f& tf, kIOS& bv); + const Transform3s& tf, kIOS& bv); template <> COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, + const Transform3s& tf, KDOP<16>& bv); template <> COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, + const Transform3s& tf, KDOP<18>& bv); template <> COAL_DLLAPI void computeBV, Halfspace>(const Halfspace& s, - const Transform3f& tf, + const Transform3s& tf, KDOP<24>& bv); template <> -COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, OBB& bv); template <> -COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, RSS& bv); template <> -COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, OBBRSS& bv); template <> -COAL_DLLAPI void computeBV(const Plane& s, const Transform3f& tf, +COAL_DLLAPI void computeBV(const Plane& s, const Transform3s& tf, kIOS& bv); template <> COAL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, + const Transform3s& tf, KDOP<16>& bv); template <> COAL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, + const Transform3s& tf, KDOP<18>& bv); template <> COAL_DLLAPI void computeBV, Plane>(const Plane& s, - const Transform3f& tf, + const Transform3s& tf, KDOP<24>& bv); /// @brief construct a box shape (with a configuration) from a given bounding /// volume -COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const AABB& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const AABB& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf); +COAL_DLLAPI void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf); -COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf); +COAL_DLLAPI void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf); -COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); -COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, - Box& box, Transform3f& tf); +COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, + Box& box, Transform3s& tf); -COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3f& tf); +COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3s& tf); -COAL_DLLAPI Plane transform(const Plane& a, const Transform3f& tf); +COAL_DLLAPI Plane transform(const Plane& a, const Transform3s& tf); COAL_DLLAPI std::array transformToHalfspaces( - const Plane& a, const Transform3f& tf); + const Plane& a, const Transform3s& tf); } // namespace coal diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 63d160bbe..fe841d893 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -699,7 +699,7 @@ void exposeCollisionObject() { .def(dv::init>()) .def(dv::init>()) + const Transform3s&, bp::optional>()) .def(dv::init>()) @@ -721,7 +721,7 @@ void exposeCollisionObject() { bp::return_value_policy()) .def(dv::member_func( "setTransform", - static_cast( + static_cast( &CollisionObject::setTransform))) .DEF_CLASS_FUNC(CollisionObject, isIdentityTransform) diff --git a/python/collision.cc b/python/collision.cc index 12e596a10..2c92ad083 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -260,8 +260,8 @@ void exposeCollisionAPI() { const CollisionRequest&, CollisionResult&)>(&collide)); doxygen::def( "collide", - static_cast( &collide)); @@ -271,6 +271,6 @@ void exposeCollisionAPI() { const CollisionGeometry*>()) .def("__call__", static_cast(&ComputeCollision::operator())); } diff --git a/python/contact_patch.cc b/python/contact_patch.cc index 4721e8754..831137b74 100644 --- a/python/contact_patch.cc +++ b/python/contact_patch.cc @@ -140,8 +140,8 @@ void exposeContactPatchAPI() { ContactPatchResult&)>(&computeContactPatch)); doxygen::def( "computeContactPatch", - static_cast(&computeContactPatch)); @@ -154,7 +154,7 @@ void exposeContactPatchAPI() { const CollisionGeometry*>()) .def("__call__", static_cast( &ComputeContactPatch::operator())); } diff --git a/python/distance.cc b/python/distance.cc index d83df0143..1251428bb 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -154,8 +154,8 @@ void exposeDistanceAPI() { &distance)); doxygen::def( "distance", - static_cast( &distance)); @@ -165,6 +165,6 @@ void exposeDistanceAPI() { const CollisionGeometry*>()) .def("__call__", static_cast(&ComputeDistance::operator())); } diff --git a/python/gjk.cc b/python/gjk.cc index a098f2b15..cab5ebde1 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -81,8 +81,8 @@ struct MinkowskiDiffWrapper { } static void set(MinkowskiDiff& self, const ShapeBase* shape0, - const ShapeBase* shape1, const Transform3f& tf0, - const Transform3f& tf1, + const ShapeBase* shape1, const Transform3s& tf0, + const Transform3s& tf1, bool compute_swept_sphere_supports = false) { if (compute_swept_sphere_supports) { self.set(shape0, shape1, tf0, tf1); @@ -119,13 +119,13 @@ void exposeGJK() { .def("set", static_cast( + const ShapeBase*, const Transform3s&, + const Transform3s&, bool)>( &MinkowskiDiffWrapper::set), doxygen::member_func_doc( static_cast( + const ShapeBase*, const ShapeBase*, const Transform3s&, + const Transform3s&)>( &MinkowskiDiff::set))) .def("support0", &MinkowskiDiffWrapper::support0, diff --git a/python/math.cc b/python/math.cc index 119d29e04..be6d13dc4 100644 --- a/python/math.cc +++ b/python/math.cc @@ -77,53 +77,53 @@ void exposeMaths() { eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); - class_("Transform3f", doxygen::class_doc(), no_init) - .def(dv::init()) - .def(dv::init("Transform3s", doxygen::class_doc(), no_init) + .def(dv::init()) + .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - .def(dv::init()) - - .def(dv::member_func("getQuatRotation", &Transform3f::getQuatRotation)) - .def("getTranslation", &Transform3f::getTranslation, - doxygen::member_func_doc(&Transform3f::getTranslation), + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + .def(dv::init()) + + .def(dv::member_func("getQuatRotation", &Transform3s::getQuatRotation)) + .def("getTranslation", &Transform3s::getTranslation, + doxygen::member_func_doc(&Transform3s::getTranslation), return_value_policy()) - .def("getRotation", &Transform3f::getRotation, + .def("getRotation", &Transform3s::getRotation, return_value_policy()) - .def("isIdentity", &Transform3f::isIdentity, + .def("isIdentity", &Transform3s::isIdentity, (bp::arg("self"), bp::arg("prec") = Eigen::NumTraits::dummy_precision()), - doxygen::member_func_doc(&Transform3f::getTranslation)) + doxygen::member_func_doc(&Transform3s::getTranslation)) - .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) - .def("setTranslation", &Transform3f::setTranslation) - .def("setRotation", &Transform3f::setRotation) + .def(dv::member_func("setQuatRotation", &Transform3s::setQuatRotation)) + .def("setTranslation", &Transform3s::setTranslation) + .def("setRotation", &Transform3s::setRotation) .def(dv::member_func("setTransform", - &Transform3f::setTransform)) + &Transform3s::setTransform)) .def(dv::member_func( "setTransform", - static_cast( - &Transform3f::setTransform))) - .def(dv::member_func("setIdentity", &Transform3f::setIdentity)) - .def(dv::member_func("Identity", &Transform3f::Identity)) + static_cast( + &Transform3s::setTransform))) + .def(dv::member_func("setIdentity", &Transform3s::setIdentity)) + .def(dv::member_func("Identity", &Transform3s::Identity)) .staticmethod("Identity") - .def(dv::member_func("transform", &Transform3f::transform)) - .def("inverseInPlace", &Transform3f::inverseInPlace, + .def(dv::member_func("transform", &Transform3s::transform)) + .def("inverseInPlace", &Transform3s::inverseInPlace, return_internal_reference<>(), - doxygen::member_func_doc(&Transform3f::inverseInPlace)) - .def(dv::member_func("inverse", &Transform3f::inverse)) - .def(dv::member_func("inverseTimes", &Transform3f::inverseTimes)) + doxygen::member_func_doc(&Transform3s::inverseInPlace)) + .def(dv::member_func("inverse", &Transform3s::inverse)) + .def(dv::member_func("inverseTimes", &Transform3s::inverseTimes)) .def(self * self) .def(self *= self) .def(self == self) .def(self != self) - .def_pickle(PickleObject()) - .def(SerializableVisitor()); + .def_pickle(PickleObject()) + .def(SerializableVisitor()); class_("Triangle", no_init) .def(dv::init()) diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp index 5ce7c2ccd..d3a74960c 100644 --- a/src/BVH/BVH_utility.cpp +++ b/src/BVH/BVH_utility.cpp @@ -44,13 +44,13 @@ namespace coal { namespace details { template -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& _aabb) { assert(model.getModelType() == BVH_MODEL_TRIANGLES); const Matrix3s& q = pose.getRotation(); AABB aabb = translate(_aabb, -pose.getTranslation()); - Transform3f box_pose; + Transform3s box_pose; Box box; constructBox(_aabb, box, box_pose); box_pose = pose.inverseTimes(box_pose); @@ -89,7 +89,7 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, compute_penetration); DistanceResult distanceResult; const CoalScalar distance = ShapeShapeDistance( - &box, box_pose, &tri, Transform3f(), &gjk, distanceRequest, + &box, box_pose, &tri, Transform3s(), &gjk, distanceRequest, distanceResult); bool is_collision = (distance <= gjk.getDistancePrecision(compute_penetration)); @@ -139,43 +139,43 @@ BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, } // namespace details template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> -BVHModel* BVHExtract(const BVHModel& model, const Transform3f& pose, +BVHModel* BVHExtract(const BVHModel& model, const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel* BVHExtract(const BVHModel& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } template <> BVHModel >* BVHExtract(const BVHModel >& model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { return details::BVHExtract(model, pose, aabb); } diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 05ac31513..755d2763f 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -55,7 +55,7 @@ namespace dynamic_AABB_tree { //============================================================================== bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (!root2) { if (root1->isLeaf()) { @@ -63,12 +63,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); @@ -92,12 +92,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); @@ -112,7 +112,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, } OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; @@ -148,12 +148,12 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, CoalScalar& min_dist) { if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, @@ -223,7 +223,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (tf2.rotation().isIdentity()) return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), @@ -235,7 +235,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, //============================================================================== bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, CoalScalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(), diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 95f440c65..9306fa8eb 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -50,7 +50,7 @@ namespace dynamic_AABB_tree_array { bool collisionRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; @@ -60,12 +60,12 @@ bool collisionRecurse_( if (!obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); @@ -88,12 +88,12 @@ bool collisionRecurse_( CollisionObject* obj1 = static_cast(root1->data); if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (obb1.overlap(obb2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); @@ -108,7 +108,7 @@ bool collisionRecurse_( } OBB obb1, obb2; - convertBV(root1->bv, Transform3f::Identity(), obb1); + convertBV(root1->bv, Transform3s::Identity(), obb1); convertBV(root2_bv, tf2, obb2); if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; @@ -148,14 +148,14 @@ bool collisionRecurse_( bool distanceRecurse_( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, CoalScalar& min_dist) { DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = nodes1 + root1_id; if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if (tree2->isNodeOccupied(root2)) { Box* box = new Box(); - Transform3f box_tf; + Transform3s box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(shared_ptr(box), box_tf); return (*callback)(static_cast(root1->data), &obj, @@ -447,7 +447,7 @@ bool selfDistanceRecurse( bool collisionRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, CollisionCallBackBase* callback) { if (tf2.rotation().isIdentity()) return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, @@ -461,7 +461,7 @@ bool collisionRecurse( bool distanceRecurse( DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, - const AABB& root2_bv, const Transform3f& tf2, + const AABB& root2_bv, const Transform3s& tf2, DistanceCallBackBase* callback, CoalScalar& min_dist) { if (tf2.rotation().isIdentity()) return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, diff --git a/src/collision.cpp b/src/collision.cpp index 57b923045..95085f3be 100644 --- a/src/collision.cpp +++ b/src/collision.cpp @@ -65,8 +65,8 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, result); } -std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { // If security margin is set to -infinity, return that there is no collision if (request.security_margin == -std::numeric_limits::infinity()) { @@ -156,8 +156,8 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1, func = looktable.collision_matrix[node_type1][node_type2]; } -std::size_t ComputeCollision::run(const Transform3f& tf1, - const Transform3f& tf2, +std::size_t ComputeCollision::run(const Transform3s& tf1, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const { // If security margin is set to -infinity, return that there is no collision @@ -183,8 +183,8 @@ std::size_t ComputeCollision::run(const Transform3f& tf1, return res; } -std::size_t ComputeCollision::operator()(const Transform3f& tf1, - const Transform3f& tf2, +std::size_t ComputeCollision::operator()(const Transform3s& tf1, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) const diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 030711cb6..e30a2c510 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -49,8 +49,8 @@ namespace coal { #ifdef COAL_HAS_OCTOMAP template -std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { @@ -99,9 +99,9 @@ template ::Options> struct COAL_LOCAL BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -118,9 +118,9 @@ struct COAL_LOCAL BVHShapeCollider { } static std::size_t aligned(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -130,7 +130,7 @@ struct COAL_LOCAL BVHShapeCollider { node(request); const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); @@ -141,9 +141,9 @@ struct COAL_LOCAL BVHShapeCollider { } static std::size_t oriented(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -168,9 +168,9 @@ struct COAL_LOCAL HeightFieldShapeCollider { typedef HeightField HF; static std::size_t collide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -189,9 +189,9 @@ struct COAL_LOCAL HeightFieldShapeCollider { namespace details { template std::size_t orientedMeshCollide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -209,8 +209,8 @@ std::size_t orientedMeshCollide(const CollisionGeometry* o1, } // namespace details template -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); @@ -226,9 +226,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); - Transform3f tf2_tmp = tf2; + Transform3s tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, result); coal::collide(&node, request, result); @@ -240,8 +240,8 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, } template <> -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( @@ -250,9 +250,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, template <> std::size_t BVHCollide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( @@ -261,9 +261,9 @@ std::size_t BVHCollide(const CollisionGeometry* o1, template <> std::size_t BVHCollide(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide( @@ -271,8 +271,8 @@ std::size_t BVHCollide(const CollisionGeometry* o1, } template -std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* /*nsolver*/, const CollisionRequest& request, CollisionResult& result) { diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp index 8fe84680e..52839d100 100644 --- a/src/collision_utility.cpp +++ b/src/collision_utility.cpp @@ -22,7 +22,7 @@ namespace details { template inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, - const Transform3f& pose, + const Transform3s& pose, const AABB& aabb) { // Ensure AABB is already computed if (model->aabb_radius < 0) @@ -39,7 +39,7 @@ inline CollisionGeometry* extractBVHtpl(const CollisionGeometry* model, } CollisionGeometry* extractBVH(const CollisionGeometry* model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { switch (model->getNodeType()) { case BV_AABB: return extractBVHtpl(model, pose, aabb); @@ -64,7 +64,7 @@ CollisionGeometry* extractBVH(const CollisionGeometry* model, } // namespace details CollisionGeometry* extract(const CollisionGeometry* model, - const Transform3f& pose, const AABB& aabb) { + const Transform3s& pose, const AABB& aabb) { switch (model->getObjectType()) { case OT_BVH: return details::extractBVH(model, pose, aabb); diff --git a/src/contact_patch.cpp b/src/contact_patch.cpp index b6e0e1924..bfc8c7c7d 100644 --- a/src/contact_patch.cpp +++ b/src/contact_patch.cpp @@ -44,8 +44,8 @@ ContactPatchFunctionMatrix& getContactPatchFunctionLookTable() { return table; } -void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +void computeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) { @@ -136,7 +136,7 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1, } } -void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2, +void ComputeContactPatch::run(const Transform3s& tf1, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const { @@ -157,8 +157,8 @@ void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2, } } -void ComputeContactPatch::operator()(const Transform3f& tf1, - const Transform3f& tf2, +void ComputeContactPatch::operator()(const Transform3s& tf1, + const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchRequest& request, ContactPatchResult& result) const diff --git a/src/contact_patch_func_matrix.cpp b/src/contact_patch_func_matrix.cpp index e20305b37..aec69f679 100644 --- a/src/contact_patch_func_matrix.cpp +++ b/src/contact_patch_func_matrix.cpp @@ -43,8 +43,8 @@ namespace coal { template struct BVHShapeComputeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, @@ -68,8 +68,8 @@ struct BVHShapeComputeContactPatch { template struct HeightFieldShapeComputeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, @@ -93,8 +93,8 @@ struct HeightFieldShapeComputeContactPatch { template struct BVHComputeContactPatch { - static void run(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + static void run(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const CollisionResult& collision_result, const ContactPatchSolver* csolver, const ContactPatchRequest& request, @@ -117,8 +117,8 @@ struct BVHComputeContactPatch { }; COAL_LOCAL void contact_patch_function_not_implemented( - const CollisionGeometry* o1, const Transform3f& /*tf1*/, - const CollisionGeometry* o2, const Transform3f& /*tf2*/, + const CollisionGeometry* o1, const Transform3s& /*tf1*/, + const CollisionGeometry* o2, const Transform3s& /*tf2*/, const CollisionResult& /*collision_result*/, const ContactPatchSolver* /*csolver*/, const ContactPatchRequest& /*request*/, ContactPatchResult& /*result*/) { diff --git a/src/distance.cpp b/src/distance.cpp index 97dc1f5e5..42cc52e09 100644 --- a/src/distance.cpp +++ b/src/distance.cpp @@ -56,8 +56,8 @@ CoalScalar distance(const CollisionObject* o1, const CollisionObject* o2, result); } -CoalScalar distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +CoalScalar distance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { GJKSolver solver(request); @@ -135,7 +135,7 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1, func = looktable.distance_matrix[node_type1][node_type2]; } -CoalScalar ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, +CoalScalar ComputeDistance::run(const Transform3s& tf1, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const { CoalScalar res; @@ -156,8 +156,8 @@ CoalScalar ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2, return res; } -CoalScalar ComputeDistance::operator()(const Transform3f& tf1, - const Transform3f& tf2, +CoalScalar ComputeDistance::operator()(const Transform3s& tf1, + const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) const { solver.set(request); diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp index 8a4c3a185..913f626e3 100644 --- a/src/distance/box_halfspace.cpp +++ b/src/distance/box_halfspace.cpp @@ -49,8 +49,8 @@ namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -62,8 +62,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp index cdcde09e2..b3cb6015e 100644 --- a/src/distance/box_plane.cpp +++ b/src/distance/box_plane.cpp @@ -48,9 +48,9 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); @@ -63,9 +63,9 @@ CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, template <> CoalScalar ShapeShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); diff --git a/src/distance/box_sphere.cpp b/src/distance/box_sphere.cpp index 4c249b40c..0b6dbfb24 100644 --- a/src/distance/box_sphere.cpp +++ b/src/distance/box_sphere.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Box& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); @@ -58,8 +58,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Box& s2 = static_cast(*o2); diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp index ad5a2c051..0ec5137f6 100644 --- a/src/distance/capsule_capsule.cpp +++ b/src/distance/capsule_capsule.cpp @@ -78,8 +78,8 @@ void clamped_linear(Vec3s& a_sd, const Vec3s& a, const CoalScalar& s_n, /// @param normal: normal pointing from capsule1 to capsule2 template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& wp1, Vec3s& wp2, Vec3s& normal) { const Capsule* capsule1 = static_cast(o1); const Capsule* capsule2 = static_cast(o2); diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp index 16b42d259..70c53b918 100644 --- a/src/distance/capsule_halfspace.cpp +++ b/src/distance/capsule_halfspace.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp index 2d587cfb7..27fe19778 100644 --- a/src/distance/capsule_plane.cpp +++ b/src/distance/capsule_plane.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp index 10159a989..38276b539 100644 --- a/src/distance/cone_halfspace.cpp +++ b/src/distance/cone_halfspace.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cone& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp index 9e905bc36..b26832a96 100644 --- a/src/distance/cone_plane.cpp +++ b/src/distance/cone_plane.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cone& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Cone& s2 = static_cast(*o2); diff --git a/src/distance/convex_halfspace.cpp b/src/distance/convex_halfspace.cpp index 274632596..bda32965d 100644 --- a/src/distance/convex_halfspace.cpp +++ b/src/distance/convex_halfspace.cpp @@ -44,8 +44,8 @@ namespace coal { namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const ConvexBase& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -57,8 +57,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp index a4907880d..c89b57c7f 100644 --- a/src/distance/convex_plane.cpp +++ b/src/distance/convex_plane.cpp @@ -44,8 +44,8 @@ namespace coal { namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const ConvexBase& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -57,8 +57,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const ConvexBase& s2 = static_cast(*o2); diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp index bdff1a2ed..c29e6f7ca 100644 --- a/src/distance/cylinder_halfspace.cpp +++ b/src/distance/cylinder_halfspace.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp index 807d3bf99..2d3db927c 100644 --- a/src/distance/cylinder_plane.cpp +++ b/src/distance/cylinder_plane.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp index 9c2674dc6..d69e3db97 100644 --- a/src/distance/ellipsoid_halfspace.cpp +++ b/src/distance/ellipsoid_halfspace.cpp @@ -46,8 +46,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Ellipsoid& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -59,8 +59,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp index 4cc31f3aa..eae31b844 100644 --- a/src/distance/ellipsoid_plane.cpp +++ b/src/distance/ellipsoid_plane.cpp @@ -45,8 +45,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Ellipsoid& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -58,8 +58,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Ellipsoid& s2 = static_cast(*o2); diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp index e1fa824fd..9bad0a596 100644 --- a/src/distance/halfspace_halfspace.cpp +++ b/src/distance/halfspace_halfspace.cpp @@ -45,8 +45,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp index 7fdf4bf98..b0bd3c079 100644 --- a/src/distance/halfspace_plane.cpp +++ b/src/distance/halfspace_plane.cpp @@ -45,8 +45,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -55,8 +55,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp index 5598e92f1..b14bb3099 100644 --- a/src/distance/plane_plane.cpp +++ b/src/distance/plane_plane.cpp @@ -45,8 +45,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); diff --git a/src/distance/sphere_capsule.cpp b/src/distance/sphere_capsule.cpp index b2b79d7aa..719041b6e 100644 --- a/src/distance/sphere_capsule.cpp +++ b/src/distance/sphere_capsule.cpp @@ -45,8 +45,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Capsule& s2 = static_cast(*o2); @@ -55,8 +55,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Capsule& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp index 6844c201f..743989f19 100644 --- a/src/distance/sphere_cylinder.cpp +++ b/src/distance/sphere_cylinder.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Cylinder& s2 = static_cast(*o2); @@ -58,8 +58,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Cylinder& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp index 632836570..9a6fbe305 100644 --- a/src/distance/sphere_halfspace.cpp +++ b/src/distance/sphere_halfspace.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp index d423786eb..21438a549 100644 --- a/src/distance/sphere_plane.cpp +++ b/src/distance/sphere_plane.cpp @@ -48,8 +48,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -61,8 +61,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index 6546ab183..ac72a8c74 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -54,8 +54,8 @@ struct GJKSolver; namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp index 8f4b0892d..5f4da4807 100644 --- a/src/distance/triangle_halfspace.cpp +++ b/src/distance/triangle_halfspace.cpp @@ -44,8 +44,8 @@ namespace coal { namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Halfspace& s2 = static_cast(*o2); @@ -57,8 +57,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Halfspace& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp index 8db569fb8..87a90ae5d 100644 --- a/src/distance/triangle_plane.cpp +++ b/src/distance/triangle_plane.cpp @@ -44,8 +44,8 @@ namespace coal { namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Plane& s2 = static_cast(*o2); @@ -57,8 +57,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Plane& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp index 278e54db3..2bcb6beb9 100644 --- a/src/distance/triangle_sphere.cpp +++ b/src/distance/triangle_sphere.cpp @@ -44,8 +44,8 @@ namespace coal { namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const TriangleP& s1 = static_cast(*o1); const Sphere& s2 = static_cast(*o2); @@ -57,8 +57,8 @@ CoalScalar ShapeShapeDistance( template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver*, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Sphere& s1 = static_cast(*o1); const TriangleP& s2 = static_cast(*o2); diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp index 9f3166642..ccbe2b7f9 100644 --- a/src/distance/triangle_triangle.cpp +++ b/src/distance/triangle_triangle.cpp @@ -44,8 +44,8 @@ namespace coal { namespace internal { template <> CoalScalar ShapeShapeDistance( - const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) { // Transform the triangles in world frame const TriangleP& s1 = static_cast(*o1); diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index 5b482f298..fbfeb6640 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -48,8 +48,8 @@ namespace coal { #ifdef COAL_HAS_OCTOMAP template -CoalScalar Distance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +CoalScalar Distance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; @@ -67,8 +67,8 @@ CoalScalar Distance(const CollisionGeometry* o1, const Transform3f& tf1, #endif COAL_LOCAL CoalScalar distance_function_not_implemented( - const CollisionGeometry* o1, const Transform3f& /*tf1*/, - const CollisionGeometry* o2, const Transform3f& /*tf2*/, + const CollisionGeometry* o1, const Transform3s& /*tf1*/, + const CollisionGeometry* o2, const Transform3s& /*tf2*/, const GJKSolver* /*nsolver*/, const DistanceRequest& /*request*/, DistanceResult& /*result*/) { NODE_TYPE node_type1 = o1->getNodeType(); @@ -85,16 +85,16 @@ COAL_LOCAL CoalScalar distance_function_not_implemented( template struct COAL_LOCAL BVHShapeDistancer { static CoalScalar distance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; MeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); @@ -110,9 +110,9 @@ namespace details { template CoalScalar orientedBVHShapeDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { @@ -132,9 +132,9 @@ CoalScalar orientedBVHShapeDistance(const CollisionGeometry* o1, template struct COAL_LOCAL BVHShapeDistancer { static CoalScalar distance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance< @@ -146,9 +146,9 @@ struct COAL_LOCAL BVHShapeDistancer { template struct COAL_LOCAL BVHShapeDistancer { static CoalScalar distance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance< @@ -160,9 +160,9 @@ struct COAL_LOCAL BVHShapeDistancer { template struct COAL_LOCAL BVHShapeDistancer { static CoalScalar distance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance< @@ -174,9 +174,9 @@ struct COAL_LOCAL BVHShapeDistancer { template struct COAL_LOCAL HeightFieldShapeDistancer { static CoalScalar distance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, + const Transform3s& tf2, const GJKSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { COAL_UNUSED_VARIABLE(o1); @@ -203,17 +203,17 @@ struct COAL_LOCAL HeightFieldShapeDistancer { }; template -CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; MeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); - Transform3f tf2_tmp = tf2; + Transform3s tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); distance(&node); @@ -226,9 +226,9 @@ CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, namespace details { template CoalScalar orientedMeshDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { if (request.isSatisfied(result)) return result.min_distance; @@ -245,8 +245,8 @@ CoalScalar orientedMeshDistance(const CollisionGeometry* o1, } // namespace details template <> -CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance( @@ -255,9 +255,9 @@ CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, template <> CoalScalar BVHDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance( @@ -266,9 +266,9 @@ CoalScalar BVHDistance(const CollisionGeometry* o1, template <> CoalScalar BVHDistance(const CollisionGeometry* o1, - const Transform3f& tf1, + const Transform3s& tf1, const CollisionGeometry* o2, - const Transform3f& tf2, + const Transform3s& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance( @@ -276,8 +276,8 @@ CoalScalar BVHDistance(const CollisionGeometry* o1, } template -CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, +CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, const GJKSolver* /*nsolver*/, const DistanceRequest& request, DistanceResult& result) { return BVHDistance(o1, tf1, o2, tf2, request, result); diff --git a/src/intersect.cpp b/src/intersect.cpp index 6440ab5da..cd1320dd3 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -395,7 +395,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], } CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3], - const Transform3f& tf, Vec3s& P, + const Transform3s& tf, Vec3s& P, Vec3s& Q) { Vec3s T_transformed[3]; T_transformed[0] = tf.transform(T[0]); @@ -420,7 +420,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2, const Vec3s& S3, const Vec3s& T1, const Vec3s& T2, const Vec3s& T3, - const Transform3f& tf, Vec3s& P, + const Transform3s& tf, Vec3s& P, Vec3s& Q) { Vec3s T1_transformed = tf.transform(T1); Vec3s T2_transformed = tf.transform(T2); diff --git a/src/math/transform.cpp b/src/math/transform.cpp index 844c9c183..502b22ab1 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -39,15 +39,15 @@ namespace coal { -void relativeTransform(const Transform3f& tf1, const Transform3f& tf2, - Transform3f& tf) { +void relativeTransform(const Transform3s& tf1, const Transform3s& tf2, + Transform3s& tf) { tf = tf1.inverseTimes(tf2); } -void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2, - Transform3f& tf) { +void relativeTransform2(const Transform3s& tf1, const Transform3s& tf2, + Transform3s& tf) { Matrix3s R(tf2.getRotation() * tf1.getRotation().transpose()); - tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation()); + tf = Transform3s(R, tf2.getTranslation() - R * tf1.getTranslation()); } } // namespace coal diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h index b6a8b4ae0..628953b02 100644 --- a/src/narrowphase/details.h +++ b/src/narrowphase/details.h @@ -73,9 +73,9 @@ static inline void lineSegmentPointClosestToPoint(const Vec3s& p, /// @param normal pointing from shape 1 to shape 2 (sphere to capsule). /// @return the distance between the two shapes (negative if penetration). inline CoalScalar sphereCapsuleDistance(const Sphere& s1, - const Transform3f& tf1, + const Transform3s& tf1, const Capsule& s2, - const Transform3f& tf2, Vec3s& p1, + const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength))); Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength))); @@ -106,9 +106,9 @@ inline CoalScalar sphereCapsuleDistance(const Sphere& s1, /// @param normal pointing from shape 1 to shape 2 (sphere to cylinder). /// @return the distance between the two shapes (negative if penetration). inline CoalScalar sphereCylinderDistance(const Sphere& s1, - const Transform3f& tf1, + const Transform3s& tf1, const Cylinder& s2, - const Transform3f& tf2, Vec3s& p1, + const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { static const CoalScalar eps(sqrt(std::numeric_limits::epsilon())); CoalScalar r1(s1.radius); @@ -214,8 +214,8 @@ inline CoalScalar sphereCylinderDistance(const Sphere& s1, /// @param p2 witness point on the second Sphere. /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two spheres (negative if penetration). -inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, - const Sphere& s2, const Transform3f& tf2, +inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3s& tf1, + const Sphere& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const coal::Vec3s& center1 = tf1.getTranslation(); const coal::Vec3s& center2 = tf2.getTranslation(); @@ -286,9 +286,9 @@ inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3, /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2). /// @return the distance between the two shapes (negative if penetration). inline CoalScalar sphereTriangleDistance(const Sphere& s, - const Transform3f& tf1, + const Transform3s& tf1, const TriangleP& tri, - const Transform3f& tf2, Vec3s& p1, + const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { const Vec3s& P1 = tf2.transform(tri.a); const Vec3s& P2 = tf2.transform(tri.b); @@ -347,8 +347,8 @@ inline CoalScalar sphereTriangleDistance(const Sphere& s, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). -inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1, - const ShapeBase& s, const Transform3f& tf2, +inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3s& tf1, + const ShapeBase& s, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the halfspace normal is // parallel to the shape's surface (every primitive except sphere and @@ -381,8 +381,8 @@ inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1, /// @param p2 closest (or most penetrating) point on the shape, /// @param normal the halfspace normal. /// @return the distance between the two shapes (negative if penetration). -inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1, - const ShapeBase& s, const Transform3f& tf2, +inline CoalScalar planeDistance(const Plane& plane, const Transform3s& tf1, + const ShapeBase& s, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { // TODO(louis): handle multiple contact points when the plane normal is // parallel to the shape's surface (every primitive except sphere and @@ -435,8 +435,8 @@ inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1, /// @param ps the witness point on the sphere. /// @param normal pointing from box to sphere /// @return the distance between the two shapes (negative if penetration). -inline CoalScalar boxSphereDistance(const Box& b, const Transform3f& tfb, - const Sphere& s, const Transform3f& tfs, +inline CoalScalar boxSphereDistance(const Box& b, const Transform3s& tfb, + const Sphere& s, const Transform3s& tfs, Vec3s& pb, Vec3s& ps, Vec3s& normal) { const Vec3s& os = tfs.getTranslation(); const Vec3s& ob = tfb.getTranslation(); @@ -510,9 +510,9 @@ inline CoalScalar boxSphereDistance(const Box& b, const Transform3f& tfb, /// intersection line between the objects. The normal is the direction of this /// line. inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1, - const Transform3f& tf1, + const Transform3s& tf1, const Halfspace& s2, - const Transform3f& tf2, Vec3s& p1, + const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Halfspace new_s2 = transform(s2, tf2); @@ -586,9 +586,9 @@ inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1, /// intersection line between the objects. The normal is the direction of this /// line. inline CoalScalar halfspacePlaneDistance(const Halfspace& s1, - const Transform3f& tf1, + const Transform3s& tf1, const Plane& s2, - const Transform3f& tf2, Vec3s& p1, + const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Halfspace new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); @@ -647,8 +647,8 @@ inline CoalScalar halfspacePlaneDistance(const Halfspace& s1, /// The points p1 and p2 are the same point and represent the origin of the /// intersection line between the objects. The normal is the direction of this /// line. -inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3f& tf1, - const Plane& s2, const Transform3f& tf2, +inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3s& tf1, + const Plane& s2, const Transform3s& tf2, Vec3s& p1, Vec3s& p2, Vec3s& normal) { Plane new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); @@ -722,8 +722,8 @@ inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2, inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2, const Vec3s& P3, const Vec3s& Q1, const Vec3s& Q2, const Vec3s& Q3, - const Transform3f& tf1, - const Transform3f& tf2, Vec3s& normal) { + const Transform3s& tf1, + const Transform3s& tf2, Vec3s& normal) { Vec3s globalP1(tf1.transform(P1)); Vec3s globalP2(tf1.transform(P2)); Vec3s globalP3(tf1.transform(P3)); diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp index baa3f4dc9..97c90bb57 100644 --- a/src/narrowphase/minkowski_difference.cpp +++ b/src/narrowphase/minkowski_difference.cpp @@ -267,7 +267,7 @@ void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0, // ============================================================================ template void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, - const Transform3f& tf0, const Transform3f& tf1) { + const Transform3s& tf0, const Transform3s& tf1) { shapes[0] = shape0; shapes[1] = shape1; getNormalizeSupportDirectionFromShapes(shape0, shape1, @@ -303,9 +303,9 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { shape0, shape1, true, swept_sphere_radius, data); } // clang-format off -template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); -template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3f&, const Transform3f&); +template void COAL_DLLAPI MinkowskiDiff::set(const ShapeBase*, const ShapeBase*, const Transform3s&, const Transform3s&); // clang-format on // ============================================================================ diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp index 64be0e0cc..a900999d3 100644 --- a/src/narrowphase/support_functions.cpp +++ b/src/narrowphase/support_functions.cpp @@ -569,7 +569,7 @@ void getShapeSupportSet(const Box* box, SupportSet& support_set, SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); - const Transform3f& tf = support_set.tf; + const Transform3s& tf = support_set.tf; for (const Vec3s& corner : corners) { const CoalScalar val = corner.dot(support_dir); if (support_value - val < tol) { @@ -813,7 +813,7 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set, const std::vector& points = *(convex->points); SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); - const Transform3f& tf = support_set.tf; + const Transform3s& tf = support_set.tf; for (const Vec3s& point : points) { const CoalScalar dot = support_dir.dot(point); if (support_value - dot <= tol) { @@ -840,7 +840,7 @@ void convexSupportSetRecurse( const std::vector& neighbors, const CoalScalar swept_sphere_radius, const size_t vertex_idx, const Vec3s& support_dir, const CoalScalar support_value, - const Transform3f& tf, std::vector& visited, + const Transform3s& tf, std::vector& visited, SupportSet::Polygon& polygon, CoalScalar tol) { COAL_UNUSED_VARIABLE(swept_sphere_radius); @@ -895,7 +895,7 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set, SupportSet::Polygon& polygon = support_data.polygon; polygon.clear(); - const Transform3f& tf = support_set.tf; + const Transform3s& tf = support_set.tf; const size_t vertex_idx = (size_t)(hint); convexSupportSetRecurse<_SupportOptions>( diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp index ab6c04a73..cea609b3f 100644 --- a/src/shape/geometric_shapes.cpp +++ b/src/shape/geometric_shapes.cpp @@ -142,7 +142,7 @@ void Plane::unitNormalTest() { } void Box::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -153,7 +153,7 @@ void Box::computeLocalAABB() { } void Sphere::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -164,7 +164,7 @@ void Sphere::computeLocalAABB() { } void Ellipsoid::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -175,7 +175,7 @@ void Ellipsoid::computeLocalAABB() { } void Capsule::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -186,7 +186,7 @@ void Capsule::computeLocalAABB() { } void Cone::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -197,7 +197,7 @@ void Cone::computeLocalAABB() { } void Cylinder::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -208,7 +208,7 @@ void Cylinder::computeLocalAABB() { } void ConvexBase::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -219,7 +219,7 @@ void ConvexBase::computeLocalAABB() { } void Halfspace::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -230,7 +230,7 @@ void Halfspace::computeLocalAABB() { } void Plane::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); @@ -241,7 +241,7 @@ void Plane::computeLocalAABB() { } void TriangleP::computeLocalAABB() { - computeBV(*this, Transform3f(), aabb_local); + computeBV(*this, Transform3s(), aabb_local); const CoalScalar ssr = this->getSweptSphereRadius(); if (ssr > 0) { aabb_local.min_ -= Vec3s::Constant(ssr); diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index 119607292..a687a8e30 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -43,7 +43,7 @@ namespace coal { namespace details { -std::vector getBoundVertices(const Box& box, const Transform3f& tf) { +std::vector getBoundVertices(const Box& box, const Transform3s& tf) { std::vector result(8); CoalScalar a = box.halfSide[0]; CoalScalar b = box.halfSide[1]; @@ -62,7 +62,7 @@ std::vector getBoundVertices(const Box& box, const Transform3f& tf) { // we use icosahedron to bound the sphere std::vector getBoundVertices(const Sphere& sphere, - const Transform3f& tf) { + const Transform3s& tf) { std::vector result(12); const CoalScalar m = (1 + sqrt(5.0)) / 2.0; CoalScalar edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); @@ -87,7 +87,7 @@ std::vector getBoundVertices(const Sphere& sphere, // we use scaled icosahedron to bound the ellipsoid std::vector getBoundVertices(const Ellipsoid& ellipsoid, - const Transform3f& tf) { + const Transform3s& tf) { std::vector result(12); const CoalScalar phi = (1 + sqrt(5.0)) / 2.0; @@ -121,7 +121,7 @@ std::vector getBoundVertices(const Ellipsoid& ellipsoid, } std::vector getBoundVertices(const Capsule& capsule, - const Transform3f& tf) { + const Transform3s& tf) { std::vector result(36); const CoalScalar m = (1 + sqrt(5.0)) / 2.0; @@ -176,7 +176,7 @@ std::vector getBoundVertices(const Capsule& capsule, return result; } -std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { +std::vector getBoundVertices(const Cone& cone, const Transform3s& tf) { std::vector result(7); CoalScalar hl = cone.halfLength; @@ -197,7 +197,7 @@ std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { } std::vector getBoundVertices(const Cylinder& cylinder, - const Transform3f& tf) { + const Transform3s& tf) { std::vector result(12); CoalScalar hl = cylinder.halfLength; @@ -223,7 +223,7 @@ std::vector getBoundVertices(const Cylinder& cylinder, } std::vector getBoundVertices(const ConvexBase& convex, - const Transform3f& tf) { + const Transform3s& tf) { std::vector result(convex.num_points); const std::vector& points_ = *(convex.points); for (std::size_t i = 0; i < convex.num_points; ++i) { @@ -234,7 +234,7 @@ std::vector getBoundVertices(const ConvexBase& convex, } std::vector getBoundVertices(const TriangleP& triangle, - const Transform3f& tf) { + const Transform3s& tf) { std::vector result(3); result[0] = tf.transform(triangle.a); result[1] = tf.transform(triangle.b); @@ -245,7 +245,7 @@ std::vector getBoundVertices(const TriangleP& triangle, } // namespace details -Halfspace transform(const Halfspace& a, const Transform3f& tf) { +Halfspace transform(const Halfspace& a, const Transform3s& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' @@ -260,7 +260,7 @@ Halfspace transform(const Halfspace& a, const Transform3f& tf) { return result; } -Plane transform(const Plane& a, const Transform3f& tf) { +Plane transform(const Plane& a, const Transform3s& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' @@ -276,7 +276,7 @@ Plane transform(const Plane& a, const Transform3f& tf) { } std::array transformToHalfspaces(const Plane& a, - const Transform3f& tf) { + const Transform3s& tf) { // A plane can be represented by two halfspaces Vec3s n = tf.getRotation() * a.n; @@ -289,7 +289,7 @@ std::array transformToHalfspaces(const Plane& a, } template <> -void computeBV(const Box& s, const Transform3f& tf, AABB& bv) { +void computeBV(const Box& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); @@ -299,7 +299,7 @@ void computeBV(const Box& s, const Transform3f& tf, AABB& bv) { } template <> -void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) { +void computeBV(const Sphere& s, const Transform3s& tf, AABB& bv) { const Vec3s& T = tf.getTranslation(); Vec3s v_delta(Vec3s::Constant(s.radius)); @@ -308,7 +308,7 @@ void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) { } template <> -void computeBV(const Ellipsoid& e, const Transform3f& tf, +void computeBV(const Ellipsoid& e, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); @@ -319,7 +319,7 @@ void computeBV(const Ellipsoid& e, const Transform3f& tf, } template <> -void computeBV(const Capsule& s, const Transform3f& tf, +void computeBV(const Capsule& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); @@ -330,7 +330,7 @@ void computeBV(const Capsule& s, const Transform3f& tf, } template <> -void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { +void computeBV(const Cone& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); @@ -347,7 +347,7 @@ void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { } template <> -void computeBV(const Cylinder& s, const Transform3f& tf, +void computeBV(const Cylinder& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); @@ -365,7 +365,7 @@ void computeBV(const Cylinder& s, const Transform3f& tf, } template <> -void computeBV(const ConvexBase& s, const Transform3f& tf, +void computeBV(const ConvexBase& s, const Transform3s& tf, AABB& bv) { const Matrix3s& R = tf.getRotation(); const Vec3s& T = tf.getTranslation(); @@ -381,13 +381,13 @@ void computeBV(const ConvexBase& s, const Transform3f& tf, } template <> -void computeBV(const TriangleP& s, const Transform3f& tf, +void computeBV(const TriangleP& s, const Transform3s& tf, AABB& bv) { bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c)); } template <> -void computeBV(const Halfspace& s, const Transform3f& tf, +void computeBV(const Halfspace& s, const Transform3s& tf, AABB& bv) { Halfspace new_s = transform(s, tf); const Vec3s& n = new_s.n; @@ -420,7 +420,7 @@ void computeBV(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { +void computeBV(const Plane& s, const Transform3s& tf, AABB& bv) { Plane new_s = transform(s, tf); const Vec3s& n = new_s.n; const CoalScalar& d = new_s.d; @@ -455,7 +455,7 @@ void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { } template <> -void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Box& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -469,7 +469,7 @@ void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Sphere& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -482,7 +482,7 @@ void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Capsule& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -496,7 +496,7 @@ void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Cone& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -510,7 +510,7 @@ void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Cylinder& s, const Transform3f& tf, +void computeBV(const Cylinder& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -525,7 +525,7 @@ void computeBV(const Cylinder& s, const Transform3f& tf, } template <> -void computeBV(const ConvexBase& s, const Transform3f& tf, +void computeBV(const ConvexBase& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -542,7 +542,7 @@ void computeBV(const ConvexBase& s, const Transform3f& tf, } template <> -void computeBV(const Halfspace& s, const Transform3f&, +void computeBV(const Halfspace& s, const Transform3s&, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -555,7 +555,7 @@ void computeBV(const Halfspace& s, const Transform3f&, } template <> -void computeBV(const Halfspace& s, const Transform3f&, +void computeBV(const Halfspace& s, const Transform3s&, RSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -569,7 +569,7 @@ void computeBV(const Halfspace& s, const Transform3f&, } template <> -void computeBV(const Halfspace& s, const Transform3f& tf, +void computeBV(const Halfspace& s, const Transform3s& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -580,7 +580,7 @@ void computeBV(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV(const Halfspace& s, const Transform3f& tf, +void computeBV(const Halfspace& s, const Transform3s& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -593,7 +593,7 @@ void computeBV(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, +void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -653,7 +653,7 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, +void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -719,7 +719,7 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, +void computeBV, Halfspace>(const Halfspace& s, const Transform3s& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -800,7 +800,7 @@ void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, } template <> -void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { +void computeBV(const Plane& s, const Transform3s& tf, OBB& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -818,7 +818,7 @@ void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { } template <> -void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { +void computeBV(const Plane& s, const Transform3s& tf, RSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -838,7 +838,7 @@ void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { } template <> -void computeBV(const Plane& s, const Transform3f& tf, +void computeBV(const Plane& s, const Transform3s& tf, OBBRSS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -849,7 +849,7 @@ void computeBV(const Plane& s, const Transform3f& tf, } template <> -void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { +void computeBV(const Plane& s, const Transform3s& tf, kIOS& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", std::runtime_error); @@ -861,7 +861,7 @@ void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { } template <> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, +void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<16>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -907,7 +907,7 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, } template <> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, +void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<18>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -955,7 +955,7 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, } template <> -void computeBV, Plane>(const Plane& s, const Transform3f& tf, +void computeBV, Plane>(const Plane& s, const Transform3s& tf, KDOP<24>& bv) { if (s.getSweptSphereRadius() > 0) { COAL_THROW_PRETTY("Swept-sphere radius not yet supported.", @@ -1008,92 +1008,92 @@ void computeBV, Plane>(const Plane& s, const Transform3f& tf, } } -void constructBox(const AABB& bv, Box& box, Transform3f& tf) { +void constructBox(const AABB& bv, Box& box, Transform3s& tf) { box = Box(bv.max_ - bv.min_); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const OBB& bv, Box& box, Transform3f& tf) { +void constructBox(const OBB& bv, Box& box, Transform3s& tf) { box = Box(bv.extent * 2); - tf = Transform3f(bv.axes, bv.To); + tf = Transform3s(bv.axes, bv.To); } -void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) { +void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = Transform3f(bv.obb.axes, bv.obb.To); + tf = Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const kIOS& bv, Box& box, Transform3f& tf) { +void constructBox(const kIOS& bv, Box& box, Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = Transform3f(bv.obb.axes, bv.obb.To); + tf = Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const RSS& bv, Box& box, Transform3f& tf) { +void constructBox(const RSS& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.axes, bv.Tr); + tf = Transform3s(bv.axes, bv.Tr); } -void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) { +void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) { +void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) { +void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = Transform3f(bv.center()); + tf = Transform3s(bv.center()); } -void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const AABB& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.max_ - bv.min_); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } -void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.extent * 2); - tf = tf_bv * Transform3f(bv.axes, bv.To); + tf = tf_bv * Transform3s(bv.axes, bv.To); } -void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const OBBRSS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); + tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const kIOS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.obb.extent * 2); - tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); + tf = tf_bv * Transform3s(bv.obb.axes, bv.obb.To); } -void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.axes, bv.Tr); + tf = tf_bv * Transform3s(bv.axes, bv.Tr); } -void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } -void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } -void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, - Transform3f& tf) { +void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv, Box& box, + Transform3s& tf) { box = Box(bv.width(), bv.height(), bv.depth()); - tf = tf_bv * Transform3f(bv.center()); + tf = tf_bv * Transform3s(bv.center()); } } // namespace coal diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp index 624c422a4..367b49aa4 100644 --- a/test/accelerated_gjk.cpp +++ b/test/accelerated_gjk.cpp @@ -54,7 +54,7 @@ using coal::GJKSolver; using coal::GJKVariant; using coal::ShapeBase; using coal::support_func_guess_t; -using coal::Transform3f; +using coal::Transform3s; using coal::Triangle; using coal::Vec3s; using coal::details::GJK; @@ -120,9 +120,9 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) { // Generate random transforms size_t n = 1000; CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.}; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f identity = Transform3f::Identity(); + Transform3s identity = Transform3s::Identity(); // Same init for both solvers Vec3s init_guess = Vec3s(1, 0, 0); diff --git a/test/benchmark.cpp b/test/benchmark.cpp index 2f7b37422..fd1e973c4 100644 --- a/test/benchmark.cpp +++ b/test/benchmark.cpp @@ -38,15 +38,15 @@ void makeModel(const std::vector& vertices, SplitMethodType split_method, BVHModel& model); template -double distance(const std::vector& tf, const BVHModel& m1, +double distance(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose); template -double collide(const std::vector& tf, const BVHModel& m1, +double collide(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose); template -double run(const std::vector& tf, +double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* sm_name); @@ -90,9 +90,9 @@ void makeModel(const std::vector& vertices, } template -double distance(const std::vector& tf, const BVHModel& m1, +double distance(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose) { - Transform3f pose2; + Transform3s pose2; DistanceResult local_result; DistanceRequest request(true); @@ -114,9 +114,9 @@ double distance(const std::vector& tf, const BVHModel& m1, } template -double collide(const std::vector& tf, const BVHModel& m1, +double collide(const std::vector& tf, const BVHModel& m1, const BVHModel& m2, bool verbose) { - Transform3f pose2; + Transform3s pose2; CollisionResult local_result; CollisionRequest request; @@ -141,7 +141,7 @@ double collide(const std::vector& tf, const BVHModel& m1, } template -double run(const std::vector& tf, +double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* prefix) { double col = collide::CollisionTraversalNode>( @@ -154,7 +154,7 @@ double run(const std::vector& tf, } template <> -double run(const std::vector& tf, +double run(const std::vector& tf, const BVHModel (&models)[2][3], int split_method, const char* prefix) { double col = collide::CollisionTraversalNode>( @@ -207,7 +207,7 @@ int main(int, char*[]) { ms_obbrss[1][SPLIT_METHOD_BV_CENTER]); makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]); - std::vector transforms; // t0 + std::vector transforms; // t0 CoalScalar extents[] = {-3000, -3000, -3000, 3000, 3000, 3000}; std::size_t n = 10000; diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp index 8a99e3b11..63c1f4fcf 100644 --- a/test/box_box_collision.cpp +++ b/test/box_box_collision.cpp @@ -14,7 +14,7 @@ using coal::collide; using coal::CollisionRequest; using coal::CollisionResult; using coal::ComputeCollision; -using coal::Transform3f; +using coal::Transform3s; using coal::Vec3s; BOOST_AUTO_TEST_CASE(box_box_collision) { @@ -23,8 +23,8 @@ BOOST_AUTO_TEST_CASE(box_box_collision) { Box shape2(1, 1, 1); // Define transforms - Transform3f T1 = Transform3f::Identity(); - Transform3f T2 = Transform3f::Identity(); + Transform3s T1 = Transform3s::Identity(); + Transform3s T2 = Transform3s::Identity(); // Compute collision CollisionRequest req; diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp index dc5189a1e..82ff5bfee 100644 --- a/test/box_box_distance.cpp +++ b/test/box_box_distance.cpp @@ -56,15 +56,15 @@ using coal::CollisionGeometryPtr_t; using coal::CollisionObject; using coal::DistanceRequest; using coal::DistanceResult; -using coal::Transform3f; +using coal::Transform3s; using coal::Vec3s; BOOST_AUTO_TEST_CASE(distance_box_box_1) { CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); - Transform3f tf1; - Transform3f tf2(Vec3s(25, 20, 5)); + Transform3s tf1; + Transform3s tf2(Vec3s(25, 20, 5)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -104,8 +104,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) { CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2)); CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2)); static double pi = M_PI; - Transform3f tf1; - Transform3f tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), + Transform3s tf1; + Transform3s tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)), Vec3s(0, 0, 10)); @@ -144,9 +144,9 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { CollisionGeometryPtr_t s1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1)); static double pi = M_PI; - Transform3f tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), + Transform3s tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)), Vec3s(-2, 1, .5)); - Transform3f tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), + Transform3s tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0), Vec3s(2, .5, .5)); CollisionObject o1(s1, tf1); @@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) { BOOST_CHECK_CLOSE(p2[2], p2Ref[2], 1e-4); // Apply the same global transform to both objects and recompute - Transform3f tf3(coal::makeQuat(0.435952844074, -0.718287018243, + Transform3s tf3(coal::makeQuat(0.435952844074, -0.718287018243, 0.310622451066, 0.444435113443), Vec3s(4, 5, 6)); tf1 = tf3 * tf1; @@ -223,8 +223,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_4) { DistanceResult distanceResult; double distance; - Transform3f tf1(Vec3s(2, 0, 0)); - Transform3f tf2; + Transform3s tf1(Vec3s(2, 0, 0)); + Transform3s tf2; coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult); distance = 1.; diff --git a/test/broadphase.cpp b/test/broadphase.cpp index 8750e55d0..a28b85636 100644 --- a/test/broadphase.cpp +++ b/test/broadphase.cpp @@ -159,7 +159,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Box* box = new Box(single_size, single_size, single_size); env.push_back(new CollisionObject( shared_ptr(box), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -174,7 +174,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Sphere* sphere = new Sphere(single_size / 2); env.push_back(new CollisionObject( shared_ptr(sphere), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -189,7 +189,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Cylinder* cylinder = new Cylinder(single_size / 2, single_size); env.push_back(new CollisionObject( shared_ptr(cylinder), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -204,7 +204,7 @@ void generateSelfDistanceEnvironments(std::vector& env, Cone* cone = new Cone(single_size / 2, single_size); env.push_back(new CollisionObject( shared_ptr(cone), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -228,10 +228,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Box box(single_size, single_size, single_size); BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3f()); + generateBVHModel(*model, box, Transform3s()); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -245,10 +245,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Sphere sphere(single_size / 2); BVHModel* model = new BVHModel(); - generateBVHModel(*model, sphere, Transform3f(), 16, 16); + generateBVHModel(*model, sphere, Transform3s(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -262,10 +262,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Cylinder cylinder(single_size / 2, single_size); BVHModel* model = new BVHModel(); - generateBVHModel(*model, cylinder, Transform3f(), 16, 16); + generateBVHModel(*model, cylinder, Transform3s(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -279,10 +279,10 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, Cone cone(single_size / 2, single_size); BVHModel* model = new BVHModel(); - generateBVHModel(*model, cone, Transform3f(), 16, 16); + generateBVHModel(*model, cone, Transform3s(), 16, 16); env.push_back(new CollisionObject( shared_ptr(model), - Transform3f(Vec3s( + Transform3s(Vec3s( x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index b4abb8618..74a946621 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -190,7 +190,7 @@ void testBVHModelTriangles() { BOOST_CHECK_EQUAL(model->num_tris, 12); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); - Transform3f pose; + Transform3s pose; shared_ptr > cropped(BVHExtract(*model, pose, aabb)); BOOST_REQUIRE(cropped); BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED); @@ -335,7 +335,7 @@ void testLoadGerardBauzil() { scale.setConstant(1); loadPolyhedronFromResource(env, scale, P1); CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27)); - Transform3f pos(Vec3s(-1.33, 1.36, .14)); + Transform3s pos(Vec3s(-1.33, 1.36, .14)); CollisionObject obj(cylinder, pos); CollisionObject stairs(P1); @@ -376,7 +376,7 @@ BOOST_AUTO_TEST_CASE(test_convex) { Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); - generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + generateBVHModel(box_bvh_model, *box_ptr, Transform3s()); box_bvh_model.buildConvexRepresentation(false); box_bvh_model.convex->computeLocalAABB(); diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp index 386aecf46..30b882b02 100644 --- a/test/capsule_box_1.cpp +++ b/test/capsule_box_1.cpp @@ -59,8 +59,8 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { coal::DistanceRequest distanceRequest(true, 0, 0); coal::DistanceResult distanceResult; - coal::Transform3f tf1(coal::Vec3s(3., 0, 0)); - coal::Transform3f tf2; + coal::Transform3s tf1(coal::Vec3s(3., 0, 0)); + coal::Transform3s tf2; coal::CollisionObject capsule(capsuleGeometry, tf1); coal::CollisionObject box(boxGeometry, tf2); @@ -77,7 +77,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { CHECK_CLOSE_TO_0(o2[1], 1e-1); // Move capsule above box - tf1 = coal::Transform3f(coal::Vec3s(0., 0., 8.)); + tf1 = coal::Transform3s(coal::Vec3s(0., 0., 8.)); capsule.setTransform(tf1); // test distance diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp index 8e3bb3ba7..bdf84d592 100644 --- a/test/capsule_box_2.cpp +++ b/test/capsule_box_2.cpp @@ -60,9 +60,9 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { coal::DistanceResult distanceResult; // Rotate capsule around y axis by pi/2 and move it behind box - coal::Transform3f tf1(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), + coal::Transform3s tf1(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), coal::Vec3s(-10., 0.8, 1.5)); - coal::Transform3f tf2; + coal::Transform3s tf2; coal::CollisionObject capsule(capsuleGeometry, tf1); coal::CollisionObject box(boxGeometry, tf2); diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp index 4534af243..b3f3e5304 100644 --- a/test/capsule_capsule.cpp +++ b/test/capsule_capsule.cpp @@ -68,8 +68,8 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial) { int num_tests = 1e6; #endif - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; for (int i = 0; i < num_tests; ++i) { Eigen::Vector3d p1 = Eigen::Vector3d::Random() * (2. * radius); @@ -121,8 +121,8 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { int num_tests = 1e6; #endif - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; Eigen::Vector3d p1 = Eigen::Vector3d::Zero(); Eigen::Vector3d p2_no_collision = @@ -180,9 +180,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { p2_no_collision = Eigen::Vector3d(0., 0., 2 * (length / 2. + radius) + 1e-3); - Transform3f geom1_placement(Eigen::Matrix3d::Identity(), + Transform3s geom1_placement(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero()); - Transform3f geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision); + Transform3s geom2_placement(Eigen::Matrix3d::Identity(), p2_no_collision); for (int i = 0; i < num_tests; ++i) { Eigen::Matrix3d rot = @@ -190,9 +190,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { .toRotationMatrix(); Eigen::Vector3d trans = Eigen::Vector3d::Random(); - Transform3f displacement(rot, trans); - Transform3f tf1 = displacement * geom1_placement; - Transform3f tf2 = displacement * geom2_placement; + Transform3s displacement(rot, trans); + Transform3s tf1 = displacement * geom1_placement; + Transform3s tf2 = displacement * geom2_placement; CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); @@ -218,9 +218,9 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned) { .toRotationMatrix(); Eigen::Vector3d trans = Eigen::Vector3d::Random(); - Transform3f displacement(rot, trans); - Transform3f tf1 = displacement * geom1_placement; - Transform3f tf2 = displacement * geom2_placement; + Transform3s displacement(rot, trans); + Transform3s tf1 = displacement * geom1_placement; + Transform3s tf2 = displacement * geom2_placement; CollisionObject capsule_o1(c1, tf1); CollisionObject capsule_o2(c2, tf2); @@ -240,8 +240,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(Vec3s(20.1, 0, 0)); + Transform3s tf1; + Transform3s tf2(Vec3s(20.1, 0, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -266,8 +266,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(Vec3s(20, 20, 0)); + Transform3s tf1; + Transform3s tf2(Vec3s(20, 20, 0)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -293,8 +293,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(Vec3s(0, 0, 20.1)); + Transform3s tf1; + Transform3s tf2(Vec3s(0, 0, 20.1)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); @@ -319,8 +319,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { CollisionGeometryPtr_t s1(new Capsule(5, 10)); CollisionGeometryPtr_t s2(new Capsule(5, 10)); - Transform3f tf1; - Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3s(0, 0, 25.1)); + Transform3s tf1; + Transform3s tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3s(0, 0, 25.1)); CollisionObject o1(s1, tf1); CollisionObject o2(s2, tf2); diff --git a/test/collision.cpp b/test/collision.cpp index d02959948..04bddb945 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -74,7 +74,7 @@ int num_max_contacts = (std::numeric_limits::max)(); BOOST_AUTO_TEST_CASE(OBB_Box_test) { CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; - std::vector rotate_transform; + std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; @@ -84,13 +84,13 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; - Transform3f box1_tf; + Transform3s box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < transforms.size(); ++i) { @@ -102,7 +102,7 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { convertBV(aabb, transforms[i], obb2); Box box2; - Transform3f box2_tf; + Transform3s box2_tf; constructBox(aabb, transforms[i], box2, box2_tf); GJKSolver solver; @@ -124,7 +124,7 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) { BOOST_AUTO_TEST_CASE(OBB_shape_test) { CoalScalar r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; - std::vector rotate_transform; + std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; @@ -134,13 +134,13 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) { OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; - Transform3f box1_tf; + Transform3s box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < transforms.size(); ++i) { @@ -209,7 +209,7 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) { CoalScalar extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); AABB aabb1; @@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) { aabb1.max_ = Vec3s(600, 600, 600); OBB obb1; - convertBV(aabb1, Transform3f(), obb1); + convertBV(aabb1, Transform3s(), obb1); for (std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; @@ -227,7 +227,7 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) { AABB aabb2 = translate(aabb, transforms[i].getTranslation()); OBB obb2; - convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2); + convertBV(aabb, Transform3s(transforms[i].getTranslation()), obb2); bool overlap_aabb = aabb1.overlap(aabb2); bool overlap_obb = obb1.overlap(obb2); @@ -312,7 +312,7 @@ HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS struct mesh_mesh_run_test { - mesh_mesh_run_test(const std::vector& _transforms, + mesh_mesh_run_test(const std::vector& _transforms, const CollisionRequest _request) : transforms(_transforms), request(_request), @@ -321,7 +321,7 @@ struct mesh_mesh_run_test { isInit(false), indent(0) {} - const std::vector& transforms; + const std::vector& transforms; const CollisionRequest request; bool enable_statistics, benchmark; std::vector contacts; @@ -347,7 +347,7 @@ struct mesh_mesh_run_test { } template - void query(const std::vector& transforms, + void query(const std::vector& transforms, SplitMethodType splitMethod, const CollisionRequest request, std::vector& contacts) { BENCHMARK_HEADER("BV"); @@ -375,7 +375,7 @@ struct mesh_mesh_run_test { model2); Timer timer(false); - const Transform3f tf2; + const Transform3s tf2; const std::size_t N = transforms.size(); contacts.resize(3 * N); @@ -385,7 +385,7 @@ struct mesh_mesh_run_test { ++indent; for (std::size_t i = 0; i < transforms.size(); ++i) { - const Transform3f& tf1 = transforms[i]; + const Transform3s& tf1 = transforms[i]; timer.start(); CollisionResult local_result; @@ -429,7 +429,7 @@ struct mesh_mesh_run_test { ++indent; for (std::size_t i = 0; i < transforms.size(); ++i) { - const Transform3f tf1 = transforms[i]; + const Transform3s tf1 = transforms[i]; timer.start(); CollisionResult local_result; @@ -438,9 +438,9 @@ struct mesh_mesh_run_test { node.enable_statistics = enable_statistics; BVH_t* model1_tmp = new BVH_t(*model1); - Transform3f tf1_tmp = tf1; + Transform3s tf1_tmp = tf1; BVH_t* model2_tmp = new BVH_t(*model2); - Transform3f tf2_tmp = tf2; + Transform3s tf2_tmp = tf2; bool success = initialize(node, *model1_tmp, tf1_tmp, *model2_tmp, tf2_tmp, local_result, true, true); @@ -482,7 +482,7 @@ struct mesh_mesh_run_test { for (std::size_t i = 0; i < transforms.size(); ++i) { timer.start(); - const Transform3f tf1 = transforms[i]; + const Transform3s tf1 = transforms[i]; CollisionResult local_result; MeshCollisionTraversalNode node(request); @@ -623,7 +623,7 @@ struct mesh_mesh_run_test { // calls function collide with identity for both object poses, // BOOST_AUTO_TEST_CASE(mesh_mesh) { - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; @@ -654,7 +654,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { } BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) { - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG std::size_t n = 0; diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp index 3214a96c1..698d1d5b4 100644 --- a/test/collision_node_asserts.cpp +++ b/test/collision_node_asserts.cpp @@ -36,8 +36,8 @@ BOOST_AUTO_TEST_CASE(TestTriangles) { ComputeCollision compute(&tri1, &tri2); - Transform3f tri1Tf{}; - Transform3f tri2Tf{}; + Transform3s tri1Tf{}; + Transform3s tri2Tf{}; /// check some angles for two triangles for (int i = 0; i < 360; i += 30) { diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp index 6db7f1069..a78b84f44 100644 --- a/test/contact_patch.cpp +++ b/test/contact_patch.cpp @@ -48,8 +48,8 @@ BOOST_AUTO_TEST_CASE(box_box_no_collision) { const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to separate the shapes const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, 2 * halfside + offset)); @@ -75,8 +75,8 @@ BOOST_AUTO_TEST_CASE(box_sphere) { const Box box(2 * halfside, 2 * halfside, 2 * halfside); const Sphere sphere(halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); @@ -113,8 +113,8 @@ BOOST_AUTO_TEST_CASE(box_box) { const Box box1(2 * halfside, 2 * halfside, 2 * halfside); const Box box2(2 * halfside, 2 * halfside, 2 * halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); @@ -171,8 +171,8 @@ BOOST_AUTO_TEST_CASE(halfspace_box) { const CoalScalar halfside = 0.5; const Box box(2 * halfside, 2 * halfside, 2 * halfside); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, halfside - offset)); @@ -231,8 +231,8 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) { const CoalScalar height = 1.; const Capsule capsule(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); @@ -343,8 +343,8 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) { const CoalScalar height = 1.; const Cone cone(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); @@ -468,8 +468,8 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) { const CoalScalar height = 1.; const Cylinder cylinder(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); @@ -577,8 +577,8 @@ BOOST_AUTO_TEST_CASE(convex_convex) { const Convex box1(buildBox(halfside, halfside, halfside)); const Convex box2(buildBox(halfside, halfside, halfside)); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset)); @@ -638,8 +638,8 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) { const Vec3s expected_cp1(0, 0.5, 0); const Vec3s expected_cp2(0, 1, 0); - const Transform3f tf1; // identity - const Transform3f tf2; // identity + const Transform3s tf1; // identity + const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -804,8 +804,8 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) { const size_t expected_size = 1; const Vec3s expected_cp(0, 0, 0); - const Transform3f tf1; // identity - const Transform3f tf2; // identity + const Transform3s tf1; // identity + const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, @@ -966,8 +966,8 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) { const Vec3s expected_cp1(0, 0, 0); const Vec3s expected_cp2(-0.5, 0.5, 0); - const Transform3f tf1; // identity - const Transform3f tf2; // identity + const Transform3s tf1; // identity + const Transform3s tf2; // identity const size_t num_max_contact = 1; const CollisionRequest col_req(CollisionRequestFlag::CONTACT, diff --git a/test/convex.cpp b/test/convex.cpp index 02507b263..52363b7d8 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -88,7 +88,7 @@ BOOST_AUTO_TEST_CASE(convex) { template void compareShapeIntersection(const Sa& sa, const Sb& sb, - const Transform3f& tf1, const Transform3f& tf2, + const Transform3s& tf1, const Transform3s& tf2, CoalScalar tol = 1e-9) { CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); CollisionResult resA, resB; @@ -117,8 +117,8 @@ void compareShapeIntersection(const Sa& sa, const Sb& sb, } template -void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3f& tf1, - const Transform3f& tf2, CoalScalar tol = 1e-9) { +void compareShapeDistance(const Sa& sa, const Sb& sb, const Transform3s& tf1, + const Transform3s& tf2, CoalScalar tol = 1e-9) { DistanceRequest request(true); DistanceResult resA, resB; @@ -154,8 +154,8 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) { Box box(l * 2, w * 2, d * 2); Convex convex_box(buildBox(l, w, d)); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; tf2.setTranslation(Vec3s(3, 0, 0)); compareShapeIntersection(box, convex_box, tf1, tf2, eps); diff --git a/test/distance.cpp b/test/distance.cpp index 77a35f60c..acb892513 100644 --- a/test/distance.cpp +++ b/test/distance.cpp @@ -56,14 +56,14 @@ bool verbose = false; CoalScalar DELTA = 0.001; template -void distance_Test(const Transform3f& tf, const std::vector& vertices1, +void distance_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, unsigned int qsize, DistanceRes& distance_result, bool verbose = true); -bool collide_Test_OBB(const Transform3f& tf, +bool collide_Test_OBB(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -71,7 +71,7 @@ bool collide_Test_OBB(const Transform3f& tf, SplitMethodType split_method, bool verbose); template -void distance_Test_Oriented(const Transform3f& tf, +void distance_Test_Oriented(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -93,7 +93,7 @@ BOOST_AUTO_TEST_CASE(mesh_distance) { loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - std::vector transforms; // t0 + std::vector transforms; // t0 CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; #ifndef NDEBUG // if debug mode std::size_t n = 1; @@ -468,7 +468,7 @@ BOOST_AUTO_TEST_CASE(mesh_distance) { } template -void distance_Test_Oriented(const Transform3f& tf, +void distance_Test_Oriented(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -491,7 +491,7 @@ void distance_Test_Oriented(const Transform3f& tf, DistanceResult local_result; TraversalNode node; if (!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, - Transform3f(), DistanceRequest(true), local_result)) + Transform3s(), DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; @@ -516,7 +516,7 @@ void distance_Test_Oriented(const Transform3f& tf, } template -void distance_Test(const Transform3f& tf, const std::vector& vertices1, +void distance_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, @@ -535,7 +535,7 @@ void distance_Test(const Transform3f& tf, const std::vector& vertices1, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; DistanceResult local_result; MeshDistanceTraversalNode node; @@ -565,7 +565,7 @@ void distance_Test(const Transform3f& tf, const std::vector& vertices1, } } -bool collide_Test_OBB(const Transform3f& tf, +bool collide_Test_OBB(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -588,7 +588,7 @@ bool collide_Test_OBB(const Transform3f& tf, CollisionRequest request(CONTACT | DISTANCE_LOWER_BOUND, 1); MeshCollisionTraversalNodeOBB node(request); bool success(initialize(node, (const BVHModel&)m1, tf, - (const BVHModel&)m2, Transform3f(), + (const BVHModel&)m2, Transform3s(), local_result)); BOOST_REQUIRE(success); diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp index 21c2f3eea..f4386597d 100644 --- a/test/distance_lower_bound.cpp +++ b/test/distance_lower_bound.cpp @@ -56,15 +56,15 @@ using coal::DistanceRequest; using coal::DistanceResult; using coal::OBBRSS; using coal::shared_ptr; -using coal::Transform3f; +using coal::Transform3s; using coal::Triangle; using coal::Vec3s; -bool testDistanceLowerBound(const Transform3f& tf, +bool testDistanceLowerBound(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2, CoalScalar& distance) { - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; CollisionRequest request; @@ -78,9 +78,9 @@ bool testDistanceLowerBound(const Transform3f& tf, return result.isCollision(); } -bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1, +bool testCollide(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2) { - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; CollisionRequest request(coal::NO_REQUEST, 1); request.enable_distance_lower_bound = false; @@ -93,9 +93,9 @@ bool testCollide(const Transform3f& tf, const CollisionGeometryPtr_t& m1, return result.isCollision(); } -bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1, +bool testDistance(const Transform3s& tf, const CollisionGeometryPtr_t& m1, const CollisionGeometryPtr_t& m2, CoalScalar& distance) { - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; DistanceRequest request; DistanceResult result; @@ -131,7 +131,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { m2->addSubModel(p2, t2); m2->endModel(); - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; @@ -161,12 +161,12 @@ BOOST_AUTO_TEST_CASE(box_sphere) { shared_ptr sphere(new coal::Sphere(0.5)); shared_ptr box(new coal::Box(1., 1., 1.)); - Transform3f M1; + Transform3s M1; M1.setIdentity(); - Transform3f M2; + Transform3s M2; M2.setIdentity(); - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; @@ -198,12 +198,12 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { shared_ptr sphere1(new coal::Sphere(0.5)); shared_ptr sphere2(new coal::Sphere(1.)); - Transform3f M1; + Transform3s M1; M1.setIdentity(); - Transform3f M2; + Transform3s M2; M2.setIdentity(); - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; const std::size_t n = 1000; @@ -245,7 +245,7 @@ BOOST_AUTO_TEST_CASE(box_mesh) { m1->addSubModel(p1, t1); m1->endModel(); - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 100; diff --git a/test/frontlist.cpp b/test/frontlist.cpp index df1e6e6dc..9e68a326c 100644 --- a/test/frontlist.cpp +++ b/test/frontlist.cpp @@ -51,7 +51,7 @@ using namespace coal; namespace utf = boost::unit_test::framework; template -bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, +bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -60,8 +60,8 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, bool verbose); template -bool collide_front_list_Test_Oriented(const Transform3f& tf1, - const Transform3f& tf2, +bool collide_front_list_Test_Oriented(const Transform3s& tf1, + const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -70,7 +70,7 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, bool verbose); template -bool collide_Test(const Transform3f& tf, const std::vector& vertices1, +bool collide_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, @@ -84,8 +84,8 @@ BOOST_AUTO_TEST_CASE(front_list) { loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); - std::vector transforms; // t0 - std::vector transforms2; // t1 + std::vector transforms; // t0 + std::vector transforms2; // t1 CoalScalar extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; CoalScalar delta_trans[] = {1, 1, 1}; #ifndef NDEBUG // if debug mode @@ -270,7 +270,7 @@ BOOST_AUTO_TEST_CASE(front_list) { } template -bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, +bool collide_front_list_Test(const Transform3s& tf1, const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -297,7 +297,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1, pose2; + Transform3s pose1, pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); @@ -336,8 +336,8 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, } template -bool collide_front_list_Test_Oriented(const Transform3f& tf1, - const Transform3f& tf2, +bool collide_front_list_Test_Oriented(const Transform3s& tf1, + const Transform3s& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, @@ -359,7 +359,7 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1(tf1), pose2; + Transform3s pose1(tf1), pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); @@ -392,7 +392,7 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, } template -bool collide_Test(const Transform3f& tf, const std::vector& vertices1, +bool collide_Test(const Transform3s& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, @@ -410,7 +410,7 @@ bool collide_Test(const Transform3f& tf, const std::vector& vertices1, m2.addSubModel(vertices2, triangles2); m2.endModel(); - Transform3f pose1(tf), pose2; + Transform3s pose1(tf), pose2; CollisionResult local_result; CollisionRequest request(NO_REQUEST, (std::numeric_limits::max)()); diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp index ad89819cc..ec8e6b468 100644 --- a/test/geometric_shapes.cpp +++ b/test/geometric_shapes.cpp @@ -78,8 +78,8 @@ std::ostream& operator<<(std::ostream& os, const Box& b) { template void printComparisonError(const std::string& comparison_type, const S1& s1, - const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, + const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, const Vec3s& contact_or_normal, const Vec3s& expected_contact_or_normal, bool check_opposite_normal, CoalScalar tol) { @@ -109,8 +109,8 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, template void printComparisonError(const std::string& comparison_type, const S1& s1, - const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, CoalScalar depth, + const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, CoalScalar depth, CoalScalar expected_depth, CoalScalar tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) @@ -126,8 +126,8 @@ void printComparisonError(const std::string& comparison_type, const S1& s1, } template -void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, const Vec3s& contact, +void compareContact(const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, const Vec3s& contact, Vec3s* expected_point, CoalScalar depth, CoalScalar* expected_depth, const Vec3s& normal, Vec3s* expected_normal, bool check_opposite_normal, @@ -162,8 +162,8 @@ void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2, } template -void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, bool expect_collision, +void testShapeCollide(const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, bool expect_collision, Vec3s* expected_point = NULL, CoalScalar* expected_depth = NULL, Vec3s* expected_normal = NULL, @@ -210,85 +210,85 @@ void testShapeCollide(const S1& s1, const Transform3f& tf1, const S2& s2, BOOST_AUTO_TEST_CASE(box_to_bvh) { Box shape(1, 1, 1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f()); + generateBVHModel(bvh, shape, Transform3s()); } BOOST_AUTO_TEST_CASE(sphere_to_bvh) { Sphere shape(1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f(), 10, 10); - generateBVHModel(bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3s(), 10, 10); + generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(cylinder_to_bvh) { Cylinder shape(1, 1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f(), 10, 10); - generateBVHModel(bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3s(), 10, 10); + generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(cone_to_bvh) { Cone shape(1, 1); BVHModel bvh; - generateBVHModel(bvh, shape, Transform3f(), 10, 10); - generateBVHModel(bvh, shape, Transform3f(), 50); + generateBVHModel(bvh, shape, Transform3s(), 10, 10); + generateBVHModel(bvh, shape, Transform3s(), 50); } BOOST_AUTO_TEST_CASE(collide_spheresphere) { Sphere s1(20); Sphere s2(10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(40, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(40, 0, 0)); + tf2 = transform * Transform3s(Vec3s(40, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(30, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(30, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(30.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(30.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(30.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(30.01, 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(29.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(29.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(29.9, 0, 0)); + tf2 = transform * Transform3s(Vec3s(29.9, 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); normal << 1, 0, 0; // If the centers of two sphere are at the same position, // the normal is (1, 0, 0) SET_LINE; @@ -301,32 +301,32 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-29.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-29.9, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-29.9, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-29.9, 0, 0)); normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-30.0, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-30.0, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-30.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-30.01, 0, 0)); normal << -1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-30.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-30.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -354,8 +354,8 @@ void testBoxBoxContactPoints(const Matrix3s& R) { vertices[i].array() *= s2.halfSide.array(); } - Transform3f tf1 = Transform3f(Vec3s(0, 0, -50)); - Transform3f tf2 = Transform3f(R); + Transform3s tf1 = Transform3s(Vec3s(0, 0, -50)); + Transform3s tf2 = Transform3s(R); Vec3s normal; Vec3s p1, p2; @@ -385,10 +385,10 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; @@ -398,8 +398,8 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { Quatf q; q = AngleAxis((CoalScalar)3.140 / 6, UnitZ); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (1, 0, 0). normal << 1, 0, 0; @@ -414,37 +414,37 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(15, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(15, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-8); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(15.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(15.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(15.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(15.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(q); + tf1 = Transform3s(); + tf2 = Transform3s(q); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); tf1 = transform; - tf2 = transform * Transform3f(q); + tf2 = transform * Transform3s(q); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0); int numTests = 100; for (int i = 0; i < numTests; ++i) { - Transform3f tf; + Transform3s tf; generateRandomTransform(extents, tf); SET_LINE; testBoxBoxContactPoints(tf.getRotation()); @@ -455,18 +455,18 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { Sphere s1(20); Box s2(5, 5, 5); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. The current result is (-1, 0, 0). normal << -1, 0, 0; @@ -480,24 +480,24 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(22.50001, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(22.50001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(22.501, 0, 0)); + tf2 = transform * Transform3s(Vec3s(22.501, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(22.4, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(22.4, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(22.4, 0, 0)); + tf2 = transform * Transform3s(Vec3s(22.4, 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); @@ -516,8 +516,8 @@ BOOST_AUTO_TEST_CASE(distance_spherebox) { coal::Box box(10, 2, 10); coal::DistanceResult result; - distance(&sphere, Transform3f(rotSphere, trSphere), &box, - Transform3f(rotBox, trBox), DistanceRequest(true), result); + distance(&sphere, Transform3s(rotSphere, trSphere), &box, + Transform3s(rotBox, trBox), DistanceRequest(true), result); CoalScalar eps = Eigen::NumTraits::epsilon(); BOOST_CHECK_CLOSE(result.min_distance, 3., eps); @@ -530,18 +530,18 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) { Sphere s1(20); Capsule s2(5, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -554,38 +554,38 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(24.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(24.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(24.9, 0, 0)); + tf2 = transform * Transform3s(Vec3s(24.9, 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(25, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(24.999999, 0, 0)); + tf2 = transform * Transform3s(Vec3s(24.999999, 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(25.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(25.1, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(25.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(25.1, 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); @@ -595,18 +595,18 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { Cylinder s1(5, 15); Cylinder s2(5, 15); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -619,37 +619,37 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(9.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 9.9, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 9.9, 0)); normal << 0, 1, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(9.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(9.9, 0, 0)); + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0)); normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(10.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(10.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -658,18 +658,18 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { Cone s1(5, 10); Cone s2(5, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -682,9 +682,9 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); + tf1 = Transform3s(); // z=0 is a singular points. Two normals could be returned. - tf2 = Transform3f(Vec3s(9.9, 0, 0.00001)); + tf2 = Transform3s(Vec3s(9.9, 0, 0.00001)); normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); SET_LINE; @@ -697,36 +697,36 @@ BOOST_AUTO_TEST_CASE(collide_conecone) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(9.9, 0, 0.00001)); + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0.00001)); normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius) .normalized(); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, true, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(10.001, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(10.001, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.001, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 9.9)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 9.9)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 9.9)); + tf2 = transform * Transform3s(Vec3s(0, 0, 9.9)); normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); @@ -736,18 +736,18 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); // Vec3s point; // CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); // TODO: Need convention for normal when the centers of two objects are at // same position. SET_LINE; @@ -760,8 +760,8 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(9.9, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0)); normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius)) .normalized(); @@ -769,73 +769,73 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(9.9, 0, 0)); + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0)); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(9.9, 0, 0.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(9.9, 0, 0.1)); normal << 1, 0, 0; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(9.9, 0, 0.1)); + tf2 = transform * Transform3s(Vec3s(9.9, 0, 0.1)); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(10.01, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(10.01, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.01, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(10, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(10, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10, 0, 0)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 9.9)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 9.9)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 9.9)); + tf2 = transform * Transform3s(Vec3s(0, 0, 9.9)); normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 10.01)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.01)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 10.01)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.01)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 10)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10)); normal << 0, 0, 1; SET_LINE; testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s1, tf1, s2, tf2, false); } @@ -843,7 +843,7 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) { BOOST_AUTO_TEST_CASE(collide_spheretriangle) { Sphere s(10); - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s normal; @@ -857,12 +857,12 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity tf_tri.setTranslation(Vec3s(0, 0, 0.001)); normal << 0, 0, 1; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -871,7 +871,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal << 0, 0, -1; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -884,13 +884,13 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { t[1] << 9.9, -20, 0; t[2] << 9.9, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, 0.001)); normal << 9.9, 0, 0.001; normal.normalize(); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -900,7 +900,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { normal << 9.9, 0, -0.001; normal.normalize(); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -913,12 +913,12 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0.001, 0)); normal << 0, 1, 0; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -927,7 +927,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { tf_tri.setTranslation(Vec3s(0, -0.001, 0)); normal << 0, -1, 0; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -940,12 +940,12 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0.001, 0, 0)); normal << 1, 0, 0; SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -953,7 +953,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { tf_tri.setTranslation(Vec3s(-0.001, 0, 0)); normal << -1, 0, 0; - testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(s, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); SET_LINE; normal = transform.getRotation() * normal; SET_LINE; @@ -970,17 +970,17 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, 10.1)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, 0, -10.1)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } @@ -992,16 +992,16 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 10.1, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, -10.1, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } @@ -1012,17 +1012,17 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { t[1] << 0, -20, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(10.1, 0, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(-10.1, 0, 0)); SET_LINE; - testShapeCollide(s, Transform3f(), tri, tf_tri, false); + testShapeCollide(s, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(s, transform, tri, transform * tf_tri, false); } @@ -1031,7 +1031,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) { BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { Halfspace hs(Vec3s(0, 0, 1), 0); - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s normal; @@ -1044,12 +1044,12 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { t[1] << -20, 0, 0; t[2] << 0, 20, 0; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1057,19 +1057,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } @@ -1080,12 +1080,12 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { t[1] << -20, 0, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1093,19 +1093,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } @@ -1116,12 +1116,12 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { t[1] << 0, -10, 0; t[2] << 0, 0, 20; TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); + Transform3s tf_tri = Transform3s(); tf_tri.setTranslation(Vec3s(0, 0, -0.001)); normal = hs.n; SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(hs, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1129,26 +1129,26 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) { tf_tri.setTranslation(Vec3s(0, 0, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(1, 1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(-1, -1, 0.001)); SET_LINE; - testShapeCollide(hs, Transform3f(), tri, tf_tri, false); + testShapeCollide(hs, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(hs, transform, tri, transform * tf_tri, false); } } BOOST_AUTO_TEST_CASE(collide_planetriangle) { - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s normal; @@ -1161,10 +1161,10 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { Plane pl(Vec3s(0, 0, 1), 0); TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1173,7 +1173,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { tf_tri.setTranslation(Vec3s(0, 0, 0.05)); normal = pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1181,13 +1181,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { tf_tri.setTranslation(Vec3s(0, 0, -0.06)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, 0, 0.11)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } @@ -1200,10 +1200,10 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { Plane pl(Vec3s(0, 1, 0), 0); TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1212,7 +1212,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { tf_tri.setTranslation(Vec3s(0, 0.05, 0)); normal = pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1220,13 +1220,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { tf_tri.setTranslation(Vec3s(0, -0.06, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0, 0.11, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } @@ -1239,10 +1239,10 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { Plane pl(Vec3s(1, 0, 0), 0); TriangleP tri(t[0], t[1], t[2]); - Transform3f tf_tri = Transform3f(); // identity + Transform3s tf_tri = Transform3s(); // identity normal = -pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1251,7 +1251,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { tf_tri.setTranslation(Vec3s(0.05, 0, 0)); normal = pl.n; SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal); + testShapeCollide(pl, Transform3s(), tri, tf_tri, true, NULL, NULL, &normal); normal = transform.getRotation() * normal; SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL, @@ -1259,13 +1259,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) { tf_tri.setTranslation(Vec3s(-0.06, 0, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); tf_tri.setTranslation(Vec3s(0.11, 0, 0)); SET_LINE; - testShapeCollide(pl, Transform3f(), tri, tf_tri, false); + testShapeCollide(pl, Transform3s(), tri, tf_tri, false); SET_LINE; testShapeCollide(pl, transform, tri, transform * tf_tri, false); } @@ -1275,18 +1275,18 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { Sphere s(10); Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -5, 0, 0; depth = -10; normal << -1, 0, 0; @@ -1301,8 +1301,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5, 0, 0)); contact << -2.5, 0, 0; depth = -15; normal << -1, 0, 0; @@ -1310,15 +1310,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5, 0, 0)); contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -15; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5, 0, 0)); contact << -7.5, 0, 0; depth = -5; normal << -1, 0, 0; @@ -1326,25 +1326,25 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5, 0, 0)); contact = transform.transform(Vec3s(-7.5, 0, 0)); depth = -5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-10.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.1, 0, 0)); contact << 0.05, 0, 0; depth = -20.1; normal << -1, 0, 0; @@ -1352,7 +1352,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(10.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.1, 0, 0)); contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -20.1; normal = transform.getRotation() * Vec3s(-1, 0, 0); @@ -1364,10 +1364,10 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { Sphere s(10); Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; @@ -1376,8 +1376,8 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { Vec3s p1, p2; CoalScalar eps = 1e-6; - tf1 = Transform3f(Vec3s(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); depth = -10 + eps; p1 << -10 + eps, 0, 0; p2 << 0, 0, 0; @@ -1395,8 +1395,8 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3s(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); depth = -10 - eps; p1 << 10 + eps, 0, 0; p2 << 0, 0, 0; @@ -1412,8 +1412,8 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5, 0, 0)); p1 << 10, 0, 0; p2 << 5, 0, 0; contact << (p1 + p2) / 2; @@ -1423,15 +1423,15 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5, 0, 0)); p1 << -10, 0, 0; p2 << -5, 0, 0; contact << (p1 + p2) / 2; @@ -1441,30 +1441,30 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-10.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(10.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(10.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(10.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } @@ -1473,18 +1473,18 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { Box s(5, 10, 20); Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -1.25, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1499,8 +1499,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(1.25, 0, 0)); contact << -0.625, 0, 0; depth = -3.75; normal << -1, 0, 0; @@ -1508,15 +1508,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(1.25, 0, 0)); contact = transform.transform(Vec3s(-0.625, 0, 0)); depth = -3.75; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-1.25, 0, 0)); contact << -1.875, 0, 0; depth = -1.25; normal << -1, 0, 0; @@ -1524,15 +1524,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0)); contact = transform.transform(Vec3s(-1.875, 0, 0)); depth = -1.25; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.51, 0, 0)); contact << 0.005, 0, 0; depth = -5.01; normal << -1, 0, 0; @@ -1540,25 +1540,25 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.51, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.51, 0, 0)); contact = transform.transform(Vec3s(0.005, 0, 0)); depth = -5.01; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.51, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(transform.getRotation()); - tf2 = Transform3f(); + tf1 = Transform3s(transform.getRotation()); + tf2 = Transform3s(); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true); } @@ -1567,18 +1567,18 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { Box s(5, 10, 20); Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); Vec3s p1(2.5, 0, 0); Vec3s p2(0, 0, 0); contact << (p1 + p2) / 2; @@ -1595,8 +1595,8 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(1.25, 0, 0)); p2 << 1.25, 0, 0; contact << (p1 + p2) / 2; depth = -1.25; @@ -1605,15 +1605,15 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-1.25, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-1.25, 0, 0)); p1 << -2.5, 0, 0; p2 << -1.25, 0, 0; contact << (p1 + p2) / 2; @@ -1623,35 +1623,35 @@ BOOST_AUTO_TEST_CASE(collide_planebox) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-1.25, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-1.25, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -1.25; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.51, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.51, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.51, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.51, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(transform.getRotation()); - tf2 = Transform3f(); + tf1 = Transform3s(transform.getRotation()); + tf2 = Transform3s(); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true); } @@ -1660,18 +1660,18 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { Capsule s(5, 10); Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -2.5, 0, 0; depth = -5; normal << -1, 0, 0; @@ -1686,8 +1686,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; @@ -1695,15 +1695,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1711,15 +1711,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); contact << 0.05, 0, 0; depth = -10.1; normal << -1, 0, 0; @@ -1727,27 +1727,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -10.1; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, -2.5, 0; depth = -5; normal << 0, -1, 0; @@ -1762,8 +1762,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; @@ -1771,15 +1771,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; @@ -1787,15 +1787,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); contact << 0, 0.05, 0; depth = -10.1; normal << 0, -1, 0; @@ -1803,27 +1803,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); contact = transform.transform(Vec3s(0, 0.05, 0)); depth = -10.1; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, -5; depth = -10; normal << 0, 0, -1; @@ -1838,8 +1838,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -3.75; depth = -12.5; normal << 0, 0, -1; @@ -1847,15 +1847,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -12.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -6.25; depth = -7.5; normal << 0, 0, -1; @@ -1863,15 +1863,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -6.25)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); contact << 0, 0, 0.05; depth = -20.1; normal << 0, 0, -1; @@ -1879,20 +1879,20 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -20.1; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } @@ -1901,18 +1901,18 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { Capsule s(5, 10); Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, 0; depth = -5; normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0) @@ -1927,8 +1927,8 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << 2.5, 0, 0; depth = -2.5; normal << 1, 0, 0; @@ -1936,15 +1936,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(2.5, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -2.5, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -1952,37 +1952,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-2.5, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, 0; depth = -5; normal << 0, 1, 0; // (0, 1, 0) or (0, -1, 0) @@ -1997,8 +1997,8 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, 2.5, 0; depth = -2.5; normal << 0, 1, 0; @@ -2006,15 +2006,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, 2.5, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -2.5, 0; depth = -2.5; normal << 0, -1, 0; @@ -2022,37 +2022,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -2.5, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, 0; depth = -10; normal << 0, 0, 1; // (0, 0, 1) or (0, 0, -1) @@ -2067,8 +2067,8 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, 2.5; depth = -7.5; normal << 0, 0, 1; @@ -2076,15 +2076,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, 2.5)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -2.5; depth = -7.5; normal << 0, 0, -1; @@ -2092,30 +2092,30 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) { testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -2.5)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } @@ -2124,18 +2124,18 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { Cylinder s(5, 10); Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -2.5, 0, 0; depth = -5; normal << -1, 0, 0; @@ -2150,8 +2150,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, 0; depth = -7.5; normal << -1, 0, 0; @@ -2159,15 +2159,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(-1.25, 0, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, 0; depth = -2.5; normal << -1, 0, 0; @@ -2175,15 +2175,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-3.75, 0, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); contact << 0.05, 0, 0; depth = -10.1; normal << -1, 0, 0; @@ -2191,27 +2191,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); contact = transform.transform(Vec3s(0.05, 0, 0)); depth = -10.1; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, -2.5, 0; depth = -5; normal << 0, -1, 0; @@ -2226,8 +2226,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, 0; depth = -7.5; normal << 0, -1, 0; @@ -2235,15 +2235,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, -1.25, 0)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, 0; depth = -2.5; normal << 0, -1, 0; @@ -2251,15 +2251,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -3.75, 0)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); contact << 0, 0.05, 0; depth = -10.1; normal << 0, -1, 0; @@ -2267,27 +2267,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); contact = transform.transform(Vec3s(0, 0.05, 0)); depth = -10.1; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, -2.5; depth = -5; normal << 0, 0, -1; @@ -2302,8 +2302,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; @@ -2311,15 +2311,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; @@ -2327,15 +2327,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 5.1)); contact << 0, 0, 0.05; depth = -10.1; normal << 0, 0, -1; @@ -2343,20 +2343,20 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 5.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 5.1)); contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -10.1; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -5.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } @@ -2365,10 +2365,10 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { Cylinder s(5, 10); Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; @@ -2377,8 +2377,8 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { Vec3s p1, p2; CoalScalar eps = 1e-6; - tf1 = Transform3f(Vec3s(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << -5 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2397,8 +2397,8 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3s(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << 5 + eps, 0, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2416,8 +2416,8 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); p1 << 5, 0, 0; p2 << 2.5, 0, 0; contact << (p1 + p2) / 2; @@ -2427,15 +2427,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); p1 << -5, 0, 0; p2 << -2.5, 0, 0; contact << (p1 + p2) / 2; @@ -2445,38 +2445,38 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 1, 0), 0); eps = 1e-6; - tf1 = Transform3f(Vec3s(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, -5 + eps, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2494,8 +2494,8 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3s(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, 5 + eps, 0; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2512,8 +2512,8 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); p1 << 0, 5, 0; p2 << 0, 2.5, 0; contact << (p1 + p2) / 2; @@ -2523,15 +2523,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); p1 << 0, -5, 0; p2 << 0, -2.5, 0; contact << (p1 + p2) / 2; @@ -2541,38 +2541,38 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 0, 1), 0); eps = 1e-6; - tf1 = Transform3f(Vec3s(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2590,8 +2590,8 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3s(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -2608,8 +2608,8 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; @@ -2619,15 +2619,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5.; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; @@ -2637,30 +2637,30 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } @@ -2669,18 +2669,18 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { Cone s(5, 10); Halfspace hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; CoalScalar depth; Vec3s normal; - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << -2.5, 0, -5; depth = -5; normal << -1, 0, 0; @@ -2695,8 +2695,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); contact << -1.25, 0, -5; depth = -7.5; normal << -1, 0, 0; @@ -2704,15 +2704,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform(Vec3s(-1.25, 0, -5)); depth = -7.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); contact << -3.75, 0, -5; depth = -2.5; normal << -1, 0, 0; @@ -2720,15 +2720,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform(Vec3s(-3.75, 0, -5)); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); contact << 0.05, 0, -5; depth = -10.1; normal << -1, 0, 0; @@ -2736,27 +2736,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); contact = transform.transform(Vec3s(0.05, 0, -5)); depth = -10.1; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 1, 0), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, -2.5, -5; depth = -5; normal << 0, -1, 0; @@ -2771,8 +2771,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); contact << 0, -1.25, -5; depth = -7.5; normal << 0, -1, 0; @@ -2780,15 +2780,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform(Vec3s(0, -1.25, -5)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); contact << 0, -3.75, -5; depth = -2.5; normal << 0, -1, 0; @@ -2796,15 +2796,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform(Vec3s(0, -3.75, -5)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); contact << 0, 0.05, -5; depth = -10.1; normal << 0, -1, 0; @@ -2812,27 +2812,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); contact = transform.transform(Vec3s(0, 0.05, -5)); depth = -10.1; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Halfspace(Vec3s(0, 0, 1), 0); - tf1 = Transform3f(); - tf2 = Transform3f(); + tf1 = Transform3s(); + tf2 = Transform3s(); contact << 0, 0, -2.5; depth = -5; normal << 0, 0, -1; @@ -2847,8 +2847,8 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); contact << 0, 0, -1.25; depth = -7.5; normal << 0, 0, -1; @@ -2856,15 +2856,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform(Vec3s(0, 0, -1.25)); depth = -7.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); contact << 0, 0, -3.75; depth = -2.5; normal << 0, 0, -1; @@ -2872,15 +2872,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform(Vec3s(0, 0, -3.75)); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 5.1)); contact << 0, 0, 0.05; depth = -10.1; normal << 0, 0, -1; @@ -2888,20 +2888,20 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 5.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 5.1)); contact = transform.transform(Vec3s(0, 0, 0.05)); depth = -10.1; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -5.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -5.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -5.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } @@ -2910,10 +2910,10 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { Cone s(5, 10); Plane hs(Vec3s(1, 0, 0), 0); - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); Vec3s contact; @@ -2922,8 +2922,8 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { Vec3s p1, p2; CoalScalar eps = 1e-6; - tf1 = Transform3f(Vec3s(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << -5 + eps, 0, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -2942,8 +2942,8 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3s(eps, 0, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(eps, 0, 0)); + tf2 = Transform3s(); p1 << 5 + eps, 0, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -2961,8 +2961,8 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(2.5, 0, 0)); p1 << 5, 0, -5; p2 << 2.5, 0, -5; contact << (p1 + p2) / 2; @@ -2972,15 +2972,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-2.5, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-2.5, 0, 0)); p1 << -5, 0, -5; p2 << -2.5, 0, -5; contact << (p1 + p2) / 2; @@ -2990,38 +2990,38 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-2.5, 0, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(-1, 0, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(-5.1, 0, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0)); + tf2 = transform * Transform3s(Vec3s(-5.1, 0, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 1, 0), 0); eps = 1e-6; - tf1 = Transform3f(Vec3s(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, -5 + eps, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -3039,8 +3039,8 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3s(0, eps, 0)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, eps, 0)); + tf2 = Transform3s(); p1 << 0, 5 + eps, -5; p2 << 0, 0, -5; contact << (p1 + p2) / 2; @@ -3057,8 +3057,8 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 2.5, 0)); p1 << 0, 5, -5; p2 << 0, 2.5, -5; contact << (p1 + p2) / 2; @@ -3068,15 +3068,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, 2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -2.5, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -2.5, 0)); p1 << 0, -5, -5; p2 << 0, -2.5, -5; contact << (p1 + p2) / 2; @@ -3086,38 +3086,38 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -2.5, 0)); + tf2 = transform * Transform3s(Vec3s(0, -2.5, 0)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, -1, 0); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, 5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, -5.1, 0)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, -5.1, 0)); + tf2 = transform * Transform3s(Vec3s(0, -5.1, 0)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); hs = Plane(Vec3s(0, 0, 1), 0); eps = 1e-6; - tf1 = Transform3f(Vec3s(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, -5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -3135,8 +3135,8 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); eps = -1e-6; - tf1 = Transform3f(Vec3s(0, 0, eps)); - tf2 = Transform3f(); + tf1 = Transform3s(Vec3s(0, 0, eps)); + tf2 = Transform3s(); p1 << 0, 0, 5 + eps; p2 << 0, 0, 0; contact << (p1 + p2) / 2; @@ -3153,8 +3153,8 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 2.5)); p1 << 0, 0, 5; p2 << 0, 0, 2.5; contact << (p1 + p2) / 2; @@ -3164,15 +3164,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, 2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, 1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -2.5)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -2.5)); p1 << 0, 0, -5; p2 << 0, 0, -2.5; contact << (p1 + p2) / 2; @@ -3182,43 +3182,43 @@ BOOST_AUTO_TEST_CASE(collide_planecone) { testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -2.5)); + tf2 = transform * Transform3s(Vec3s(0, 0, -2.5)); contact = transform.transform((p1 + p2) / 2); depth = -2.5; normal = transform.getRotation() * Vec3s(0, 0, -1); SET_LINE; testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, 10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, 10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, 10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); - tf1 = Transform3f(); - tf2 = Transform3f(Vec3s(0, 0, -10.1)); + tf1 = Transform3s(); + tf2 = Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); tf1 = transform; - tf2 = transform * Transform3f(Vec3s(0, 0, -10.1)); + tf2 = transform * Transform3s(Vec3s(0, 0, -10.1)); SET_LINE; testShapeCollide(s, tf1, hs, tf2, false); } BOOST_AUTO_TEST_CASE(collide_planeplane) { - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; Vec3s normal; Vec3s contact; CoalScalar distance; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); { @@ -3331,14 +3331,14 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) { } BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; Vec3s normal; Vec3s contact; CoalScalar distance; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); { @@ -3447,14 +3447,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) { } BOOST_AUTO_TEST_CASE(collide_halfspaceplane) { - Transform3f tf1; - Transform3f tf2; + Transform3s tf1; + Transform3s tf2; Vec3s normal; Vec3s contact; CoalScalar distance; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); { @@ -3570,67 +3570,67 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) { Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); CoalScalar dist = -1; dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(30.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(30.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(29.9, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(29.9, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); - dist = solver1.shapeDistance(s1, Transform3f(Vec3s(40, 0, 0)), s2, - Transform3f(), compute_penetration, closest_p1, + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(40, 0, 0)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); - dist = solver1.shapeDistance(s1, Transform3f(Vec3s(30.1, 0, 0)), s2, - Transform3f(), compute_penetration, closest_p1, + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(30.1, 0, 0)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); - dist = solver1.shapeDistance(s1, Transform3f(Vec3s(29.9, 0, 0)), s2, - Transform3f(), compute_penetration, closest_p1, + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(29.9, 0, 0)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(30.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(30.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(29.9, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(29.9, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(40, 0, 0)), s2, + dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(40, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10) < 0.001); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(30.1, 0, 0)), + dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(30.1, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); - dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(29.9, 0, 0)), + dist = solver1.shapeDistance(s1, transform * Transform3s(Vec3s(29.9, 0, 0)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist < 0); @@ -3642,12 +3642,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); CoalScalar dist; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3658,57 +3658,57 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) { BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(20.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(20.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(0, 20.2, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(0, 20.2, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.2) < 0.001); dist = solver1.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 10.1, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 10.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(20.1, 0, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(20.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(0, 20.1, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(0, 20.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 10.1) < 0.001); dist = solver2.shapeDistance( - s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 10.1, 0)), + s2, Transform3s(), s2, Transform3s(Vec3s(10.1, 10.1, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(15.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(15.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(20, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(20, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(20, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 5) < 0.001); } @@ -3717,12 +3717,12 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) { Cylinder s1(0.029, 0.1); Box s2(1.6, 0.6, 0.025); - Transform3f tf1( + Transform3s tf1( Quatf(0.5279170511703305, -0.50981118132505521, -0.67596178682051911, 0.0668715876735793), Vec3s(0.041218354748013122, 1.2022554710435607, 0.77338855025700015)); - Transform3f tf2( + Transform3s tf2( Quatf(0.70738826916719977, 0, 0, 0.70682518110536596), Vec3s(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003)); @@ -3771,7 +3771,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); CoalScalar dist; @@ -3779,8 +3779,8 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { int N = 10; for (int i = 0; i < N + 1; ++i) { CoalScalar dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N); - dist = solver1.shapeDistance(s1, Transform3f(Vec3s(dbox, 0., 0.)), s2, - Transform3f(), compute_penetration, closest_p1, + dist = solver1.shapeDistance(s1, Transform3s(Vec3s(dbox, 0., 0.)), s2, + Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); EIGEN_VECTOR_IS_APPROX(normal, -Vec3s(1, 0, 0), 1e-6); @@ -3789,13 +3789,13 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { solver1.shapeDistance(s1, transform, s2, transform, compute_penetration, closest_p1, closest_p2, normal); dist = solver1.shapeDistance( - s1, transform * Transform3f(Vec3s(dbox, 0., 0.)), s2, transform, + s1, transform * Transform3s(Vec3s(dbox, 0., 0.)), s2, transform, compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6); EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), 1e-6); } - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3806,22 +3806,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) { BOOST_CHECK(dist <= 0); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(22.6, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(22.6, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(22.6, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(22.6, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 17.5) < 0.001); } @@ -3832,7 +3832,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); CoalScalar dist; @@ -3841,7 +3841,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { // The following situations corresponds to the case where the two cylinders // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3858,7 +3858,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3874,22 +3874,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.001); } @@ -3900,7 +3900,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); CoalScalar dist; @@ -3909,7 +3909,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { // The following situations corresponds to the case where the two cones // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3926,7 +3926,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3942,22 +3942,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.001); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(0, 0, 40)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(0, 0, 40)), + s1, transform, s2, transform * Transform3s(Vec3s(0, 0, 40)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 1); } @@ -3968,7 +3968,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { Vec3s closest_p1, closest_p2, normal; bool compute_penetration = true; - Transform3f transform; + Transform3s transform; generateRandomTransform(extents, transform); CoalScalar dist; @@ -3977,7 +3977,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { // The following situations corresponds to the case where the two cones // are exactly superposed. This is the worst case for EPA which will take // forever to converge with default parameters. - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -3994,7 +3994,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { solver1.epa_tolerance = 1e-2; solver1.epa_max_iterations = 1000; - dist = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), + dist = solver1.shapeDistance(s1, Transform3s(), s2, Transform3s(), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(dist <= 0); @@ -4010,22 +4010,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { } dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)), + s1, Transform3s(), s2, Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.01); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(10.1, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 0.1) < 0.02); dist = solver1.shapeDistance( - s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration, + s1, Transform3s(), s2, Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.01); dist = solver1.shapeDistance( - s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)), + s1, transform, s2, transform * Transform3s(Vec3s(40, 0, 0)), compute_penetration, closest_p1, closest_p2, normal); BOOST_CHECK(fabs(dist - 30) < 0.1); } @@ -4033,8 +4033,8 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) { template void testReversibleShapeDistance(const S1& s1, const S2& s2, CoalScalar distance) { - Transform3f tf1(Vec3s(-0.5 * distance, 0.0, 0.0)); - Transform3f tf2(Vec3s(+0.5 * distance, 0.0, 0.0)); + Transform3s tf1(Vec3s(-0.5 * distance, 0.0, 0.0)); + Transform3s tf2(Vec3s(+0.5 * distance, 0.0, 0.0)); CoalScalar distA; CoalScalar distB; diff --git a/test/gjk.cpp b/test/gjk.cpp index bb4d8996d..9871029d5 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -51,7 +51,7 @@ using coal::GJKSolver; using coal::GJKVariant; using coal::Matrix3s; using coal::Quatf; -using coal::Transform3f; +using coal::Transform3s; using coal::TriangleP; using coal::Vec3s; @@ -78,7 +78,7 @@ void test_gjk_distance_triangle_triangle( GJKSolver solver; if (enable_gjk_nesterov_acceleration) solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration; - Transform3f tf1, tf2; + Transform3s tf1, tf2; Vec3s p1, p2, a1, a2; Matrix3s M; CoalScalar distance(sqrt(-1)); @@ -103,7 +103,7 @@ void test_gjk_distance_triangle_triangle( Vec3s(-25.655000000000001, -1.2858199462890625, 3.7249809570312502); Q2_loc = Vec3s(-10.926, -1.284259033203125, 3.7281499023437501); Q3_loc = Vec3s(-10.926, -1.2866180419921875, 3.72335400390625); - Transform3f tf( + Transform3s tf( Quatf(-0.42437287410898855, -0.26862477561450587, -0.46249645019513175, 0.73064726592483387), Vec3s(-12.824601270753471, -1.6840516940066426, 3.8914453043793844)); @@ -131,7 +131,7 @@ void test_gjk_distance_triangle_triangle( tf1.setRotation(R); tf1.setTranslation(T); } else { - tf1 = Transform3f(); + tf1 = Transform3s(); } TriangleP tri1(P1_loc, P2_loc, P3_loc); @@ -343,8 +343,8 @@ void test_gjk_unit_sphere(CoalScalar center_distance, Vec3s ray, sphere.setSweptSphereRadius(swept_sphere_radius); typedef Eigen::Matrix Vec4f; - Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3s::Zero()); - Transform3f tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray); + Transform3s tf0(Quatf(Vec4f::Random().normalized()), Vec3s::Zero()); + Transform3s tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray); bool expect_collision = center_distance <= 2 * (r + swept_sphere_radius); @@ -420,7 +420,7 @@ void test_gjk_triangle_capsule(Vec3s T, bool expect_collision, Capsule capsule(1., 2.); // Radius 1 and length 2 TriangleP triangle(Vec3s(0., 0., 0.), Vec3s(1., 0., 0.), Vec3s(1., 1., 0.)); - Transform3f tf0, tf1; + Transform3s tf0, tf1; tf1.setTranslation(T); details::MinkowskiDiff shape; diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp index d18a17f7c..c4cb0be98 100644 --- a/test/gjk_asserts.cpp +++ b/test/gjk_asserts.cpp @@ -64,8 +64,8 @@ BOOST_AUTO_TEST_CASE(TestSpheres) { ComputeCollision compute(&sphere2, &sphere1); - Transform3f sphere1Tf = Transform3f::Identity(); - Transform3f sphere2Tf = Transform3f::Identity(); + Transform3s sphere1Tf = Transform3s::Identity(); + Transform3s sphere2Tf = Transform3s::Identity(); for (int i = 0; i < 360; ++i) { for (int j = 0; j < 180; ++j) { diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index 139df1ada..6002c38be 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -53,7 +53,7 @@ using coal::GJKConvergenceCriterionType; using coal::GJKSolver; using coal::ShapeBase; using coal::support_func_guess_t; -using coal::Transform3f; +using coal::Transform3s; using coal::Vec3s; using coal::details::GJK; using coal::details::MinkowskiDiff; @@ -123,9 +123,9 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, // Generate random transforms size_t n = 1000; CoalScalar extents[] = {-3., -3., 0, 3., 3., 3.}; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f identity = Transform3f::Identity(); + Transform3s identity = Transform3s::Identity(); // Same init for both solvers Vec3s init_guess = Vec3s(1, 0, 0); diff --git a/test/hfields.cpp b/test/hfields.cpp index a9fcc4c09..09d1b7449 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -97,16 +97,16 @@ void test_constant_hfields(const Eigen::DenseIndex nx, // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); - const Transform3f box_placement( + const Transform3s box_placement( Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.)); // Test collision const Sphere sphere(1.); - static const Transform3f IdTransform; + static const Transform3s IdTransform; const Box box(Vec3s::Ones()); - Transform3f M_sphere, M_box; + Transform3s M_sphere, M_box; // No collision case { @@ -281,16 +281,16 @@ void test_negative_security_margin(const Eigen::DenseIndex nx, // Build equivalent object const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); - const Transform3f box_placement( + const Transform3s box_placement( Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.)); // Test collision const Sphere sphere(1.); - static const Transform3f IdTransform; + static const Transform3s IdTransform; const Box box(Vec3s::Ones()); - Transform3f M_sphere, M_box; + Transform3s M_sphere, M_box; // No collision case { @@ -430,8 +430,8 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { const HeightField hfield(2., 2., heights, -10.); Sphere sphere(0.48); - const Transform3f sphere_pos(Vec3s(0., 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 0.5)); + const Transform3s hfield_pos; const CollisionRequest request; @@ -480,8 +480,8 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); Sphere sphere(0.975); - const Transform3f sphere_pos(Vec3s(0., 0., 1.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 1.)); + const Transform3s hfield_pos; const CoalScalar thresholds[3] = {0., 0.01, -0.005}; @@ -728,8 +728,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the TOP { - const Transform3f sphere_pos(Vec3s(0., 0., 2.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 2.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -743,8 +743,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Same, but with a positive margin. { - const Transform3f sphere_pos(Vec3s(0., 0., 2.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., 2.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -763,8 +763,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the BOTTOM { - const Transform3f sphere_pos(Vec3s(0., 0., -1.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., -1.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -775,8 +775,8 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos(Vec3s(0., 0., -1.)); - const Transform3f hfield_pos; + const Transform3s sphere_pos(Vec3s(0., 0., -1.)); + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -795,9 +795,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the WEST { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -808,9 +808,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -827,9 +827,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the EAST { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -840,9 +840,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -860,9 +860,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the NORTH { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -873,9 +873,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -893,9 +893,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { // Collision from the SOUTH { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; @@ -906,9 +906,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { } { - const Transform3f sphere_pos( + const Transform3s sphere_pos( Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); - const Transform3f hfield_pos; + const Transform3s hfield_pos; CollisionResult result; CollisionRequest request; diff --git a/test/math.cpp b/test/math.cpp index 231739030..c0f16ff3e 100644 --- a/test/math.cpp +++ b/test/math.cpp @@ -126,14 +126,14 @@ BOOST_AUTO_TEST_CASE(quaternion) { BOOST_AUTO_TEST_CASE(transform) { Quatf q = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2); Vec3s T(0, 1, 2); - Transform3f tf(q, T); + Transform3s tf(q, T); Vec3s v(1, -1, 0); BOOST_CHECK(isEqual(q * v + T, q * v + T)); Vec3s rv(q * v); - // typename Transform3f::transform_return_type::type output = + // typename Transform3s::transform_return_type::type output = // tf * v; // std::cout << rv << std::endl; // std::cout << output.lhs() << std::endl; @@ -142,7 +142,7 @@ BOOST_AUTO_TEST_CASE(transform) { BOOST_AUTO_TEST_CASE(random_transform) { for (size_t i = 0; i < 100; ++i) { - const Transform3f tf = Transform3f::Random(); + const Transform3s tf = Transform3s::Random(); BOOST_CHECK((tf.inverseTimes(tf)).isIdentity()); } } diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp index 76e62af0f..322777480 100644 --- a/test/normal_and_nearest_points.cpp +++ b/test/normal_and_nearest_points.cpp @@ -87,9 +87,9 @@ void test_normal_and_nearest_points( // We want to make sure we generate poses that are in collision // so we take a relatively small extent for the random poses CoalScalar extents[] = {-1.5, -1.5, -1.5, 1.5, 1.5, 1.5}; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f tf1 = Transform3f::Identity(); + Transform3s tf1 = Transform3s::Identity(); CollisionRequest colreq; colreq.distance_upper_bound = std::numeric_limits::max(); @@ -114,7 +114,7 @@ void test_normal_and_nearest_points( // Since CollisionRequest::distance_lower_bound is set to infinity, // these functions should agree on the results regardless of collision or // not. - Transform3f tf2 = transforms[i]; + Transform3s tf2 = transforms[i]; CollisionResult colres; DistanceResult distres; size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); @@ -146,7 +146,7 @@ void test_normal_and_nearest_points( } // Separate the shapes - Transform3f new_tf1 = tf1; + Transform3s new_tf1 = tf1; CoalScalar eps = 1e-2; new_tf1.setTranslation(tf1.getTranslation() + separation_vector - eps * contact.normal); @@ -200,7 +200,7 @@ void test_normal_and_nearest_points( // happens to be parallel to the axis of the cone, the two shapes will // still be disjoint. CoalScalar eps = 1e-2; - Transform3f new_tf1 = tf1; + Transform3s new_tf1 = tf1; new_tf1.setTranslation(tf1.getTranslation() + separation_vector + eps * distres.normal); CollisionResult new_colres; @@ -625,10 +625,10 @@ void test_normal_and_nearest_points(const BVHModel& o1, size_t n = 10000; #endif CoalScalar extents[] = {-2., -2., -2., 2., 2., 2.}; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); - Transform3f tf1 = Transform3f::Identity(); - transforms[0] = Transform3f::Identity(); + Transform3s tf1 = Transform3s::Identity(); + transforms[0] = Transform3s::Identity(); CollisionRequest colreq; colreq.distance_upper_bound = std::numeric_limits::max(); @@ -638,7 +638,7 @@ void test_normal_and_nearest_points(const BVHModel& o1, DistanceResult distres; for (size_t i = 0; i < n; i++) { - Transform3f tf2 = transforms[i]; + Transform3s tf2 = transforms[i]; colres.clear(); distres.clear(); size_t col = collide(&o1, tf1, &o2, tf2, colreq, colres); @@ -683,7 +683,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) { Box* box_ptr = new coal::Box(1, 1, 1); coal::CollisionGeometryPtr_t b1(box_ptr); BVHModel o1 = BVHModel(); - generateBVHModel(o1, *box_ptr, Transform3f()); + generateBVHModel(o1, *box_ptr, Transform3s()); o1.buildConvexRepresentation(false); CoalScalar offset = 0.1; diff --git a/test/obb.cpp b/test/obb.cpp index 77c16fe3c..134a8daa2 100644 --- a/test/obb.cpp +++ b/test/obb.cpp @@ -107,7 +107,7 @@ bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, CoalScalar& distance) { GJKSolver gjk; Box ba(2 * a), bb(2 * b); - Transform3f tfa, tfb(B, T); + Transform3s tfa, tfb(B, T); Vec3s p1, p2, normal; bool compute_penetration = true; diff --git a/test/octree.cpp b/test/octree.cpp index 9cedd2e6c..d7f3b5d35 100644 --- a/test/octree.cpp +++ b/test/octree.cpp @@ -73,7 +73,7 @@ coal::OcTree makeOctree(const BVHModel& mesh, coal::Box box(resolution, resolution, resolution); CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1); CollisionResult result; - Transform3f tfBox; + Transform3s tfBox; octomap::OcTreePtr_t octree(new octomap::OcTree(resolution)); for (CoalScalar x = resolution * floor(m[0] / resolution); x <= M[0]; @@ -85,7 +85,7 @@ coal::OcTree makeOctree(const BVHModel& mesh, Vec3s center(x + .5 * resolution, y + .5 * resolution, z + .5 * resolution); tfBox.setTranslation(center); - coal::collide(&box, tfBox, &mesh, Transform3f(), request, result); + coal::collide(&box, tfBox, &mesh, Transform3s(), request, result); if (result.isCollision()) { octomap::point3d p((float)center[0], (float)center[1], (float)center[2]); @@ -139,7 +139,7 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { 3 * sizeof(CoalScalar)); } - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 100; @@ -155,8 +155,8 @@ BOOST_AUTO_TEST_CASE(octree_mesh) { for (std::size_t i = 0; i < N; ++i) { CollisionResult resultMesh; CollisionResult resultOctree; - Transform3f tf1(transforms[2 * i]); - Transform3f tf2(transforms[2 * i + 1]); + Transform3s tf1(transforms[2 * i]); + Transform3s tf2(transforms[2 * i + 1]); ; // Test collision between meshes with random transform for robot. coal::collide(&robMesh, tf1, &envMesh, tf2, request, resultMesh); @@ -204,7 +204,7 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { HeightField hfield(x_dim, y_dim, heights, min_altitude); hfield.computeLocalAABB(); - std::vector transforms; + std::vector transforms; CoalScalar extents[] = {-2000, -2000, 0, 2000, 2000, 2000}; #ifndef NDEBUG // if debug mode std::size_t N = 1000; @@ -220,11 +220,11 @@ BOOST_AUTO_TEST_CASE(octree_height_field) { for (std::size_t i = 0; i < N; ++i) { CollisionResult resultBox; CollisionResult resultHfield1, resultHfield2; - Transform3f tf1(transforms[2 * i]); - Transform3f tf2(transforms[2 * i + 1]); + Transform3s tf1(transforms[2 * i]); + Transform3s tf2(transforms[2 * i + 1]); Box box; - Transform3f box_tf; + Transform3s box_tf; constructBox(hfield.aabb_local, tf2, box, box_tf); // Test collision between octree and equivalent box. diff --git a/test/profiling.cpp b/test/profiling.cpp index 5319e422c..ab97a9648 100644 --- a/test/profiling.cpp +++ b/test/profiling.cpp @@ -82,10 +82,10 @@ struct Results { Results(size_t i) : rs(i), times((Eigen::DenseIndex)i) {} }; -void collide(const std::vector& tf, const CollisionGeometry* o1, +void collide(const std::vector& tf, const CollisionGeometry* o1, const CollisionGeometry* o2, const CollisionRequest& request, Results& results) { - Transform3f Id; + Transform3s Id; BenchTimer timer; for (std::size_t i = 0; i < tf.size(); ++i) { timer.start(); @@ -199,7 +199,7 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { o->computeLocalAABB(); OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- " << o->aabb_local.max_.transpose() << " ..."); - o.reset(extract(o.get(), Transform3f(), aabb)); + o.reset(extract(o.get(), Transform3s(), aabb)); if (!o) throw std::invalid_argument("Failed to crop."); OUT("Crop has " << dynamic_pointer_cast >(o)->num_tris << " triangles"); @@ -221,7 +221,7 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) { } int main(int argc, char** argv) { - std::vector transforms; + std::vector transforms; CollisionRequest request; diff --git a/test/python_unit/api.py b/test/python_unit/api.py index ac7331d46..f15b08f60 100644 --- a/test/python_unit/api.py +++ b/test/python_unit/api.py @@ -8,8 +8,8 @@ class TestMainAPI(TestCase): def test_collision(self): capsule = coal.Capsule(1.0, 2.0) - M1 = coal.Transform3f() - M2 = coal.Transform3f(np.eye(3), np.array([3, 0, 0])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0])) req = coal.CollisionRequest() res = coal.CollisionResult() @@ -18,8 +18,8 @@ def test_collision(self): def test_distance(self): capsule = coal.Capsule(1.0, 2.0) - M1 = coal.Transform3f() - M2 = coal.Transform3f(np.eye(3), np.array([3, 0, 0])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([3, 0, 0])) req = coal.DistanceRequest() res = coal.DistanceResult() diff --git a/test/python_unit/collision.py b/test/python_unit/collision.py index fbf543bce..afd69d503 100644 --- a/test/python_unit/collision.py +++ b/test/python_unit/collision.py @@ -27,19 +27,19 @@ def test_convex_halfspace(self): req = coal.CollisionRequest() res = coal.CollisionResult() - M1 = coal.Transform3f() - M2 = coal.Transform3f(np.eye(3), np.array([0, 0, -0.1])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([0, 0, -0.1])) res.clear() coal.collide(convex, M1, halfspace, M2, req, res) self.assertFalse(coal.collide(convex, M1, halfspace, M2, req, res)) - M1 = coal.Transform3f() - M2 = coal.Transform3f(np.eye(3), np.array([0, 0, 0.1])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 0.1])) res.clear() self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) - M1 = coal.Transform3f() - M2 = coal.Transform3f(np.eye(3), np.array([0, 0, 2])) + M1 = coal.Transform3s() + M2 = coal.Transform3s(np.eye(3), np.array([0, 0, 2])) res.clear() self.assertTrue(coal.collide(convex, M1, halfspace, M2, req, res)) diff --git a/test/python_unit/collision_manager.py b/test/python_unit/collision_manager.py index 9dfe8474f..241fda1c6 100644 --- a/test/python_unit/collision_manager.py +++ b/test/python_unit/collision_manager.py @@ -4,14 +4,14 @@ sphere = coal.Sphere(0.5) sphere_obj = coal.CollisionObject(sphere) -M_sphere = coal.Transform3f.Identity() +M_sphere = coal.Transform3s.Identity() M_sphere.setTranslation(np.array([-0.6, 0.0, 0.0])) sphere_obj.setTransform(M_sphere) box = coal.Box(np.array([0.5, 0.5, 0.5])) box_obj = coal.CollisionObject(box) -M_box = coal.Transform3f.Identity() +M_box = coal.Transform3s.Identity() M_box.setTranslation(np.array([-0.6, 0.0, 0.0])) box_obj.setTransform(M_box) diff --git a/test/security_margin.cpp b/test/security_margin.cpp index c56dba7c1..5993343da 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -54,7 +54,7 @@ using coal::CollisionRequest; using coal::CollisionResult; using coal::DistanceRequest; using coal::DistanceResult; -using coal::Transform3f; +using coal::Transform3s; using coal::Vec3s; #define MATH_SQUARED(x) (x * x) @@ -63,15 +63,15 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3s(0, 1, 1)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 1, 1)); coal::Box s1(1, 1, 1); coal::Box s2(1, 1, 1); const double tol = 1e-8; AABB bv1, bv2; - computeBV(s1, Transform3f(), bv1); - computeBV(s2, Transform3f(), bv2); + computeBV(s1, Transform3s(), bv1); + computeBV(s2, Transform3s(), bv2); // No security margin - collision { @@ -89,7 +89,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; - Transform3f tf2_no_collision( + Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); @@ -105,7 +105,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; collisionRequest.security_margin = distance; - Transform3f tf2_no_collision( + Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); AABB bv2_transformed; computeBV(s2, tf2_no_collision, bv2_transformed); @@ -121,7 +121,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) { CollisionRequest collisionRequest(CONTACT, 1); const double distance = -0.01; collisionRequest.security_margin = distance; - const Transform3f tf2( + const Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, distance))); AABB bv2_transformed; computeBV(s2, tf2, bv2_transformed); @@ -153,14 +153,14 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3s(0, 0, 0)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 0, 0)); coal::Box s1(1, 1, 1); coal::Box s2(1, 1, 1); AABB bv1, bv2; - computeBV(s1, Transform3f(), bv1); - computeBV(s2, Transform3f(), bv2); + computeBV(s1, Transform3s(), bv1); + computeBV(s2, Transform3s(), bv2); // The two AABB are collocated { @@ -183,8 +183,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionGeometryPtr_t s1(new coal::Sphere(1)); CollisionGeometryPtr_t s2(new coal::Sphere(2)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3s(0, 0, 3)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 0, 3)); // NOTE: when comparing a result to zero, **do not use BOOST_CHECK_CLOSE**! // Zero is not close to any other number, so the test will always fail. @@ -206,7 +206,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const double distance = 0.01; - Transform3f tf2_no_collision( + Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest, collisionResult); @@ -220,7 +220,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionResult collisionResult; const double distance = 0.01; collisionRequest.security_margin = distance; - Transform3f tf2( + Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); @@ -235,7 +235,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionResult collisionResult; const double distance = -0.01; collisionRequest.security_margin = distance; - Transform3f tf2( + Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance))); collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); @@ -261,8 +261,8 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionGeometryPtr_t c1(new coal::Capsule(0.5, 1.)); CollisionGeometryPtr_t c2(new coal::Capsule(0.5, 1.)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3s(0, 1., 0)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 1., 0)); // No security margin - collision { @@ -280,7 +280,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; const double distance = 0.01; - Transform3f tf2_no_collision( + Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); @@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionResult collisionResult; const double distance = 0.01; collisionRequest.security_margin = distance; - Transform3f tf2_no_collision( + Transform3s tf2_no_collision( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest, collisionResult); @@ -310,7 +310,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) { CollisionResult collisionResult; const double distance = -0.01; collisionRequest.security_margin = distance; - Transform3f tf2( + Transform3s tf2( Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0))); collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); @@ -335,8 +335,8 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionGeometryPtr_t b1(new coal::Box(1, 1, 1)); CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3s(0, 1, 1)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 1, 1)); const double tol = 1e-3; @@ -355,7 +355,7 @@ BOOST_AUTO_TEST_CASE(box_box) { { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; - const Transform3f tf2_no_collision( + const Transform3s tf2_no_collision( (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; @@ -398,7 +398,7 @@ BOOST_AUTO_TEST_CASE(box_box) { collisionRequest.security_margin = distance; CollisionResult collisionResult; - const Transform3f tf2((tf2_collision.getTranslation() + + const Transform3s tf2((tf2_collision.getTranslation() + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); @@ -411,9 +411,9 @@ BOOST_AUTO_TEST_CASE(box_box) { } template -void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, +void test_shape_shape(const ShapeType1& shape1, const Transform3s& tf1, const ShapeType2& shape2, - const Transform3f& tf2_collision, const CoalScalar tol) { + const Transform3s& tf2_collision, const CoalScalar tol) { // No security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); @@ -429,7 +429,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, { CollisionRequest collisionRequest(CONTACT, 1); const double distance = 0.01; - const Transform3f tf2_no_collision( + const Transform3s tf2_no_collision( (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval()); CollisionResult collisionResult; @@ -475,7 +475,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, collisionRequest.security_margin = distance; CollisionResult collisionResult; - const Transform3f tf2((tf2_collision.getTranslation() + + const Transform3s tf2((tf2_collision.getTranslation() + Vec3s(0, collisionRequest.security_margin, collisionRequest.security_margin)) .eval()); @@ -491,13 +491,13 @@ BOOST_AUTO_TEST_CASE(sphere_box) { Box* box_ptr = new coal::Box(1, 1, 1); CollisionGeometryPtr_t b1(box_ptr); BVHModel box_bvh_model = BVHModel(); - generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + generateBVHModel(box_bvh_model, *box_ptr, Transform3s()); box_bvh_model.buildConvexRepresentation(false); ConvexBase& box_convex = *box_bvh_model.convex.get(); CollisionGeometryPtr_t s2(new coal::Sphere(0.5)); - const Transform3f tf1; - const Transform3f tf2_collision(Vec3s(0, 0, 1)); + const Transform3s tf1; + const Transform3s tf2_collision(Vec3s(0, 0, 1)); const double tol = 1e-6; diff --git a/test/serialization.cpp b/test/serialization.cpp index c1dc66fbd..f54ace2d1 100644 --- a/test/serialization.cpp +++ b/test/serialization.cpp @@ -286,8 +286,8 @@ BOOST_AUTO_TEST_CASE(test_collision_data) { const CoalScalar height = 1.; const Cylinder cylinder(radius, height); - const Transform3f tf1; - Transform3f tf2; + const Transform3s tf1; + Transform3s tf2; // set translation to have a collision const CoalScalar offset = 0.001; tf2.setTranslation(Vec3s(0, 0, height / 2 - offset)); @@ -436,11 +436,11 @@ BOOST_AUTO_TEST_CASE(test_HeightField) { } BOOST_AUTO_TEST_CASE(test_transform) { - Transform3f T; + Transform3s T; T.setQuatRotation(Quaternion3f::UnitRandom()); T.setTranslation(Vec3s::Random()); - Transform3f T_copy; + Transform3s T_copy; test_serialization(T, T_copy); } diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp index a571c380a..cfd321024 100644 --- a/test/shape_inflation.cpp +++ b/test/shape_inflation.cpp @@ -53,7 +53,7 @@ using coal::CollisionRequest; using coal::CollisionResult; using coal::DistanceRequest; using coal::DistanceResult; -using coal::Transform3f; +using coal::Transform3s; using coal::Vec3s; #define MATH_SQUARED(x) (x * x) @@ -114,7 +114,7 @@ void test(const Shape &original_shape, const CoalScalar inflation, { const CoalScalar inflation = 0.; const auto &inflation_result = original_shape.inflated(inflation); - const Transform3f &shift = inflation_result.second; + const Transform3s &shift = inflation_result.second; const Shape &inflated_shape = inflation_result.first; BOOST_CHECK(isApprox(original_shape, inflated_shape, tol)); @@ -125,13 +125,13 @@ void test(const Shape &original_shape, const CoalScalar inflation, { const auto &inflation_result = original_shape.inflated(inflation); const Shape &inflated_shape = inflation_result.first; - const Transform3f &inflation_shift = inflation_result.second; + const Transform3s &inflation_shift = inflation_result.second; BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); const auto &deflation_result = inflated_shape.inflated(-inflation); const Shape &deflated_shape = deflation_result.first; - const Transform3f &deflation_shift = deflation_result.second; + const Transform3s &deflation_shift = deflation_result.second; BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); @@ -141,13 +141,13 @@ void test(const Shape &original_shape, const CoalScalar inflation, { const auto &inflation_result = original_shape.inflated(-inflation); const Shape &inflated_shape = inflation_result.first; - const Transform3f &inflation_shift = inflation_result.second; + const Transform3s &inflation_shift = inflation_result.second; BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); const auto &deflation_result = inflated_shape.inflated(+inflation); const Shape &deflated_shape = deflation_result.first; - const Transform3f &deflation_shift = deflation_result.second; + const Transform3s &deflation_shift = deflation_result.second; BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp index d234602ef..6b8d35e33 100644 --- a/test/shape_mesh_consistency.cpp +++ b/test/shape_mesh_consistency.cpp @@ -56,39 +56,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -111,27 +111,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) { pose.setTranslation(Vec3s(40.1, 0, 0)); res.clear(), res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -159,39 +159,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f()); - generateBVHModel(s2_rss, s2, Transform3f()); + generateBVHModel(s1_rss, s1, Transform3s()); + generateBVHModel(s2_rss, s2, Transform3s()); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -215,27 +215,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) { res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -263,39 +263,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -320,27 +320,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) { res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -368,38 +368,38 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -424,27 +424,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) { res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -471,39 +471,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -527,27 +527,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -575,39 +575,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f()); - generateBVHModel(s2_rss, s2, Transform3f()); + generateBVHModel(s1_rss, s1, Transform3s()); + generateBVHModel(s2_rss, s2, Transform3s()); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(50, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -631,27 +631,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -679,39 +679,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -747,27 +747,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -795,39 +795,39 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { BVHModel s1_rss; BVHModel s2_rss; - generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); - generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); + generateBVHModel(s1_rss, s1, Transform3s(), 16, 16); + generateBVHModel(s2_rss, s2, Transform3s(), 16, 16); DistanceRequest request; DistanceResult res, res1; - Transform3f pose; + Transform3s pose; pose.setTranslation(Vec3s(20, 0, 0)); res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -851,27 +851,27 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { res.clear(); res1.clear(); - distance(&s1, Transform3f(), &s2, pose, request, res); - distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2, pose, request, res); + distance(&s1_rss, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1, Transform3f(), &s2_rss, pose, request, res1); + distance(&s1, Transform3s(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); - distance(&s1_rss, Transform3f(), &s2, pose, request, res1); + distance(&s1_rss, Transform3s(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for (std::size_t i = 0; i < 10; ++i) { - Transform3f t; + Transform3s t; generateRandomTransform(extents, t); - Transform3f pose1(t); - Transform3f pose2 = t * pose; + Transform3s pose1(t); + Transform3s pose2 = t * pose; res.clear(); res1.clear(); @@ -900,17 +900,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -918,31 +918,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(40, 0, 0)); @@ -950,31 +950,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(30, 0, 0)); @@ -982,31 +982,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(29.9, 0, 0)); @@ -1016,31 +1016,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-29.9, 0, 0)); @@ -1050,31 +1050,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-30, 0, 0)); @@ -1082,31 +1082,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) { pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1119,17 +1119,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f()); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f()); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s()); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s()); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1137,31 +1137,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(15.01, 0, 0)); @@ -1169,31 +1169,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(14.99, 0, 0)); @@ -1201,31 +1201,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) { pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } @@ -1238,17 +1238,17 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1256,31 +1256,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(22.4, 0, 0)); @@ -1288,31 +1288,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(22.51, 0, 0)); @@ -1320,31 +1320,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) { pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1357,48 +1357,48 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.99, 0, 0)); pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.01, 0, 0)); @@ -1406,31 +1406,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) { pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1443,48 +1443,48 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); CollisionResult result; bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.9, 0, 0)); pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.1, 0, 0)); @@ -1492,31 +1492,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(0, 0, 9.9)); @@ -1524,31 +1524,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(0, 0, 10.1)); @@ -1556,31 +1556,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) { pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1592,10 +1592,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); @@ -1603,7 +1603,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1611,31 +1611,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(40, 0, 0)); @@ -1643,31 +1643,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { pose_obb.setTranslation(Vec3s(40, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(30, 0, 0)); @@ -1675,31 +1675,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { pose_obb.setTranslation(Vec3s(30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(29.9, 0, 0)); @@ -1709,31 +1709,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-29.9, 0, 0)); @@ -1743,31 +1743,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(-30, 0, 0)); @@ -1775,31 +1775,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { pose_obb.setTranslation(Vec3s(-30, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -1812,10 +1812,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f()); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f()); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s()); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s()); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); @@ -1823,7 +1823,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1831,31 +1831,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(15.01, 0, 0)); @@ -1863,31 +1863,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { pose_obb.setTranslation(Vec3s(15.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(14.99, 0, 0)); @@ -1895,31 +1895,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { pose_obb.setTranslation(Vec3s(14.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } @@ -1932,10 +1932,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f()); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f()); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s()); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s()); CollisionRequest request(false, 1, false); @@ -1943,7 +1943,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision @@ -1951,31 +1951,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(22.4, 0, 0)); @@ -1983,31 +1983,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { pose_obb.setTranslation(Vec3s(22.4, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(22.51, 0, 0)); @@ -2015,31 +2015,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { pose_obb.setTranslation(Vec3s(22.51, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -2052,10 +2052,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); @@ -2063,38 +2063,38 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.99, 0, 0)); pose_aabb.setTranslation(Vec3s(9.99, 0, 0)); pose_obb.setTranslation(Vec3s(9.99, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.01, 0, 0)); @@ -2102,31 +2102,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { pose_obb.setTranslation(Vec3s(10.01, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } @@ -2139,10 +2139,10 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { BVHModel s1_obb; BVHModel s2_obb; - generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); - generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); - generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); + generateBVHModel(s1_aabb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_aabb, s2, Transform3s(), 16, 16); + generateBVHModel(s1_obb, s1, Transform3s(), 16, 16); + generateBVHModel(s2_obb, s2, Transform3s(), 16, 16); CollisionRequest request(false, 1, false); @@ -2150,38 +2150,38 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { bool res; - Transform3f pose, pose_aabb, pose_obb; + Transform3s pose, pose_aabb, pose_obb; pose.setTranslation(Vec3s(9.9, 0, 0)); pose_aabb.setTranslation(Vec3s(9.9, 0, 0)); pose_obb.setTranslation(Vec3s(9.9, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(10.1, 0, 0)); @@ -2189,31 +2189,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { pose_obb.setTranslation(Vec3s(10.1, 0, 0)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3s(0, 0, 9.9)); @@ -2221,31 +2221,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { pose_obb.setTranslation(Vec3s(0, 0, 9.9)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3s(0, 0, 10.1)); @@ -2253,30 +2253,30 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { pose_obb.setTranslation(Vec3s(0, 0, 10.1)); result.clear(); - res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_aabb, pose_aabb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); + res = (collide(&s2_obb, pose_obb, &s1, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); + res = (collide(&s1, Transform3s(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_aabb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); + res = (collide(&s2, pose, &s1_obb, Transform3s(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); - res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); + res = (collide(&s1_aabb, Transform3s(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index b1a687a91..e43d92ec2 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -114,8 +114,8 @@ int line; struct SweptSphereGJKSolver : public GJKSolver { template CoalScalar shapeDistance( - const S1& s1, const Transform3f& tf1, const S2& s2, - const Transform3f& tf2, bool compute_penetration, Vec3s& p1, Vec3s& p2, + const S1& s1, const Transform3s& tf1, const S2& s2, + const Transform3s& tf2, bool compute_penetration, Vec3s& p1, Vec3s& p2, Vec3s& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const { if (use_swept_sphere_radius_in_gjk_epa_iterations) { CoalScalar distance; @@ -146,8 +146,8 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { CoalScalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 10; - std::vector tf1s; - std::vector tf2s; + std::vector tf1s; + std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); const std::array swept_sphere_radius = {0, 0.1, 1., 10.}; @@ -157,8 +157,8 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { for (const CoalScalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { - Transform3f tf1 = tf1s[i]; - Transform3f tf2 = tf2s[i]; + Transform3s tf1 = tf1s[i]; + Transform3s tf2 = tf2s[i]; SET_LINE; @@ -283,8 +283,8 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { CoalScalar extents[] = {-2, -2, -2, 2, 2, 2}; std::size_t n = 1; - std::vector tf1s; - std::vector tf2s; + std::vector tf1s; + std::vector tf2s; generateRandomTransforms(extents, tf1s, n); generateRandomTransforms(extents, tf2s, n); @@ -294,8 +294,8 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { for (const CoalScalar& ssr2 : swept_sphere_radius) { shape2.setSweptSphereRadius(ssr2); for (std::size_t i = 0; i < n; ++i) { - Transform3f tf1 = tf1s[i]; - Transform3f tf2 = tf2s[i]; + Transform3s tf1 = tf1s[i]; + Transform3s tf2 = tf2s[i]; SET_LINE; CollisionRequest request; diff --git a/test/utility.cpp b/test/utility.cpp index 80efceba4..714469f01 100644 --- a/test/utility.cpp +++ b/test/utility.cpp @@ -207,7 +207,7 @@ void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3s& R) { -c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3; } -void generateRandomTransform(CoalScalar extents[6], Transform3f& transform) { +void generateRandomTransform(CoalScalar extents[6], Transform3s& transform) { CoalScalar x = rand_interval(extents[0], extents[3]); CoalScalar y = rand_interval(extents[1], extents[4]); CoalScalar z = rand_interval(extents[2], extents[5]); @@ -224,7 +224,7 @@ void generateRandomTransform(CoalScalar extents[6], Transform3f& transform) { } void generateRandomTransforms(CoalScalar extents[6], - std::vector& transforms, + std::vector& transforms, std::size_t n) { transforms.resize(n); for (std::size_t i = 0; i < n; ++i) { @@ -248,8 +248,8 @@ void generateRandomTransforms(CoalScalar extents[6], void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], CoalScalar delta_rot, - std::vector& transforms, - std::vector& transforms2, + std::vector& transforms, + std::vector& transforms2, std::size_t n) { transforms.resize(n); transforms2.resize(n); @@ -376,7 +376,7 @@ Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z) { return q; } -std::ostream& operator<<(std::ostream& os, const Transform3f& tf) { +std::ostream& operator<<(std::ostream& os, const Transform3s& tf) { return os << "[ " << tf.getTranslation().format(vfmt) << ", " << tf.getQuatRotation().coeffs().format(vfmt) << " ]"; } @@ -393,7 +393,7 @@ void generateEnvironments(std::vector& env, CoalScalar env_scale, std::size_t n) { CoalScalar extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; - std::vector transforms(n); + std::vector transforms(n); generateRandomTransforms(extents, transforms, n); for (std::size_t i = 0; i < n; ++i) { @@ -424,13 +424,13 @@ void generateEnvironmentsMesh(std::vector& env, CoalScalar env_scale, std::size_t n) { CoalScalar extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; - std::vector transforms; + std::vector transforms; generateRandomTransforms(extents, transforms, n); Box box(5, 10, 20); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); - generateBVHModel(*model, box, Transform3f::Identity()); + generateBVHModel(*model, box, Transform3s::Identity()); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); @@ -440,7 +440,7 @@ void generateEnvironmentsMesh(std::vector& env, Sphere sphere(30); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); - generateBVHModel(*model, sphere, Transform3f::Identity(), 16, 16); + generateBVHModel(*model, sphere, Transform3s::Identity(), 16, 16); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); @@ -450,7 +450,7 @@ void generateEnvironmentsMesh(std::vector& env, Cylinder cylinder(10, 40); for (std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); - generateBVHModel(*model, cylinder, Transform3f::Identity(), 16, 16); + generateBVHModel(*model, cylinder, Transform3s::Identity(), 16, 16); env.push_back(new CollisionObject(shared_ptr(model), transforms[i])); env.back()->collisionGeometry()->computeLocalAABB(); diff --git a/test/utility.h b/test/utility.h index 2969bb288..6be386e5e 100644 --- a/test/utility.h +++ b/test/utility.h @@ -146,12 +146,12 @@ coal::OcTree loadOctreeFile(const std::string& filename, /// extents and rotation without constraints. The translation is (x, y, z), and /// extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= /// z <= extents[5] -void generateRandomTransform(CoalScalar extents[6], Transform3f& transform); +void generateRandomTransform(CoalScalar extents[6], Transform3s& transform); /// @brief Generate n random transforms whose translations are constrained by /// extents. void generateRandomTransforms(CoalScalar extents[6], - std::vector& transforms, + std::vector& transforms, std::size_t n); /// @brief Generate n random transforms whose translations are constrained by @@ -159,8 +159,8 @@ void generateRandomTransforms(CoalScalar extents[6], /// translation & rotation to the transforms generated. void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3], CoalScalar delta_rot, - std::vector& transforms, - std::vector& transforms2, + std::vector& transforms, + std::vector& transforms2, std::size_t n); /// @ brief Structure for minimum distance between two meshes and the @@ -186,7 +186,7 @@ std::string getNodeTypeName(NODE_TYPE node_type); Quatf makeQuat(CoalScalar w, CoalScalar x, CoalScalar y, CoalScalar z); -std::ostream& operator<<(std::ostream& os, const Transform3f& tf); +std::ostream& operator<<(std::ostream& os, const Transform3s& tf); /// Get the argument --nb-run from argv std::size_t getNbRun(const int& argc, char const* const* argv, From 7651802396709b8c2f62af64230b1d25a574e21e Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 17:39:18 +0200 Subject: [PATCH 17/57] readme: update examples with new "coal" library name --- README.md | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index d731a1727..efc063e1b 100644 --- a/README.md +++ b/README.md @@ -94,10 +94,10 @@ Here is an example of using HPP-FCL in C++: // GJK or EPA can be called with this object. // Consequently, after creating the BVH structure from the point cloud, this function // also computes its convex hull. -std::shared_ptr loadConvexMesh(const std::string& file_name) { - hpp::fcl::NODE_TYPE bv_type = hpp::fcl::BV_AABB; - hpp::fcl::MeshLoader loader(bv_type); - hpp::fcl::BVHModelPtr_t bvh = loader.load(file_name); +std::shared_ptr loadConvexMesh(const std::string& file_name) { + coal::NODE_TYPE bv_type = coal::BV_AABB; + coal::MeshLoader loader(bv_type); + coal::BVHModelPtr_t bvh = loader.load(file_name); bvh->buildConvexHull(true, "Qt"); return bvh->convex; } @@ -107,16 +107,16 @@ int main() { // Hppfcl supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes, // halfspace and convex meshes (i.e. convex hulls of clouds of points). // It also supports BVHs (bounding volumes hierarchies), height-fields and octrees. - std::shared_ptr shape1 = std::make_shared(0.7, 1.0, 0.8); - std::shared_ptr shape2 = loadConvexMesh("../path/to/mesh/file.obj"); + std::shared_ptr shape1 = std::make_shared(0.7, 1.0, 0.8); + std::shared_ptr shape2 = loadConvexMesh("../path/to/mesh/file.obj"); // Define the shapes' placement in 3D space - hpp::fcl::Transform3s T1; - T1.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); - T1.setTranslation(hpp::fcl::Vec3s::Random()); - hpp::fcl::Transform3s T2 = hpp::fcl::Transform3s::Identity(); - T2.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom()); - T2.setTranslation(hpp::fcl::Vec3s::Random()); + coal::Transform3s T1; + T1.setQuatRotation(coal::Quaternion3f::UnitRandom()); + T1.setTranslation(coal::Vec3s::Random()); + coal::Transform3s T2 = coal::Transform3s::Identity(); + T2.setQuatRotation(coal::Quaternion3f::UnitRandom()); + T2.setTranslation(coal::Vec3s::Random()); // Define collision requests and results. // @@ -127,19 +127,19 @@ int main() { // Setting a positive security margin can be usefull in motion planning, // i.e to prevent shapes from getting too close to one another. // In physics simulation, allowing a negative security margin may be usefull to stabilize the simulation. - hpp::fcl::CollisionRequest col_req; + coal::CollisionRequest col_req; col_req.security_margin = 1e-1; // A collision result stores the result of the collision test (signed distance between the shapes, // witness points location, normal etc.) - hpp::fcl::CollisionResult col_res; + coal::CollisionResult col_res; // Collision call - hpp::fcl::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res); + coal::collide(shape1.get(), T1, shape2.get(), T2, col_req, col_res); // We can access the collision result once it has been populated std::cout << "Collision? " << col_res.isCollision() << "\n"; if (col_res.isCollision()) { - hpp::fcl::Contact contact = col_res.getContact(0); + coal::Contact contact = col_res.getContact(0); // The penetration depth does **not** take into account the security margin. // Consequently, the penetration depth is the true signed distance which separates the shapes. // To have the distance which takes into account the security margin, we can simply add the two together. From 835c582d4b6be1d634cc812906c9c5d5c8f82a61 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 17:40:10 +0200 Subject: [PATCH 18/57] fwd: adding hpp-fcl backward compatibility --- include/coal/fwd.hh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh index 8be5e49a8..859e21a4e 100644 --- a/include/coal/fwd.hh +++ b/include/coal/fwd.hh @@ -135,6 +135,7 @@ class CollisionGeometry; typedef shared_ptr CollisionGeometryPtr_t; typedef shared_ptr CollisionGeometryConstPtr_t; class Transform3s; +using Transform3f = Transform3s; // For backward compatibility class AABB; @@ -146,4 +147,7 @@ typedef shared_ptr OcTreePtr_t; typedef shared_ptr OcTreeConstPtr_t; } // namespace coal +// For backward compatibility +namespace hpp::fcl = coal; + #endif // COAL_FWD_HH From 9e86f41c809c956a8f46663600cf34a012df79a7 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 17:42:19 +0200 Subject: [PATCH 19/57] package.xml: update list of maintainers --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 216b4a192..0428aa2f2 100644 --- a/package.xml +++ b/package.xml @@ -7,6 +7,7 @@ Please check the repository URL for full list of authors and maintainers. --> Joseph Mirabel Justin Carpentier + Louis Montaut Wolfgang Merkt BSD From 0fe07a261aee33e72a407f41dfcebd30f1dfaf74 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Sun, 23 Jun 2024 19:05:14 +0200 Subject: [PATCH 20/57] python: fix stubs generation --- python/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 95560122d..cab976533 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -162,8 +162,7 @@ INSTALL(TARGETS ${PYTHON_LIB_NAME} # --- GENERATE STUBS IF(GENERATE_PYTHON_STUBS) LOAD_STUBGEN() - GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${PYTHON_LIB_NAME} - ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME} ${PROJECT_NAME}) + GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${PROJECT_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME}) ENDIF(GENERATE_PYTHON_STUBS) # --- INSTALL SCRIPTS From 83ad9dc14f9b980fd92fafa36e1b455b94bbf0dd Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 24 Jun 2024 12:31:40 +0200 Subject: [PATCH 21/57] Update github workflow --- .../workflows/conda/environment_macos_linux.yml | 2 +- .github/workflows/conda/environment_windows.yml | 2 +- .github/workflows/macos-linux-conda.yml | 14 +++++++------- .github/workflows/macos-linux-pip.yml | 6 +++--- .github/workflows/windows-conda-clang.yml | 10 +++++----- .github/workflows/windows-conda-v142.yml | 10 +++++----- 6 files changed, 22 insertions(+), 22 deletions(-) diff --git a/.github/workflows/conda/environment_macos_linux.yml b/.github/workflows/conda/environment_macos_linux.yml index 4a4655fbd..19d260224 100644 --- a/.github/workflows/conda/environment_macos_linux.yml +++ b/.github/workflows/conda/environment_macos_linux.yml @@ -1,4 +1,4 @@ -name: fcl +name: coal channels: - conda-forge - nodefaults diff --git a/.github/workflows/conda/environment_windows.yml b/.github/workflows/conda/environment_windows.yml index 8c6fccaed..92c01c422 100644 --- a/.github/workflows/conda/environment_windows.yml +++ b/.github/workflows/conda/environment_windows.yml @@ -1,4 +1,4 @@ -name: fcl +name: coal channels: - conda-forge - nodefaults diff --git a/.github/workflows/macos-linux-conda.yml b/.github/workflows/macos-linux-conda.yml index a875f175c..913ed513b 100644 --- a/.github/workflows/macos-linux-conda.yml +++ b/.github/workflows/macos-linux-conda.yml @@ -1,9 +1,9 @@ -name: Build hpp-fcl for Mac OS X/Linux via Conda +name: Build Coal for Mac OS X/Linux via Conda on: [push,pull_request] jobs: - hpp-fcl-conda: + coal-conda: name: CI on ${{ matrix.os }} with Conda Python ${{ matrix.python-version }} - ${{ matrix.build_type }} ${{ matrix.cxx_options }} runs-on: ${{ matrix.os }} env: @@ -40,13 +40,13 @@ jobs: - uses: conda-incubator/setup-miniconda@v3 with: - activate-environment: fcl + activate-environment: coal auto-update-conda: true environment-file: .github/workflows/conda/environment_macos_linux.yml python-version: ${{ matrix.python-version }} auto-activate-base: false - - name: Build hpp-fcl + - name: Build Coal shell: bash -el {0} run: | conda list @@ -65,14 +65,14 @@ jobs: -DCMAKE_CXX_FLAGS=${{ matrix.cxx_options }} \ -DPYTHON_EXECUTABLE=$(which python3) \ -DGENERATE_PYTHON_STUBS=ON \ - -DHPP_FCL_HAS_QHULL=ON \ + -DCOAL_HAS_QHULL=ON \ -DBUILD_DOCUMENTATION=ON \ -DINSTALL_DOCUMENTATION=ON cmake --build . -j2 ctest --output-on-failure cmake --install . - - name: Uninstall hpp-fcl + - name: Uninstall Coal shell: bash -el {0} run: | cd build @@ -83,7 +83,7 @@ jobs: name: check-macos-linux-conda needs: - - hpp-fcl-conda + - coal-conda runs-on: Ubuntu-latest diff --git a/.github/workflows/macos-linux-pip.yml b/.github/workflows/macos-linux-pip.yml index 2ced0264f..156a86c1e 100644 --- a/.github/workflows/macos-linux-pip.yml +++ b/.github/workflows/macos-linux-pip.yml @@ -1,4 +1,4 @@ -name: Build hpp-fcl for Mac OS X/Linux via pip +name: Build Coal for Mac OS X/Linux via pip on: [push, pull_request] @@ -7,7 +7,7 @@ env: CTEST_PARALLEL_LEVEL: 4 jobs: - hpp-fcl-pip: + coal-pip: name: "CI on ${{ matrix.os }} / py ${{ matrix.python-version }} with pip" runs-on: "${{ matrix.os }}-latest" @@ -29,6 +29,6 @@ jobs: - run: python -m pip install -U pip - run: python -m pip install cmeel-assimp cmeel-octomap cmeel-qhull eigenpy[build] - run: echo "CMAKE_PREFIX_PATH=$(cmeel cmake)" >> $GITHUB_ENV - - run: cmake -B build -S . -DHPP_FCL_HAS_QHULL=ON + - run: cmake -B build -S . -DCOAL_HAS_QHULL=ON - run: cmake --build build -j 4 - run: cmake --build build -t test diff --git a/.github/workflows/windows-conda-clang.yml b/.github/workflows/windows-conda-clang.yml index 33fc913ca..7d7a8d87a 100644 --- a/.github/workflows/windows-conda-clang.yml +++ b/.github/workflows/windows-conda-clang.yml @@ -1,4 +1,4 @@ -name: Build FCL for Windows (CLANG) via Conda +name: Build Coal for Windows (CLANG) via Conda on: [push,pull_request] jobs: @@ -33,13 +33,13 @@ jobs: - uses: conda-incubator/setup-miniconda@v3 with: - activate-environment: fcl + activate-environment: coal auto-update-conda: true environment-file: .github/workflows/conda/environment_windows.yml python-version: "3.10" auto-activate-base: false - - name: Build FCL + - name: Build Coal shell: cmd /C CALL {0} run: | call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64 @@ -59,7 +59,7 @@ jobs: -DGENERATE_PYTHON_STUBS=ON ^ -DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^ -DPYTHON_EXECUTABLE=%CONDA_PREFIX%\python.exe ^ - -DHPP_FCL_HAS_QHULL=ON ^ + -DCOAL_HAS_QHULL=ON ^ -DBUILD_PYTHON_INTERFACE=ON ^ .. if errorlevel 1 exit 1 @@ -74,7 +74,7 @@ jobs: :: Test Python import cd .. - python -c "import hppfcl" + python -c "import coal" if errorlevel 1 exit 1 check: diff --git a/.github/workflows/windows-conda-v142.yml b/.github/workflows/windows-conda-v142.yml index e09444ba3..e1191d014 100644 --- a/.github/workflows/windows-conda-v142.yml +++ b/.github/workflows/windows-conda-v142.yml @@ -1,4 +1,4 @@ -name: Build FCL for Windows (v142) via Conda +name: Build Coal for Windows (v142) via Conda on: [push,pull_request] jobs: @@ -32,13 +32,13 @@ jobs: - uses: conda-incubator/setup-miniconda@v3 with: - activate-environment: fcl + activate-environment: coal auto-update-conda: true environment-file: .github/workflows/conda/environment_windows.yml python-version: "3.10" auto-activate-base: false - - name: Build FCL + - name: Build Coal shell: cmd /C CALL {0} run: | call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64 @@ -56,7 +56,7 @@ jobs: -DGENERATE_PYTHON_STUBS=ON ^ -DPYTHON_SITELIB=%CONDA_PREFIX%\Lib\site-packages ^ -DPYTHON_EXECUTABLE=%CONDA_PREFIX%\python.exe ^ - -DHPP_FCL_HAS_QHULL=ON ^ + -DCOAL_HAS_QHULL=ON ^ -DBUILD_PYTHON_INTERFACE=ON ^ .. if errorlevel 1 exit 1 @@ -71,7 +71,7 @@ jobs: :: Test Python import cd .. - python -c "import hppfcl" + python -c "import coal" if errorlevel 1 exit 1 check: From c8366034963f9e9f833ef0a70577372990bf95a3 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 24 Jun 2024 12:46:03 +0200 Subject: [PATCH 22/57] fwd: fix backward compat of `hpp::fcl` namespace --- include/coal/fwd.hh | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh index 859e21a4e..d90498879 100644 --- a/include/coal/fwd.hh +++ b/include/coal/fwd.hh @@ -135,7 +135,6 @@ class CollisionGeometry; typedef shared_ptr CollisionGeometryPtr_t; typedef shared_ptr CollisionGeometryConstPtr_t; class Transform3s; -using Transform3f = Transform3s; // For backward compatibility class AABB; @@ -147,7 +146,11 @@ typedef shared_ptr OcTreePtr_t; typedef shared_ptr OcTreeConstPtr_t; } // namespace coal -// For backward compatibility -namespace hpp::fcl = coal; +namespace hpp { +namespace fcl { +using namespace coal; +using Transform3f = Transform3s; // For backward compatibility +} // namespace fcl +} // namespace hpp #endif // COAL_FWD_HH From fb34d214bc92b997e89a42e08aafd3bf69337cac Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 24 Jun 2024 12:56:50 +0200 Subject: [PATCH 23/57] cmake: add macro for hpp-fcl retro compatibility --- CMakeLists.txt | 3 ++- include/coal/data_types.h | 2 ++ include/coal/fwd.hh | 2 ++ src/CMakeLists.txt | 4 ++++ 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 081f6ebd9..c6481a3fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,8 +50,9 @@ SET(DOXYGEN_USE_TEMPLATE_CSS TRUE) # Need to be set before including base.cmake # ---------------------------------------------------- option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) -option(COAL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical COAL asserts to exception." FALSE) +option(COAL_TURN_ASSERT_INTO_EXCEPTION "Turn some critical Coal asserts to exception." FALSE) option(COAL_ENABLE_LOGGING "Activate logging for warnings or error messages. Turned on by default in Debug." FALSE) +option(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL "Make Coal retro-compatible with HPP-FCL." FALSE) # Check if the submodule cmake have been initialized set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") diff --git a/include/coal/data_types.h b/include/coal/data_types.h index ea8c97d3e..11d34a981 100644 --- a/include/coal/data_types.h +++ b/include/coal/data_types.h @@ -59,6 +59,7 @@ #endif // COAL_HAS_OCTOMAP namespace coal { +#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL // We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward // compatibility. typedef double FCL_REAL; @@ -70,6 +71,7 @@ typedef Eigen::Matrix Matrix3f; typedef Eigen::Matrix Matrixx3f; typedef Eigen::Matrix Matrixx2f; typedef Eigen::Matrix MatrixXf; +#endif typedef double CoalScalar; typedef Eigen::Matrix Vec3s; diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh index d90498879..71ffb5124 100644 --- a/include/coal/fwd.hh +++ b/include/coal/fwd.hh @@ -146,11 +146,13 @@ typedef shared_ptr OcTreePtr_t; typedef shared_ptr OcTreeConstPtr_t; } // namespace coal +#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL namespace hpp { namespace fcl { using namespace coal; using Transform3f = Transform3s; // For backward compatibility } // namespace fcl } // namespace hpp +#endif #endif // COAL_FWD_HH diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 99e6c4298..0228bf11e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -234,6 +234,10 @@ if (COAL_TURN_ASSERT_INTO_EXCEPTION) target_compile_definitions(${LIBRARY_NAME} PUBLIC -DCOAL_TURN_ASSERT_INTO_EXCEPTION) endif() +if (COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + target_compile_definitions(${LIBRARY_NAME} PUBLIC -DCOAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) +endif() + if(COAL_HAS_QHULL) target_compile_definitions(${LIBRARY_NAME} PRIVATE -DCOAL_HAS_QHULL) if (COAL_USE_SYSTEM_QHULL) From 872b2f84c0ad12ef707b4bb6bf9417c68b2bada1 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 24 Jun 2024 15:21:49 +0200 Subject: [PATCH 24/57] python: fix windows build --- python/CMakeLists.txt | 3 ++ python/coal/__init__.py | 29 ++++++++++++- python/coal/windows_dll_manager.py | 65 ++++++++++++++++++++++++++++++ 3 files changed, 95 insertions(+), 2 deletions(-) create mode 100644 python/coal/windows_dll_manager.py diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index cab976533..ca6e00e13 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -142,6 +142,8 @@ SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES SUFFIX "${PYTHON_EXT_SUFFIX}" OUTPUT_NAME "${PYTHON_LIB_NAME}" LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}" + # On Windows, shared library are treat as binary + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}" ) IF(IS_ABSOLUTE ${PYTHON_SITELIB}) @@ -169,6 +171,7 @@ ENDIF(GENERATE_PYTHON_STUBS) SET(PYTHON_FILES __init__.py viewer.py + windows_dll_manager.py ) FOREACH(python ${PYTHON_FILES}) diff --git a/python/coal/__init__.py b/python/coal/__init__.py index e26c55ab9..966642738 100644 --- a/python/coal/__init__.py +++ b/python/coal/__init__.py @@ -32,5 +32,30 @@ # POSSIBILITY OF SUCH DAMAGE. # ruff: noqa: F401, F403 -from .coal_pywrap import * -from .coal_pywrap import __raw_version__, __version__ + +# On Windows, if coal.dll is not in the same directory than +# the .pyd, it will not be loaded. +# We first try to load coal, then, if it fail and we are on Windows: +# 1. We add all paths inside COAL_WINDOWS_DLL_PATH to DllDirectory +# 2. If COAL_WINDOWS_DLL_PATH we add the relative path from the +# package directory to the bin directory to DllDirectory +# This solution is inspired from: +# - https://github.com/PixarAnimationStudios/OpenUSD/pull/1511/files +# - https://stackoverflow.com/questions/65334494/python-c-extension-packaging-dll-along-with-pyd +# More resources on https://github.com/diffpy/pyobjcryst/issues/33 +try: + from .coal_pywrap import * # noqa + from .coal_pywrap import __raw_version__, __version__ +except ImportError: + import platform + + if platform.system() == "Windows": + from .windows_dll_manager import build_directory_manager, get_dll_paths + + with build_directory_manager() as dll_dir_manager: + for p in get_dll_paths(): + dll_dir_manager.add_dll_directory(p) + from .coal_pywrap import * # noqa + from .coal_pywrap import __raw_version__, __version__ # noqa + else: + raise diff --git a/python/coal/windows_dll_manager.py b/python/coal/windows_dll_manager.py new file mode 100644 index 000000000..92516fee1 --- /dev/null +++ b/python/coal/windows_dll_manager.py @@ -0,0 +1,65 @@ +import contextlib +import os +import sys + + +def get_dll_paths(): + coal_paths = os.getenv("COAL_WINDOWS_DLL_PATH") + if coal_paths is None: + # From https://peps.python.org/pep-0250/#implementation + # lib/python-version/site-packages/package + RELATIVE_DLL_PATH1 = "..\\..\\..\\..\\bin" + # lib/site-packages/package + RELATIVE_DLL_PATH2 = "..\\..\\..\\bin" + # For unit test + RELATIVE_DLL_PATH3 = "..\\..\\bin" + return [ + os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH1), + os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH2), + os.path.join(os.path.dirname(__file__), RELATIVE_DLL_PATH3), + ] + else: + return coal_paths.split(os.pathsep) + + +class PathManager(contextlib.AbstractContextManager): + """Restore PATH state after importing Python module""" + + def add_dll_directory(self, dll_dir: str): + os.environ["PATH"] += os.pathsep + dll_dir + + def __enter__(self): + self.old_path = os.environ["PATH"] + return self + + def __exit__(self, *exc_details): + os.environ["PATH"] = self.old_path + + +class DllDirectoryManager(contextlib.AbstractContextManager): + """Restore DllDirectory state after importing Python module""" + + def add_dll_directory(self, dll_dir: str): + # add_dll_directory can fail on relative path and non + # existing path. + # Since we don't know all the fail criterion we just ignore + # thrown exception + try: + self.dll_dirs.append(os.add_dll_directory(dll_dir)) + except OSError: + pass + + def __enter__(self): + self.dll_dirs = [] + return self + + def __exit__(self, *exc_details): + for d in self.dll_dirs: + d.close() + + +def build_directory_manager(): + if sys.version_info >= (3, 8): + return DllDirectoryManager() + else: + return PathManager() From 4eabdaa4ecc2f321cd23202e7727363c31b0cf68 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 24 Jun 2024 16:10:08 +0200 Subject: [PATCH 25/57] python stubs: add missing project_name --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ca6e00e13..bbdf917b3 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -164,7 +164,7 @@ INSTALL(TARGETS ${PYTHON_LIB_NAME} # --- GENERATE STUBS IF(GENERATE_PYTHON_STUBS) LOAD_STUBGEN() - GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${PROJECT_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME}) + GENERATE_STUBS(${CMAKE_CURRENT_BINARY_DIR} ${PROJECT_NAME} ${ABSOLUTE_PYTHON_SITELIB} ${PYTHON_LIB_NAME} ${PROJECT_NAME}) ENDIF(GENERATE_PYTHON_STUBS) # --- INSTALL SCRIPTS From 43aee74faf2ed6f93113041a79e1adad93c01fa5 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 1 Jul 2024 17:00:28 +0200 Subject: [PATCH 26/57] all: fix missing macros --- include/coal/fwd.hh | 8 ++++---- include/coal/internal/traversal_node_setup.h | 6 +++--- include/coal/serialization/fwd.h | 4 ++-- python/collision.cc | 12 ++++++------ python/distance.cc | 6 +++--- test/collision.cpp | 12 ++++++------ 6 files changed, 24 insertions(+), 24 deletions(-) diff --git a/include/coal/fwd.hh b/include/coal/fwd.hh index 71ffb5124..c2408880a 100644 --- a/include/coal/fwd.hh +++ b/include/coal/fwd.hh @@ -99,22 +99,22 @@ // GCC version 4.6 and higher supports -Wmaybe-uninitialized #if (defined(__GNUC__) && \ ((__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))) -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ _Pragma("GCC diagnostic ignored \"-Wmaybe-uninitialized\"") // Use __has_warning with clang. Clang introduced it in 2024 (3.5+) #elif (defined(__clang__) && defined(__has_warning) && \ __has_warning("-Wmaybe-uninitialized")) -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ _Pragma("clang diagnostic ignored \"-Wmaybe-uninitialized\"") #else -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED #endif #elif defined(WIN32) #define COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("warning(push)") #define COAL_COMPILER_DIAGNOSTIC_POP _Pragma("warning(pop)") #define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS \ _Pragma("warning(disable : 4996)") -#define HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ +#define COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED \ _Pragma("warning(disable : 4700)") #else #define COAL_COMPILER_DIAGNOSTIC_PUSH diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h index 4f892a02a..120633edc 100644 --- a/include/coal/internal/traversal_node_setup.h +++ b/include/coal/internal/traversal_node_setup.h @@ -681,13 +681,13 @@ bool initialize(MeshDistanceTraversalNode& node, node.tri_indices2 = model2.tri_indices.get() ? model2.tri_indices->data() : NULL; - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.RT.R, node.RT.T); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP return true; } diff --git a/include/coal/serialization/fwd.h b/include/coal/serialization/fwd.h index e56e661b3..3ed363acd 100644 --- a/include/coal/serialization/fwd.h +++ b/include/coal/serialization/fwd.h @@ -74,10 +74,10 @@ struct cast_register_initializer { void init(std::false_type) const {} cast_register_initializer const& init() const { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_PUSH _Pragma("GCC diagnostic ignored \"-Wconversion\"") BOOST_STATIC_WARNING((std::is_base_of::value)); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP init(std::is_base_of()); return *this; } diff --git a/python/collision.cc b/python/collision.cc index 2c92ad083..b50a88190 100644 --- a/python/collision.cc +++ b/python/collision.cc @@ -94,8 +94,8 @@ void exposeCollisionAPI() { .def("clear", &CPUTimes::clear, arg("self"), "Reset the time values."); } - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("QueryRequest", doxygen::class_doc(), no_init) @@ -132,10 +132,10 @@ void exposeCollisionAPI() { .DEF_RW_CLASS_ATTRIB(QueryRequest, enable_timings) .DEF_CLASS_FUNC(QueryRequest, updateGuess); } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "CollisionRequest", doxygen::class_doc(), no_init) @@ -174,7 +174,7 @@ void exposeCollisionAPI() { class_ >("StdVec_CollisionRequest") .def(vector_indexing_suite >()); } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP if (!eigenpy::register_symbolic_link_to_registered_type()) { class_("Contact", doxygen::class_doc(), diff --git a/python/distance.cc b/python/distance.cc index 1251428bb..f4c1a26a6 100644 --- a/python/distance.cc +++ b/python/distance.cc @@ -68,8 +68,8 @@ struct DistanceResultWrapper { }; void exposeDistanceAPI() { - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS if (!eigenpy::register_symbolic_link_to_registered_type()) { class_ >( "DistanceRequest", doxygen::class_doc(), @@ -111,7 +111,7 @@ void exposeDistanceAPI() { .DEF_RW_CLASS_ATTRIB(DistanceRequest, abs_err) .def(SerializableVisitor()); } - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP if (!eigenpy::register_symbolic_link_to_registered_type< std::vector >()) { diff --git a/test/collision.cpp b/test/collision.cpp index 04bddb945..70c8d2810 100644 --- a/test/collision.cpp +++ b/test/collision.cpp @@ -308,8 +308,8 @@ struct traits, Oriented, recursive> : base_traits { enum { IS_IMPLEMENTED = false }; }; -HPP_FCL_COMPILER_DIAGNOSTIC_PUSH -HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS +COAL_COMPILER_DIAGNOSTIC_PUSH +COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS struct mesh_mesh_run_test { mesh_mesh_run_test(const std::vector& _transforms, @@ -330,7 +330,7 @@ struct mesh_mesh_run_test { int indent; - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP const char* getindent() { assert(indent < 9); @@ -641,8 +641,8 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { << transforms[i].getQuatRotation().coeffs().format(f)); } - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH - HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS + COAL_COMPILER_DIAGNOSTIC_PUSH + COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS // Request all contacts and check that all methods give the same result. mesh_mesh_run_test runner( @@ -650,7 +650,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh) { runner.enable_statistics = true; boost::mpl::for_each >(runner); - HPP_FCL_COMPILER_DIAGNOSTIC_POP + COAL_COMPILER_DIAGNOSTIC_POP } BOOST_AUTO_TEST_CASE(mesh_mesh_benchmark) { From 99ab6e107258d8355cfaa5cd83dfbaf1a5617977 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 1 Jul 2024 19:07:34 +0200 Subject: [PATCH 27/57] serialization: fix include --- include/coal/serialization/fwd.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/coal/serialization/fwd.h b/include/coal/serialization/fwd.h index 3ed363acd..18164b334 100644 --- a/include/coal/serialization/fwd.h +++ b/include/coal/serialization/fwd.h @@ -18,7 +18,7 @@ #include #include -#include "coal/fwd.h" +#include "coal/fwd.hh" #include "coal/serialization/eigen.h" #define COAL_SERIALIZATION_SPLIT(Type) \ From 09ee974a54cf6df33be05d78acc912c16726add3 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 1 Jul 2024 19:09:43 +0200 Subject: [PATCH 28/57] test: fix comment --- test/swept_sphere_radius.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp index e43d92ec2..26df2cca3 100644 --- a/test/swept_sphere_radius.cpp +++ b/test/swept_sphere_radius.cpp @@ -124,7 +124,7 @@ struct SweptSphereGJKSolver : public GJKSolver { return distance; } - // Default behavior of hppfcl's GJKSolver + // Default behavior of coal's GJKSolver CoalScalar distance; this->runGJKAndEPA( s1, tf1, s2, tf2, compute_penetration, distance, p1, p2, normal); @@ -167,7 +167,7 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { std::array p2; std::array normal; - // Default hppfcl behavior - Don't take swept sphere radius into account + // Default coal behavior - Don't take swept sphere radius into account // during GJK/EPA iterations. Correct the solution afterwards. distance[0] = solver.shapeDistance(shape1, tf1, shape2, tf2, compute_penetration, @@ -180,7 +180,7 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) { // Precision is dependent on the swept-sphere radius. // The issue of precision does not come from the default behavior of - // hppfcl, but from the result in which we manually take the swept + // coal, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. const CoalScalar precision = 3 * sqrt(tol) + @@ -331,7 +331,7 @@ void test_collide_swept_sphere_radius(S1& shape1, S2& shape2) { // Precision is dependent on the swept sphere radii. // The issue of precision does not come from the default behavior of - // hppfcl, but from the result in which we manually take the swept + // coal, but from the result in which we manually take the swept // sphere radius into account in GJK/EPA iterations. const CoalScalar precision = 3 * sqrt(tol) + (1 / 100.0) * std::max(ssr1, ssr2); From b308deefadf6ea07844a87fc68b0113f9850ff07 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 1 Jul 2024 19:09:54 +0200 Subject: [PATCH 29/57] readme: update python example to use coal --- README.md | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index efc063e1b..79f0114c3 100644 --- a/README.md +++ b/README.md @@ -103,8 +103,8 @@ std::shared_ptr loadConvexMesh(const std::string& file_name) { } int main() { - // Create the hppfcl shapes. - // Hppfcl supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes, + // Create the coal shapes. + // Coal supports many primitive shapes: boxes, spheres, capsules, cylinders, ellipsoids, cones, planes, // halfspace and convex meshes (i.e. convex hulls of clouds of points). // It also supports BVHs (bounding volumes hierarchies), height-fields and octrees. std::shared_ptr shape1 = std::make_shared(0.7, 1.0, 0.8); @@ -161,7 +161,7 @@ int main() { Here is the C++ example from above translated in python using HPP-FCL's python bindings: ```python import numpy as np -import hppfcl +import coal # Optional: # The Pinocchio library is a rigid body algorithms library and has a handy SE3 module. # It can be installed as simply as `conda -c conda-forge install pinocchio`. @@ -169,36 +169,36 @@ import hppfcl import pinocchio as pin def loadConvexMesh(file_name: str): - loader = hppfcl.MeshLoader() - bvh: hppfcl.BVHModelBase = loader.load(file_name) + loader = coal.MeshLoader() + bvh: coal.BVHModelBase = loader.load(file_name) bvh.buildConvexHull(True, "Qt") return bvh.convex if __name__ == "__main__": - # Create hppfcl shapes - shape1 = hppfcl.Ellipsoid(0.7, 1.0, 0.8) + # Create coal shapes + shape1 = coal.Ellipsoid(0.7, 1.0, 0.8) shape2 = loadConvexMesh("../path/to/mesh/file.obj") # Define the shapes' placement in 3D space - T1 = hppfcl.Transform3s() + T1 = coal.Transform3s() T1.setTranslation(pin.SE3.Random().translation) T1.setRotation(pin.SE3.Random().rotation) - T2 = hppfcl.Transform3s(); + T2 = coal.Transform3s(); # Using np arrays also works T1.setTranslation(np.random.rand(3)) T2.setRotation(pin.SE3.Random().rotation) # Define collision requests and results - col_req = hppfcl.CollisionRequest() - col_res = hppfcl.CollisionResult() + col_req = coal.CollisionRequest() + col_res = coal.CollisionResult() # Collision call - hppfcl.collide(shape1, T1, shape2, T2, col_req, col_res) + coal.collide(shape1, T1, shape2, T2, col_req, col_res) # Accessing the collision result once it has been populated print("Is collision? ", {col_res.isCollision()}) if col_res.isCollision(): - contact: hppfcl.Contact = col_res.getContact(0) + contact: coal.Contact = col_res.getContact(0) print("Penetration depth: ", contact.penetration_depth) print("Distance between the shapes including the security margin: ", contact.penetration_depth + col_req.security_margin) print("Witness point shape1: ", contact.getNearestPoint1()) From 56e53d291b65f1af18d136816d0ce3745cb35d9d Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Fri, 19 Jul 2024 13:55:00 +0200 Subject: [PATCH 30/57] hpp-fcl compatibility (#1) * hpp-fcl-compat: headers * hpp-fcl-compat: python * hpp-fcl-compat: CMake * hpp-fcl-compat: skip serialization * Doc: no automoc for doxygen 1.9.8 and 1.10.0 And avoid those in conda environments Co-authored-by: Joris Vaillant --------- Co-authored-by: Joris Vaillant --- .../conda/environment_macos_linux.yml | 2 +- .../workflows/conda/environment_windows.yml | 2 +- CMakeLists.txt | 138 ++++++++++++++++++ hpp-fclConfig.cmake | 6 + include/coal/serialization/eigen.h | 6 + include/hpp/fcl/BV/AABB.h | 2 + include/hpp/fcl/BV/BV.h | 2 + include/hpp/fcl/BV/BV_node.h | 2 + include/hpp/fcl/BV/OBB.h | 2 + include/hpp/fcl/BV/OBBRSS.h | 2 + include/hpp/fcl/BV/RSS.h | 2 + include/hpp/fcl/BV/kDOP.h | 2 + include/hpp/fcl/BV/kIOS.h | 2 + include/hpp/fcl/BVH/BVH_front.h | 2 + include/hpp/fcl/BVH/BVH_internal.h | 2 + include/hpp/fcl/BVH/BVH_model.h | 2 + include/hpp/fcl/BVH/BVH_utility.h | 2 + include/hpp/fcl/broadphase/broadphase.h | 2 + include/hpp/fcl/broadphase/broadphase_SSaP.h | 2 + include/hpp/fcl/broadphase/broadphase_SaP.h | 2 + .../fcl/broadphase/broadphase_bruteforce.h | 2 + .../hpp/fcl/broadphase/broadphase_callbacks.h | 2 + .../broadphase/broadphase_collision_manager.h | 2 + ...adphase_continuous_collision_manager-inl.h | 2 + .../broadphase_continuous_collision_manager.h | 2 + .../broadphase_dynamic_AABB_tree-inl.h | 2 + .../broadphase/broadphase_dynamic_AABB_tree.h | 2 + .../broadphase_dynamic_AABB_tree_array-inl.h | 2 + .../broadphase_dynamic_AABB_tree_array.h | 2 + .../fcl/broadphase/broadphase_interval_tree.h | 2 + .../broadphase/broadphase_spatialhash-inl.h | 2 + .../fcl/broadphase/broadphase_spatialhash.h | 2 + .../broadphase/default_broadphase_callbacks.h | 2 + .../broadphase/detail/hierarchy_tree-inl.h | 2 + .../fcl/broadphase/detail/hierarchy_tree.h | 2 + .../detail/hierarchy_tree_array-inl.h | 2 + .../broadphase/detail/hierarchy_tree_array.h | 2 + .../hpp/fcl/broadphase/detail/interval_tree.h | 2 + .../broadphase/detail/interval_tree_node.h | 2 + .../hpp/fcl/broadphase/detail/morton-inl.h | 2 + include/hpp/fcl/broadphase/detail/morton.h | 2 + .../hpp/fcl/broadphase/detail/node_base-inl.h | 2 + include/hpp/fcl/broadphase/detail/node_base.h | 2 + .../broadphase/detail/node_base_array-inl.h | 2 + .../fcl/broadphase/detail/node_base_array.h | 2 + .../broadphase/detail/simple_hash_table-inl.h | 2 + .../fcl/broadphase/detail/simple_hash_table.h | 2 + .../broadphase/detail/simple_interval-inl.h | 2 + .../fcl/broadphase/detail/simple_interval.h | 2 + .../broadphase/detail/sparse_hash_table-inl.h | 2 + .../fcl/broadphase/detail/sparse_hash_table.h | 2 + .../fcl/broadphase/detail/spatial_hash-inl.h | 2 + .../hpp/fcl/broadphase/detail/spatial_hash.h | 2 + include/hpp/fcl/coal.hpp | 15 ++ include/hpp/fcl/collision.h | 2 + include/hpp/fcl/collision_data.h | 2 + include/hpp/fcl/collision_func_matrix.h | 2 + include/hpp/fcl/collision_object.h | 2 + include/hpp/fcl/collision_utility.h | 2 + include/hpp/fcl/config.hh | 2 + include/hpp/fcl/contact_patch.h | 2 + .../fcl/contact_patch/contact_patch_solver.h | 2 + .../contact_patch/contact_patch_solver.hxx | 2 + include/hpp/fcl/contact_patch_func_matrix.h | 2 + include/hpp/fcl/data_types.h | 2 + include/hpp/fcl/deprecated.hh | 2 + include/hpp/fcl/distance.h | 2 + include/hpp/fcl/distance_func_matrix.h | 2 + include/hpp/fcl/fwd.hh | 2 + include/hpp/fcl/hfield.h | 2 + include/hpp/fcl/internal/BV_fitter.h | 2 + include/hpp/fcl/internal/BV_splitter.h | 2 + include/hpp/fcl/internal/intersect.h | 2 + .../internal/shape_shape_contact_patch_func.h | 2 + include/hpp/fcl/internal/shape_shape_func.h | 2 + include/hpp/fcl/internal/tools.h | 2 + include/hpp/fcl/internal/traversal.h | 2 + .../hpp/fcl/internal/traversal_node_base.h | 2 + .../fcl/internal/traversal_node_bvh_shape.h | 2 + .../hpp/fcl/internal/traversal_node_bvhs.h | 2 + .../internal/traversal_node_hfield_shape.h | 2 + .../hpp/fcl/internal/traversal_node_octree.h | 2 + .../hpp/fcl/internal/traversal_node_setup.h | 2 + .../hpp/fcl/internal/traversal_node_shapes.h | 2 + include/hpp/fcl/internal/traversal_recurse.h | 2 + include/hpp/fcl/logging.h | 2 + include/hpp/fcl/math/matrix_3f.h | 2 + include/hpp/fcl/math/transform.h | 2 + include/hpp/fcl/math/types.h | 2 + include/hpp/fcl/math/vec_3f.h | 2 + include/hpp/fcl/mesh_loader/assimp.h | 2 + include/hpp/fcl/mesh_loader/loader.h | 2 + include/hpp/fcl/narrowphase/gjk.h | 2 + .../fcl/narrowphase/minkowski_difference.h | 2 + include/hpp/fcl/narrowphase/narrowphase.h | 2 + .../fcl/narrowphase/narrowphase_defaults.h | 2 + .../hpp/fcl/narrowphase/support_functions.h | 2 + include/hpp/fcl/octree.h | 2 + include/hpp/fcl/serialization/AABB.h | 2 + include/hpp/fcl/serialization/BVH_model.h | 2 + include/hpp/fcl/serialization/BV_node.h | 2 + include/hpp/fcl/serialization/BV_splitter.h | 2 + include/hpp/fcl/serialization/OBB.h | 2 + include/hpp/fcl/serialization/OBBRSS.h | 2 + include/hpp/fcl/serialization/RSS.h | 2 + include/hpp/fcl/serialization/archive.h | 2 + .../hpp/fcl/serialization/collision_data.h | 2 + .../hpp/fcl/serialization/collision_object.h | 2 + include/hpp/fcl/serialization/contact_patch.h | 2 + include/hpp/fcl/serialization/convex.h | 2 + include/hpp/fcl/serialization/eigen.h | 2 + include/hpp/fcl/serialization/fwd.h | 2 + .../hpp/fcl/serialization/geometric_shapes.h | 2 + include/hpp/fcl/serialization/hfield.h | 2 + include/hpp/fcl/serialization/kDOP.h | 2 + include/hpp/fcl/serialization/kIOS.h | 2 + include/hpp/fcl/serialization/memory.h | 2 + include/hpp/fcl/serialization/octree.h | 2 + include/hpp/fcl/serialization/quadrilateral.h | 2 + include/hpp/fcl/serialization/serializer.h | 2 + include/hpp/fcl/serialization/transform.h | 2 + include/hpp/fcl/serialization/triangle.h | 2 + include/hpp/fcl/shape/convex.h | 2 + include/hpp/fcl/shape/details/convex.hxx | 2 + .../fcl/shape/geometric_shape_to_BVH_model.h | 2 + include/hpp/fcl/shape/geometric_shapes.h | 2 + .../hpp/fcl/shape/geometric_shapes_traits.h | 2 + .../hpp/fcl/shape/geometric_shapes_utility.h | 2 + include/hpp/fcl/timings.h | 2 + include/hpp/fcl/warning.hh | 2 + python/CMakeLists.txt | 14 ++ python/hppfcl/__init__.py | 9 ++ python/hppfcl/viewer.py | 7 + 133 files changed, 445 insertions(+), 2 deletions(-) create mode 100644 hpp-fclConfig.cmake create mode 100644 include/hpp/fcl/BV/AABB.h create mode 100644 include/hpp/fcl/BV/BV.h create mode 100644 include/hpp/fcl/BV/BV_node.h create mode 100644 include/hpp/fcl/BV/OBB.h create mode 100644 include/hpp/fcl/BV/OBBRSS.h create mode 100644 include/hpp/fcl/BV/RSS.h create mode 100644 include/hpp/fcl/BV/kDOP.h create mode 100644 include/hpp/fcl/BV/kIOS.h create mode 100644 include/hpp/fcl/BVH/BVH_front.h create mode 100644 include/hpp/fcl/BVH/BVH_internal.h create mode 100644 include/hpp/fcl/BVH/BVH_model.h create mode 100644 include/hpp/fcl/BVH/BVH_utility.h create mode 100644 include/hpp/fcl/broadphase/broadphase.h create mode 100644 include/hpp/fcl/broadphase/broadphase_SSaP.h create mode 100644 include/hpp/fcl/broadphase/broadphase_SaP.h create mode 100644 include/hpp/fcl/broadphase/broadphase_bruteforce.h create mode 100644 include/hpp/fcl/broadphase/broadphase_callbacks.h create mode 100644 include/hpp/fcl/broadphase/broadphase_collision_manager.h create mode 100644 include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h create mode 100644 include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h create mode 100644 include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h create mode 100644 include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h create mode 100644 include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h create mode 100644 include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h create mode 100644 include/hpp/fcl/broadphase/broadphase_interval_tree.h create mode 100644 include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h create mode 100644 include/hpp/fcl/broadphase/broadphase_spatialhash.h create mode 100644 include/hpp/fcl/broadphase/default_broadphase_callbacks.h create mode 100644 include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/hierarchy_tree.h create mode 100644 include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h create mode 100644 include/hpp/fcl/broadphase/detail/interval_tree.h create mode 100644 include/hpp/fcl/broadphase/detail/interval_tree_node.h create mode 100644 include/hpp/fcl/broadphase/detail/morton-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/morton.h create mode 100644 include/hpp/fcl/broadphase/detail/node_base-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/node_base.h create mode 100644 include/hpp/fcl/broadphase/detail/node_base_array-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/node_base_array.h create mode 100644 include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/simple_hash_table.h create mode 100644 include/hpp/fcl/broadphase/detail/simple_interval-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/simple_interval.h create mode 100644 include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/sparse_hash_table.h create mode 100644 include/hpp/fcl/broadphase/detail/spatial_hash-inl.h create mode 100644 include/hpp/fcl/broadphase/detail/spatial_hash.h create mode 100644 include/hpp/fcl/coal.hpp create mode 100644 include/hpp/fcl/collision.h create mode 100644 include/hpp/fcl/collision_data.h create mode 100644 include/hpp/fcl/collision_func_matrix.h create mode 100644 include/hpp/fcl/collision_object.h create mode 100644 include/hpp/fcl/collision_utility.h create mode 100644 include/hpp/fcl/config.hh create mode 100644 include/hpp/fcl/contact_patch.h create mode 100644 include/hpp/fcl/contact_patch/contact_patch_solver.h create mode 100644 include/hpp/fcl/contact_patch/contact_patch_solver.hxx create mode 100644 include/hpp/fcl/contact_patch_func_matrix.h create mode 100644 include/hpp/fcl/data_types.h create mode 100644 include/hpp/fcl/deprecated.hh create mode 100644 include/hpp/fcl/distance.h create mode 100644 include/hpp/fcl/distance_func_matrix.h create mode 100644 include/hpp/fcl/fwd.hh create mode 100644 include/hpp/fcl/hfield.h create mode 100644 include/hpp/fcl/internal/BV_fitter.h create mode 100644 include/hpp/fcl/internal/BV_splitter.h create mode 100644 include/hpp/fcl/internal/intersect.h create mode 100644 include/hpp/fcl/internal/shape_shape_contact_patch_func.h create mode 100644 include/hpp/fcl/internal/shape_shape_func.h create mode 100644 include/hpp/fcl/internal/tools.h create mode 100644 include/hpp/fcl/internal/traversal.h create mode 100644 include/hpp/fcl/internal/traversal_node_base.h create mode 100644 include/hpp/fcl/internal/traversal_node_bvh_shape.h create mode 100644 include/hpp/fcl/internal/traversal_node_bvhs.h create mode 100644 include/hpp/fcl/internal/traversal_node_hfield_shape.h create mode 100644 include/hpp/fcl/internal/traversal_node_octree.h create mode 100644 include/hpp/fcl/internal/traversal_node_setup.h create mode 100644 include/hpp/fcl/internal/traversal_node_shapes.h create mode 100644 include/hpp/fcl/internal/traversal_recurse.h create mode 100644 include/hpp/fcl/logging.h create mode 100644 include/hpp/fcl/math/matrix_3f.h create mode 100644 include/hpp/fcl/math/transform.h create mode 100644 include/hpp/fcl/math/types.h create mode 100644 include/hpp/fcl/math/vec_3f.h create mode 100644 include/hpp/fcl/mesh_loader/assimp.h create mode 100644 include/hpp/fcl/mesh_loader/loader.h create mode 100644 include/hpp/fcl/narrowphase/gjk.h create mode 100644 include/hpp/fcl/narrowphase/minkowski_difference.h create mode 100644 include/hpp/fcl/narrowphase/narrowphase.h create mode 100644 include/hpp/fcl/narrowphase/narrowphase_defaults.h create mode 100644 include/hpp/fcl/narrowphase/support_functions.h create mode 100644 include/hpp/fcl/octree.h create mode 100644 include/hpp/fcl/serialization/AABB.h create mode 100644 include/hpp/fcl/serialization/BVH_model.h create mode 100644 include/hpp/fcl/serialization/BV_node.h create mode 100644 include/hpp/fcl/serialization/BV_splitter.h create mode 100644 include/hpp/fcl/serialization/OBB.h create mode 100644 include/hpp/fcl/serialization/OBBRSS.h create mode 100644 include/hpp/fcl/serialization/RSS.h create mode 100644 include/hpp/fcl/serialization/archive.h create mode 100644 include/hpp/fcl/serialization/collision_data.h create mode 100644 include/hpp/fcl/serialization/collision_object.h create mode 100644 include/hpp/fcl/serialization/contact_patch.h create mode 100644 include/hpp/fcl/serialization/convex.h create mode 100644 include/hpp/fcl/serialization/eigen.h create mode 100644 include/hpp/fcl/serialization/fwd.h create mode 100644 include/hpp/fcl/serialization/geometric_shapes.h create mode 100644 include/hpp/fcl/serialization/hfield.h create mode 100644 include/hpp/fcl/serialization/kDOP.h create mode 100644 include/hpp/fcl/serialization/kIOS.h create mode 100644 include/hpp/fcl/serialization/memory.h create mode 100644 include/hpp/fcl/serialization/octree.h create mode 100644 include/hpp/fcl/serialization/quadrilateral.h create mode 100644 include/hpp/fcl/serialization/serializer.h create mode 100644 include/hpp/fcl/serialization/transform.h create mode 100644 include/hpp/fcl/serialization/triangle.h create mode 100644 include/hpp/fcl/shape/convex.h create mode 100644 include/hpp/fcl/shape/details/convex.hxx create mode 100644 include/hpp/fcl/shape/geometric_shape_to_BVH_model.h create mode 100644 include/hpp/fcl/shape/geometric_shapes.h create mode 100644 include/hpp/fcl/shape/geometric_shapes_traits.h create mode 100644 include/hpp/fcl/shape/geometric_shapes_utility.h create mode 100644 include/hpp/fcl/timings.h create mode 100644 include/hpp/fcl/warning.hh create mode 100644 python/hppfcl/__init__.py create mode 100644 python/hppfcl/viewer.py diff --git a/.github/workflows/conda/environment_macos_linux.yml b/.github/workflows/conda/environment_macos_linux.yml index c716d6433..34a295bad 100644 --- a/.github/workflows/conda/environment_macos_linux.yml +++ b/.github/workflows/conda/environment_macos_linux.yml @@ -10,7 +10,7 @@ dependencies: - boost - eigenpy - python - - doxygen + - doxygen<1.9.8|>=1.11 - lxml - pylatexenc - qhull diff --git a/.github/workflows/conda/environment_windows.yml b/.github/workflows/conda/environment_windows.yml index 92c01c422..febe634ee 100644 --- a/.github/workflows/conda/environment_windows.yml +++ b/.github/workflows/conda/environment_windows.yml @@ -10,7 +10,7 @@ dependencies: - boost - eigenpy - python - - doxygen + - doxygen<1.9.8|>=1.11 - lxml - pylatexenc - qhull diff --git a/CMakeLists.txt b/CMakeLists.txt index c6481a3fc..26e9bb0e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -309,6 +309,136 @@ SET(${PROJECT_NAME}_HEADERS include/coal/timings.h ) +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + LIST(APPEND ${PROJECT_NAME}_HEADERS + include/hpp/fcl/broadphase/broadphase_bruteforce.h + include/hpp/fcl/broadphase/broadphase_callbacks.h + include/hpp/fcl/broadphase/broadphase_collision_manager.h + include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h + include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h + include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h + include/hpp/fcl/broadphase/broadphase.h + include/hpp/fcl/broadphase/broadphase_interval_tree.h + include/hpp/fcl/broadphase/broadphase_SaP.h + include/hpp/fcl/broadphase/broadphase_spatialhash.h + include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h + include/hpp/fcl/broadphase/broadphase_SSaP.h + include/hpp/fcl/broadphase/default_broadphase_callbacks.h + include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h + include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h + include/hpp/fcl/broadphase/detail/hierarchy_tree.h + include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h + include/hpp/fcl/broadphase/detail/interval_tree.h + include/hpp/fcl/broadphase/detail/interval_tree_node.h + include/hpp/fcl/broadphase/detail/morton.h + include/hpp/fcl/broadphase/detail/morton-inl.h + include/hpp/fcl/broadphase/detail/node_base_array.h + include/hpp/fcl/broadphase/detail/node_base_array-inl.h + include/hpp/fcl/broadphase/detail/node_base.h + include/hpp/fcl/broadphase/detail/node_base-inl.h + include/hpp/fcl/broadphase/detail/simple_hash_table.h + include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h + include/hpp/fcl/broadphase/detail/simple_interval.h + include/hpp/fcl/broadphase/detail/simple_interval-inl.h + include/hpp/fcl/broadphase/detail/sparse_hash_table.h + include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h + include/hpp/fcl/broadphase/detail/spatial_hash.h + include/hpp/fcl/broadphase/detail/spatial_hash-inl.h + include/hpp/fcl/BV/AABB.h + include/hpp/fcl/BV/BV.h + include/hpp/fcl/BV/BV_node.h + include/hpp/fcl/BVH/BVH_front.h + include/hpp/fcl/BVH/BVH_internal.h + include/hpp/fcl/BVH/BVH_model.h + include/hpp/fcl/BVH/BVH_utility.h + include/hpp/fcl/BV/kDOP.h + include/hpp/fcl/BV/kIOS.h + include/hpp/fcl/BV/OBB.h + include/hpp/fcl/BV/OBBRSS.h + include/hpp/fcl/BV/RSS.h + include/hpp/fcl/coal.hpp + include/hpp/fcl/collision_data.h + include/hpp/fcl/collision_func_matrix.h + include/hpp/fcl/collision.h + include/hpp/fcl/collision_object.h + include/hpp/fcl/collision_utility.h + include/hpp/fcl/config.hh + include/hpp/fcl/contact_patch/contact_patch_solver.h + include/hpp/fcl/contact_patch/contact_patch_solver.hxx + include/hpp/fcl/contact_patch_func_matrix.h + include/hpp/fcl/contact_patch.h + include/hpp/fcl/data_types.h + include/hpp/fcl/deprecated.hh + include/hpp/fcl/distance_func_matrix.h + include/hpp/fcl/distance.h + include/hpp/fcl/fwd.hh + include/hpp/fcl/hfield.h + include/hpp/fcl/internal/BV_fitter.h + include/hpp/fcl/internal/BV_splitter.h + include/hpp/fcl/internal/intersect.h + include/hpp/fcl/internal/shape_shape_contact_patch_func.h + include/hpp/fcl/internal/shape_shape_func.h + include/hpp/fcl/internal/tools.h + include/hpp/fcl/internal/traversal.h + include/hpp/fcl/internal/traversal_node_base.h + include/hpp/fcl/internal/traversal_node_bvhs.h + include/hpp/fcl/internal/traversal_node_bvh_shape.h + include/hpp/fcl/internal/traversal_node_hfield_shape.h + include/hpp/fcl/internal/traversal_node_setup.h + include/hpp/fcl/internal/traversal_node_shapes.h + include/hpp/fcl/internal/traversal_recurse.h + include/hpp/fcl/internal/traversal_node_octree.h + include/hpp/fcl/logging.h + include/hpp/fcl/math/matrix_3f.h + include/hpp/fcl/math/transform.h + include/hpp/fcl/math/types.h + include/hpp/fcl/math/vec_3f.h + include/hpp/fcl/mesh_loader/assimp.h + include/hpp/fcl/mesh_loader/loader.h + include/hpp/fcl/narrowphase/gjk.h + include/hpp/fcl/narrowphase/minkowski_difference.h + include/hpp/fcl/narrowphase/narrowphase_defaults.h + include/hpp/fcl/narrowphase/narrowphase.h + include/hpp/fcl/narrowphase/support_functions.h + include/hpp/fcl/octree.h + include/hpp/fcl/serialization/AABB.h + include/hpp/fcl/serialization/archive.h + include/hpp/fcl/serialization/BVH_model.h + include/hpp/fcl/serialization/BV_node.h + include/hpp/fcl/serialization/BV_splitter.h + include/hpp/fcl/serialization/collision_data.h + include/hpp/fcl/serialization/collision_object.h + include/hpp/fcl/serialization/contact_patch.h + include/hpp/fcl/serialization/convex.h + include/hpp/fcl/serialization/eigen.h + include/hpp/fcl/serialization/fwd.h + include/hpp/fcl/serialization/geometric_shapes.h + include/hpp/fcl/serialization/hfield.h + include/hpp/fcl/serialization/kDOP.h + include/hpp/fcl/serialization/kIOS.h + include/hpp/fcl/serialization/memory.h + include/hpp/fcl/serialization/OBB.h + include/hpp/fcl/serialization/OBBRSS.h + include/hpp/fcl/serialization/octree.h + include/hpp/fcl/serialization/quadrilateral.h + include/hpp/fcl/serialization/RSS.h + include/hpp/fcl/serialization/serializer.h + include/hpp/fcl/serialization/transform.h + include/hpp/fcl/serialization/triangle.h + include/hpp/fcl/shape/convex.h + include/hpp/fcl/shape/details/convex.hxx + include/hpp/fcl/shape/geometric_shapes.h + include/hpp/fcl/shape/geometric_shapes_traits.h + include/hpp/fcl/shape/geometric_shapes_utility.h + include/hpp/fcl/shape/geometric_shape_to_BVH_model.h + include/hpp/fcl/timings.h + include/hpp/fcl/warning.hh + ) +endif() + IF(COAL_HAS_OCTOMAP) LIST(APPEND ${PROJECT_NAME}_HEADERS include/coal/octree.h @@ -342,3 +472,11 @@ file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ament_prefix_p install(FILES ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/ament_prefix_path.dsv DESTINATION share/${PROJECT_NAME}/hook) file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/python_path.dsv "prepend-non-duplicate;PYTHONPATH;${PYTHON_SITELIB}") install(FILES ${CMAKE_CURRENT_BINARY_DIR}/share/${PROJECT_NAME}/hook/python_path.dsv DESTINATION share/${PROJECT_NAME}/hook) + +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + include(CMakePackageConfigHelpers) + write_basic_package_version_file(hpp-fclConfigVersion.cmake + VERSION 3.0.0 + COMPATIBILITY AnyNewerVersion) + install(FILES hpp-fclConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/hpp-fclConfigVersion.cmake DESTINATION lib/cmake/hpp-fcl) +endif() diff --git a/hpp-fclConfig.cmake b/hpp-fclConfig.cmake new file mode 100644 index 000000000..3c30e4845 --- /dev/null +++ b/hpp-fclConfig.cmake @@ -0,0 +1,6 @@ +# This file provide bacward compatiblity for `find_package(hpp-fcl)`. + +message(WARNING "Please update your CMake from 'hpp-fcl' to 'coal'") + +find_package(coal REQUIRED) +add_library(hpp-fcl::hpp-fcl ALIAS coal::coal) diff --git a/include/coal/serialization/eigen.h b/include/coal/serialization/eigen.h index e2defdc61..24430cf02 100644 --- a/include/coal/serialization/eigen.h +++ b/include/coal/serialization/eigen.h @@ -11,6 +11,12 @@ #ifndef COAL_SERIALIZATION_EIGEN_H #define COAL_SERIALIZATION_EIGEN_H +#ifdef COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL +#ifdef HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION +#define COAL_SKIP_EIGEN_BOOST_SERIALIZATION +#endif +#endif + #ifndef COAL_SKIP_EIGEN_BOOST_SERIALIZATION #include diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h new file mode 100644 index 000000000..c76ec66b0 --- /dev/null +++ b/include/hpp/fcl/BV/AABB.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h new file mode 100644 index 000000000..cda871c3a --- /dev/null +++ b/include/hpp/fcl/BV/BV.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h new file mode 100644 index 000000000..50e3d7adc --- /dev/null +++ b/include/hpp/fcl/BV/BV_node.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h new file mode 100644 index 000000000..003da1750 --- /dev/null +++ b/include/hpp/fcl/BV/OBB.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h new file mode 100644 index 000000000..16b21dbda --- /dev/null +++ b/include/hpp/fcl/BV/OBBRSS.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h new file mode 100644 index 000000000..cc6089bab --- /dev/null +++ b/include/hpp/fcl/BV/RSS.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h new file mode 100644 index 000000000..51a7c44d2 --- /dev/null +++ b/include/hpp/fcl/BV/kDOP.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h new file mode 100644 index 000000000..cc24725dd --- /dev/null +++ b/include/hpp/fcl/BV/kIOS.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_front.h b/include/hpp/fcl/BVH/BVH_front.h new file mode 100644 index 000000000..0d567079d --- /dev/null +++ b/include/hpp/fcl/BVH/BVH_front.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_internal.h b/include/hpp/fcl/BVH/BVH_internal.h new file mode 100644 index 000000000..7b24ec33a --- /dev/null +++ b/include/hpp/fcl/BVH/BVH_internal.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h new file mode 100644 index 000000000..a069ea961 --- /dev/null +++ b/include/hpp/fcl/BVH/BVH_model.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h new file mode 100644 index 000000000..275a171e1 --- /dev/null +++ b/include/hpp/fcl/BVH/BVH_utility.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/hpp/fcl/broadphase/broadphase.h new file mode 100644 index 000000000..7834fbe4c --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_SSaP.h b/include/hpp/fcl/broadphase/broadphase_SSaP.h new file mode 100644 index 000000000..23dda8ef3 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_SSaP.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h new file mode 100644 index 000000000..0cdef6822 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_SaP.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_bruteforce.h b/include/hpp/fcl/broadphase/broadphase_bruteforce.h new file mode 100644 index 000000000..5a4811ce1 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_bruteforce.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_callbacks.h b/include/hpp/fcl/broadphase/broadphase_callbacks.h new file mode 100644 index 000000000..40b5b0a6c --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_callbacks.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_collision_manager.h new file mode 100644 index 000000000..fe49da052 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_collision_manager.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h new file mode 100644 index 000000000..5ee4f15b0 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h new file mode 100644 index 000000000..a2f73c0a1 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_continuous_collision_manager.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h new file mode 100644 index 000000000..167ec3adc --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h new file mode 100644 index 000000000..5ce7c99fc --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h new file mode 100644 index 000000000..5e67f2525 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h new file mode 100644 index 000000000..5923fd177 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_interval_tree.h b/include/hpp/fcl/broadphase/broadphase_interval_tree.h new file mode 100644 index 000000000..d00b1f6d3 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_interval_tree.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h new file mode 100644 index 000000000..31917d0a6 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/broadphase_spatialhash.h b/include/hpp/fcl/broadphase/broadphase_spatialhash.h new file mode 100644 index 000000000..5ad526077 --- /dev/null +++ b/include/hpp/fcl/broadphase/broadphase_spatialhash.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/default_broadphase_callbacks.h b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h new file mode 100644 index 000000000..7d78f9532 --- /dev/null +++ b/include/hpp/fcl/broadphase/default_broadphase_callbacks.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h new file mode 100644 index 000000000..54b709d79 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h new file mode 100644 index 000000000..ebacd648d --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h new file mode 100644 index 000000000..a6923fbb7 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h new file mode 100644 index 000000000..e0c9a90a1 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/hierarchy_tree_array.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/interval_tree.h b/include/hpp/fcl/broadphase/detail/interval_tree.h new file mode 100644 index 000000000..5480fc00f --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/interval_tree.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/interval_tree_node.h b/include/hpp/fcl/broadphase/detail/interval_tree_node.h new file mode 100644 index 000000000..531898a41 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/interval_tree_node.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/morton-inl.h b/include/hpp/fcl/broadphase/detail/morton-inl.h new file mode 100644 index 000000000..b3ef57cba --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/morton-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/morton.h b/include/hpp/fcl/broadphase/detail/morton.h new file mode 100644 index 000000000..c0a89c6b2 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/morton.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base-inl.h b/include/hpp/fcl/broadphase/detail/node_base-inl.h new file mode 100644 index 000000000..326020040 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/node_base-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base.h b/include/hpp/fcl/broadphase/detail/node_base.h new file mode 100644 index 000000000..b0c8206e3 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/node_base.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base_array-inl.h b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h new file mode 100644 index 000000000..e66c59cdd --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/node_base_array-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/node_base_array.h b/include/hpp/fcl/broadphase/detail/node_base_array.h new file mode 100644 index 000000000..8ad4c1ed3 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/node_base_array.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h new file mode 100644 index 000000000..712c72ec3 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/hpp/fcl/broadphase/detail/simple_hash_table.h new file mode 100644 index 000000000..678cd34c8 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_interval-inl.h b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h new file mode 100644 index 000000000..b6e1fbd6c --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/simple_interval-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/simple_interval.h b/include/hpp/fcl/broadphase/detail/simple_interval.h new file mode 100644 index 000000000..f0c26e038 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/simple_interval.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h new file mode 100644 index 000000000..cabe7e8fb --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h new file mode 100644 index 000000000..9f585fac5 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h new file mode 100644 index 000000000..be5a1d873 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/spatial_hash-inl.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/broadphase/detail/spatial_hash.h b/include/hpp/fcl/broadphase/detail/spatial_hash.h new file mode 100644 index 000000000..f4f10af83 --- /dev/null +++ b/include/hpp/fcl/broadphase/detail/spatial_hash.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/coal.hpp b/include/hpp/fcl/coal.hpp new file mode 100644 index 000000000..e3c8845e2 --- /dev/null +++ b/include/hpp/fcl/coal.hpp @@ -0,0 +1,15 @@ +#ifndef HPP_FCL_COAL_HH +#define HPP_FCL_COAL_HH + +#include +#include + +#define COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL + +#pragma message COAL_DEPRECATED_HEADER( \ + "Please update your includes from 'hpp/fcl' to 'coal'") + +#define HPP_FCL_VERSION_AT_LEAST(major, minor, patch) \ + COAL_VERSION_AT_LEAST(major, minor, patch) + +#endif // COAL_FWD_HH diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h new file mode 100644 index 000000000..099670f25 --- /dev/null +++ b/include/hpp/fcl/collision.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h new file mode 100644 index 000000000..185424ac7 --- /dev/null +++ b/include/hpp/fcl/collision_data.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/collision_func_matrix.h b/include/hpp/fcl/collision_func_matrix.h new file mode 100644 index 000000000..c17b82412 --- /dev/null +++ b/include/hpp/fcl/collision_func_matrix.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h new file mode 100644 index 000000000..245da1c07 --- /dev/null +++ b/include/hpp/fcl/collision_object.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/collision_utility.h b/include/hpp/fcl/collision_utility.h new file mode 100644 index 000000000..9043e1f8e --- /dev/null +++ b/include/hpp/fcl/collision_utility.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/config.hh b/include/hpp/fcl/config.hh new file mode 100644 index 000000000..7388d6087 --- /dev/null +++ b/include/hpp/fcl/config.hh @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/contact_patch.h b/include/hpp/fcl/contact_patch.h new file mode 100644 index 000000000..fef1ee0d4 --- /dev/null +++ b/include/hpp/fcl/contact_patch.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.h b/include/hpp/fcl/contact_patch/contact_patch_solver.h new file mode 100644 index 000000000..f53a91507 --- /dev/null +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/contact_patch/contact_patch_solver.hxx b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx new file mode 100644 index 000000000..b73d4e935 --- /dev/null +++ b/include/hpp/fcl/contact_patch/contact_patch_solver.hxx @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/contact_patch_func_matrix.h b/include/hpp/fcl/contact_patch_func_matrix.h new file mode 100644 index 000000000..9103090df --- /dev/null +++ b/include/hpp/fcl/contact_patch_func_matrix.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h new file mode 100644 index 000000000..690f179c6 --- /dev/null +++ b/include/hpp/fcl/data_types.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/deprecated.hh b/include/hpp/fcl/deprecated.hh new file mode 100644 index 000000000..513abdf65 --- /dev/null +++ b/include/hpp/fcl/deprecated.hh @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h new file mode 100644 index 000000000..a8301218b --- /dev/null +++ b/include/hpp/fcl/distance.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/distance_func_matrix.h b/include/hpp/fcl/distance_func_matrix.h new file mode 100644 index 000000000..5bc9be51c --- /dev/null +++ b/include/hpp/fcl/distance_func_matrix.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh new file mode 100644 index 000000000..1bf07ff6c --- /dev/null +++ b/include/hpp/fcl/fwd.hh @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h new file mode 100644 index 000000000..4d462776a --- /dev/null +++ b/include/hpp/fcl/hfield.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/BV_fitter.h b/include/hpp/fcl/internal/BV_fitter.h new file mode 100644 index 000000000..438bd599e --- /dev/null +++ b/include/hpp/fcl/internal/BV_fitter.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/BV_splitter.h b/include/hpp/fcl/internal/BV_splitter.h new file mode 100644 index 000000000..85a586bdb --- /dev/null +++ b/include/hpp/fcl/internal/BV_splitter.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/intersect.h b/include/hpp/fcl/internal/intersect.h new file mode 100644 index 000000000..bc4fe38c4 --- /dev/null +++ b/include/hpp/fcl/internal/intersect.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/shape_shape_contact_patch_func.h b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h new file mode 100644 index 000000000..cc3ecf58a --- /dev/null +++ b/include/hpp/fcl/internal/shape_shape_contact_patch_func.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h new file mode 100644 index 000000000..3789fb52a --- /dev/null +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/tools.h b/include/hpp/fcl/internal/tools.h new file mode 100644 index 000000000..aa11afec1 --- /dev/null +++ b/include/hpp/fcl/internal/tools.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal.h b/include/hpp/fcl/internal/traversal.h new file mode 100644 index 000000000..f0cbf51fc --- /dev/null +++ b/include/hpp/fcl/internal/traversal.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h new file mode 100644 index 000000000..b5f5d9938 --- /dev/null +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h new file mode 100644 index 000000000..c99f06b17 --- /dev/null +++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h new file mode 100644 index 000000000..835610130 --- /dev/null +++ b/include/hpp/fcl/internal/traversal_node_bvhs.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h new file mode 100644 index 000000000..8384a2b94 --- /dev/null +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h new file mode 100644 index 000000000..44f8b3234 --- /dev/null +++ b/include/hpp/fcl/internal/traversal_node_octree.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h new file mode 100644 index 000000000..b94dd7a84 --- /dev/null +++ b/include/hpp/fcl/internal/traversal_node_setup.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h new file mode 100644 index 000000000..996730308 --- /dev/null +++ b/include/hpp/fcl/internal/traversal_node_shapes.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/internal/traversal_recurse.h b/include/hpp/fcl/internal/traversal_recurse.h new file mode 100644 index 000000000..6fd2bf7ae --- /dev/null +++ b/include/hpp/fcl/internal/traversal_recurse.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/logging.h b/include/hpp/fcl/logging.h new file mode 100644 index 000000000..e9f591ebc --- /dev/null +++ b/include/hpp/fcl/logging.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h new file mode 100644 index 000000000..0b4e853e9 --- /dev/null +++ b/include/hpp/fcl/math/matrix_3f.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h new file mode 100644 index 000000000..970a2a936 --- /dev/null +++ b/include/hpp/fcl/math/transform.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/math/types.h b/include/hpp/fcl/math/types.h new file mode 100644 index 000000000..932a7e590 --- /dev/null +++ b/include/hpp/fcl/math/types.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h new file mode 100644 index 000000000..e6ac46c7a --- /dev/null +++ b/include/hpp/fcl/math/vec_3f.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h new file mode 100644 index 000000000..139538928 --- /dev/null +++ b/include/hpp/fcl/mesh_loader/assimp.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/hpp/fcl/mesh_loader/loader.h new file mode 100644 index 000000000..bbb43f1d0 --- /dev/null +++ b/include/hpp/fcl/mesh_loader/loader.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h new file mode 100644 index 000000000..cab294b93 --- /dev/null +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/narrowphase/minkowski_difference.h b/include/hpp/fcl/narrowphase/minkowski_difference.h new file mode 100644 index 000000000..5f6ca0e08 --- /dev/null +++ b/include/hpp/fcl/narrowphase/minkowski_difference.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h new file mode 100644 index 000000000..0a079085d --- /dev/null +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/narrowphase/narrowphase_defaults.h b/include/hpp/fcl/narrowphase/narrowphase_defaults.h new file mode 100644 index 000000000..c17096f9a --- /dev/null +++ b/include/hpp/fcl/narrowphase/narrowphase_defaults.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/narrowphase/support_functions.h b/include/hpp/fcl/narrowphase/support_functions.h new file mode 100644 index 000000000..8eb7c4b4f --- /dev/null +++ b/include/hpp/fcl/narrowphase/support_functions.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/octree.h b/include/hpp/fcl/octree.h new file mode 100644 index 000000000..b7f745e79 --- /dev/null +++ b/include/hpp/fcl/octree.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/AABB.h b/include/hpp/fcl/serialization/AABB.h new file mode 100644 index 000000000..b001c4ec3 --- /dev/null +++ b/include/hpp/fcl/serialization/AABB.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/BVH_model.h b/include/hpp/fcl/serialization/BVH_model.h new file mode 100644 index 000000000..c4f65ab50 --- /dev/null +++ b/include/hpp/fcl/serialization/BVH_model.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/BV_node.h b/include/hpp/fcl/serialization/BV_node.h new file mode 100644 index 000000000..a8407a076 --- /dev/null +++ b/include/hpp/fcl/serialization/BV_node.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/BV_splitter.h b/include/hpp/fcl/serialization/BV_splitter.h new file mode 100644 index 000000000..7d13844e3 --- /dev/null +++ b/include/hpp/fcl/serialization/BV_splitter.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/OBB.h b/include/hpp/fcl/serialization/OBB.h new file mode 100644 index 000000000..8eebfec17 --- /dev/null +++ b/include/hpp/fcl/serialization/OBB.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/OBBRSS.h b/include/hpp/fcl/serialization/OBBRSS.h new file mode 100644 index 000000000..c9673a7f4 --- /dev/null +++ b/include/hpp/fcl/serialization/OBBRSS.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/RSS.h b/include/hpp/fcl/serialization/RSS.h new file mode 100644 index 000000000..b7639aa86 --- /dev/null +++ b/include/hpp/fcl/serialization/RSS.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/archive.h b/include/hpp/fcl/serialization/archive.h new file mode 100644 index 000000000..0a3f5c188 --- /dev/null +++ b/include/hpp/fcl/serialization/archive.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/collision_data.h b/include/hpp/fcl/serialization/collision_data.h new file mode 100644 index 000000000..9ac4f985d --- /dev/null +++ b/include/hpp/fcl/serialization/collision_data.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/collision_object.h b/include/hpp/fcl/serialization/collision_object.h new file mode 100644 index 000000000..782976870 --- /dev/null +++ b/include/hpp/fcl/serialization/collision_object.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/contact_patch.h b/include/hpp/fcl/serialization/contact_patch.h new file mode 100644 index 000000000..dca0506ff --- /dev/null +++ b/include/hpp/fcl/serialization/contact_patch.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/convex.h b/include/hpp/fcl/serialization/convex.h new file mode 100644 index 000000000..7000a3e18 --- /dev/null +++ b/include/hpp/fcl/serialization/convex.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/eigen.h b/include/hpp/fcl/serialization/eigen.h new file mode 100644 index 000000000..e5c9faaa7 --- /dev/null +++ b/include/hpp/fcl/serialization/eigen.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/fwd.h b/include/hpp/fcl/serialization/fwd.h new file mode 100644 index 000000000..69e442f46 --- /dev/null +++ b/include/hpp/fcl/serialization/fwd.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/geometric_shapes.h b/include/hpp/fcl/serialization/geometric_shapes.h new file mode 100644 index 000000000..d7679f265 --- /dev/null +++ b/include/hpp/fcl/serialization/geometric_shapes.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h new file mode 100644 index 000000000..e0e862a73 --- /dev/null +++ b/include/hpp/fcl/serialization/hfield.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/kDOP.h b/include/hpp/fcl/serialization/kDOP.h new file mode 100644 index 000000000..fe4acc157 --- /dev/null +++ b/include/hpp/fcl/serialization/kDOP.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/kIOS.h b/include/hpp/fcl/serialization/kIOS.h new file mode 100644 index 000000000..41fe0d7f6 --- /dev/null +++ b/include/hpp/fcl/serialization/kIOS.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/memory.h b/include/hpp/fcl/serialization/memory.h new file mode 100644 index 000000000..b03c4026a --- /dev/null +++ b/include/hpp/fcl/serialization/memory.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/octree.h b/include/hpp/fcl/serialization/octree.h new file mode 100644 index 000000000..6d2694aa4 --- /dev/null +++ b/include/hpp/fcl/serialization/octree.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/quadrilateral.h b/include/hpp/fcl/serialization/quadrilateral.h new file mode 100644 index 000000000..641f8161d --- /dev/null +++ b/include/hpp/fcl/serialization/quadrilateral.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/serializer.h b/include/hpp/fcl/serialization/serializer.h new file mode 100644 index 000000000..6b194a18e --- /dev/null +++ b/include/hpp/fcl/serialization/serializer.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/transform.h b/include/hpp/fcl/serialization/transform.h new file mode 100644 index 000000000..a72222f79 --- /dev/null +++ b/include/hpp/fcl/serialization/transform.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/serialization/triangle.h b/include/hpp/fcl/serialization/triangle.h new file mode 100644 index 000000000..5c2b23f13 --- /dev/null +++ b/include/hpp/fcl/serialization/triangle.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/shape/convex.h b/include/hpp/fcl/shape/convex.h new file mode 100644 index 000000000..0d5dd44ab --- /dev/null +++ b/include/hpp/fcl/shape/convex.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx new file mode 100644 index 000000000..d7b78f4d7 --- /dev/null +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h new file mode 100644 index 000000000..66bb3b7e6 --- /dev/null +++ b/include/hpp/fcl/shape/geometric_shape_to_BVH_model.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h new file mode 100644 index 000000000..dd8f59a95 --- /dev/null +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h new file mode 100644 index 000000000..3afa55f58 --- /dev/null +++ b/include/hpp/fcl/shape/geometric_shapes_traits.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/shape/geometric_shapes_utility.h b/include/hpp/fcl/shape/geometric_shapes_utility.h new file mode 100644 index 000000000..65e33d534 --- /dev/null +++ b/include/hpp/fcl/shape/geometric_shapes_utility.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/timings.h b/include/hpp/fcl/timings.h new file mode 100644 index 000000000..4500e9f19 --- /dev/null +++ b/include/hpp/fcl/timings.h @@ -0,0 +1,2 @@ +#include +#include diff --git a/include/hpp/fcl/warning.hh b/include/hpp/fcl/warning.hh new file mode 100644 index 000000000..c49b82715 --- /dev/null +++ b/include/hpp/fcl/warning.hh @@ -0,0 +1,2 @@ +#include +#include diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bbdf917b3..8133d617f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,14 @@ IF( NOT ENABLE_PYTHON_DOXYGEN_AUTODOC ELSE() SET(ENABLE_DOXYGEN_AUTODOC TRUE) + IF(DOXYGEN_VERSION VERSION_GREATER_EQUAL 1.9.8 AND DOXYGEN_VERSION VERSION_LESS 1.11.0) + # deactivate python doxygen automoc for doxygen 1.9.8 and 1.10.0, + # as it incorrectly parse "const" keyword, + # generating wrong links in C++ doc, and fail generating python doc + # ref. https://github.com/doxygen/doxygen/issues/10797 + SET(ENABLE_DOXYGEN_AUTODOC FALSE) + MESSAGE(AUTHOR_WARNING "automoc deactivated because of doxygen 1.10. Please use <1.10 or >=1.11.") + ENDIF() EXECUTE_PROCESS(COMMAND ${PYTHON_EXECUTABLE} -c "import lxml" RESULT_VARIABLE _pypkg_found OUTPUT_QUIET @@ -180,3 +188,9 @@ FOREACH(python ${PYTHON_FILES}) "${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}/${python}" DESTINATION ${${PYTHON_LIB_NAME}_INSTALL_DIR}) ENDFOREACH(python) + + +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + python_install_on_site(hppfcl __init__.py) + python_install_on_site(hppfcl viewer.py) +endif() diff --git a/python/hppfcl/__init__.py b/python/hppfcl/__init__.py new file mode 100644 index 000000000..1948a5878 --- /dev/null +++ b/python/hppfcl/__init__.py @@ -0,0 +1,9 @@ +import warnings + +warnings.warn( + "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning +) + +from coal import Transform3s as Transform3f # noqa +from coal import * # noqa +from coal import __raw_version__, __version__ # noqa diff --git a/python/hppfcl/viewer.py b/python/hppfcl/viewer.py new file mode 100644 index 000000000..13353dce9 --- /dev/null +++ b/python/hppfcl/viewer.py @@ -0,0 +1,7 @@ +import warnings + +warnings.warn( + "Please update your 'hppfcl' imports to 'coal'", category=DeprecationWarning +) + +from coal.viewer import * # noqa From d914b542761e6c4ee3308dce105256b0cf986f67 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 22 Jul 2024 16:14:07 +0200 Subject: [PATCH 31/57] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4ff551c89..04007a79a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] ### Added +- Renaming the library from `hpp-fcl` to `coal`. Created a `COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL` CMake option for retro compatibility. This allows to still do `find_package(hpp-fcl)` and `#include ` in C++ and it allows to still do `import hppfcl` in python ([#596](https://github.com/humanoid-path-planner/hpp-fcl/pull/596)). - Added `Transform3f::Random` and `Transform3f::setRandom` ([#584](https://github.com/humanoid-path-planner/hpp-fcl/pull/584)) - New feature: computation of contact surfaces for any pair of primitive shapes (triangle, sphere, ellipsoid, plane, halfspace, cone, capsule, cylinder, convex) ([#574](https://github.com/humanoid-path-planner/hpp-fcl/pull/574)). - Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570)) From 9a43d36addf119878f4a9735c035a2838400d5ce Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 11:40:41 +0200 Subject: [PATCH 32/57] cmake: Disable automatic finalize call --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 26e9bb0e7..4ee7a0c28 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,7 @@ set(PROJECT_DESCRIPTION ) SET(PROJECT_USE_CMAKE_EXPORT TRUE) SET(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion) +set(PROJECT_AUTO_RUN_FINALIZE FALSE) SET(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE) SET(DOXYGEN_USE_TEMPLATE_CSS TRUE) @@ -480,3 +481,5 @@ if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) COMPATIBILITY AnyNewerVersion) install(FILES hpp-fclConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/hpp-fclConfigVersion.cmake DESTINATION lib/cmake/hpp-fcl) endif() + +setup_project_finalize() From b7c91810ea870575bc1f363aa87c8e6813dd8a42 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 11:41:08 +0200 Subject: [PATCH 33/57] cmake: Define same target than in the hpp-fclTarget.cmake --- src/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0228bf11e..2ae58c221 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -187,6 +187,7 @@ add_library(${LIBRARY_NAME} ${PROJECT_HEADERS_FULL_PATH} ${${LIBRARY_NAME}_SOURCES} ) +add_library(hpp-fcl::hpp-fcl ALIAS ${LIBRARY_NAME}) if(UNIX) get_relative_rpath(${CMAKE_INSTALL_LIBDIR} ${PROJECT_NAME}_INSTALL_RPATH) From f2382ce9a25adef8806466a714fb2ce72f18c66e Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 14:13:10 +0200 Subject: [PATCH 34/57] cmake: Add source and binary dir includ directory --- src/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2ae58c221..a760e564f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -257,6 +257,8 @@ MODERNIZE_TARGET_LINK_LIBRARIES(${PROJECT_NAME} SCOPE PUBLIC target_include_directories(${LIBRARY_NAME} PUBLIC $ + $ + $ ) IF(octomap_FOUND) From 4719963dd6ebffdbf4d5c43183134342de82a165 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 14:14:33 +0200 Subject: [PATCH 35/57] cmake: Replace CMAKE_{SOURCE,BINARY}_DIR by PROJECT_{SOURCE,BINARY}_DIR --- CMakeLists.txt | 6 +++--- python/CMakeLists.txt | 11 ++++++++--- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ee7a0c28..5f9f39731 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -169,10 +169,10 @@ if(COAL_HAS_QHULL) message(STATUS "Qhullcpp not found: it will be build from sources, if Qhull_r is found") file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/third-parties) execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink - ${CMAKE_SOURCE_DIR}/third-parties/qhull/src/libqhullcpp - ${CMAKE_CURRENT_BINARY_DIR}/third-parties/libqhullcpp + ${PROJECT_SOURCE_DIR}/third-parties/qhull/src/libqhullcpp + ${PROJECT_BINARY_DIR}/third-parties/libqhullcpp ) - set(Qhullcpp_PREFIX ${CMAKE_BINARY_DIR}/third-parties) + set(Qhullcpp_PREFIX ${PROJECT_BINARY_DIR}/third-parties) find_path(Qhull_r_INCLUDE_DIR NAMES libqhull_r/libqhull_r.h PATHS ${Qhull_PREFIX} diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 8133d617f..e3cd0b516 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -94,8 +94,8 @@ ELSE() ENDIF() IF(ENABLE_DOXYGEN_AUTODOC) ADD_CUSTOM_TARGET(generate_doxygen_cpp_doc - COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/python/doxygen_xml_parser.py - ${CMAKE_BINARY_DIR}/doc/doxygen-xml/index.xml + COMMAND ${PYTHON_EXECUTABLE} ${PROJECT_SOURCE_DIR}/doc/python/doxygen_xml_parser.py + ${PROJECT_BINARY_DIR}/doc/doxygen-xml/index.xml ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc > ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc.log BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/doxygen_autodoc/doxygen_xml_parser_for_cmake.hh @@ -127,7 +127,7 @@ ENDIF(COAL_HAS_OCTOMAP) ADD_LIBRARY(${PYTHON_LIB_NAME} MODULE ${${PYTHON_LIB_NAME}_SOURCES} ${${PYTHON_LIB_NAME}_HEADERS}) TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS}) -TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} PRIVATE "${CMAKE_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}") +TARGET_INCLUDE_DIRECTORIES(${PYTHON_LIB_NAME} PRIVATE "${PROJECT_SOURCE_DIR}/src" "${CMAKE_CURRENT_BINARY_DIR}") IF(WIN32) TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY}) ENDIF(WIN32) @@ -148,10 +148,15 @@ TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES PREFIX "" SUFFIX "${PYTHON_EXT_SUFFIX}" +<<<<<<< HEAD OUTPUT_NAME "${PYTHON_LIB_NAME}" LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}" # On Windows, shared library are treat as binary RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}" +======= + LIBRARY_OUTPUT_NAME ${LIBRARY_NAME} + LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${LIBRARY_NAME}" +>>>>>>> a41725d3 (cmake: Replace CMAKE_{SOURCE,BINARY}_DIR by PROJECT_{SOURCE,BINARY}_DIR) ) IF(IS_ABSOLUTE ${PYTHON_SITELIB}) From 041ac9f3ad90a0fe564774233e665831e9ce8a84 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 17:36:28 +0200 Subject: [PATCH 36/57] cmake: Don't take python from eigenpy transitive dependency --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f9f39731..fa5f803f5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -123,6 +123,7 @@ CMAKE_DEPENDENT_OPTION(GENERATE_PYTHON_STUBS "Generate the Python stubs associat ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0") if(BUILD_PYTHON_INTERFACE) + FINDPYTHON(REQUIRED) FIND_PACKAGE(eigenpy 2.9.2 REQUIRED) endif() From c8b0131c82643ddd568592e7166554224c883802 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 17:36:52 +0200 Subject: [PATCH 37/57] =?UTF-8?q?cmake:=C2=A0Use=20ADD=5FPROJECT=5FPRIVATE?= =?UTF-8?q?=5FDEPENDENCY=20find=20eigenpy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fa5f803f5..9f392f403 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,7 +124,7 @@ ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0") if(BUILD_PYTHON_INTERFACE) FINDPYTHON(REQUIRED) - FIND_PACKAGE(eigenpy 2.9.2 REQUIRED) + ADD_PROJECT_PRIVATE_DEPENDENCY(eigenpy 2.9.2 REQUIRED) endif() # Required dependencies From 952bbb12cad02649c696ff44ca58adfdae5a29e7 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 17:37:18 +0200 Subject: [PATCH 38/57] cmake: Use unique target property name --- python/CMakeLists.txt | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e3cd0b516..d28714c59 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -35,8 +35,8 @@ include(${JRL_CMAKE_MODULES}/python-helpers.cmake) include(${JRL_CMAKE_MODULES}/stubs.cmake) -ADD_CUSTOM_TARGET(python) -SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) +ADD_CUSTOM_TARGET(${PROJECT_NAME}_python) +SET_TARGET_PROPERTIES(${PROJECT_NAME}_python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) # Name of the Python library SET(PYTHON_LIB_NAME ${PROJECT_NAME}_pywrap) @@ -132,7 +132,7 @@ IF(WIN32) TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC ${PYTHON_LIBRARY}) ENDIF(WIN32) -ADD_DEPENDENCIES(python ${PYTHON_LIB_NAME}) +ADD_DEPENDENCIES(${PROJECT_NAME}_python ${PYTHON_LIB_NAME}) ADD_HEADER_GROUP(${PYTHON_LIB_NAME}_HEADERS) ADD_SOURCE_GROUP(${PYTHON_LIB_NAME}_SOURCES) IF(ENABLE_DOXYGEN_AUTODOC) @@ -148,15 +148,10 @@ TARGET_LINK_LIBRARIES(${PYTHON_LIB_NAME} PUBLIC SET_TARGET_PROPERTIES(${PYTHON_LIB_NAME} PROPERTIES PREFIX "" SUFFIX "${PYTHON_EXT_SUFFIX}" -<<<<<<< HEAD OUTPUT_NAME "${PYTHON_LIB_NAME}" - LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}" + LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}" # On Windows, shared library are treat as binary - RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python/${PROJECT_NAME}" -======= - LIBRARY_OUTPUT_NAME ${LIBRARY_NAME} - LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${LIBRARY_NAME}" ->>>>>>> a41725d3 (cmake: Replace CMAKE_{SOURCE,BINARY}_DIR by PROJECT_{SOURCE,BINARY}_DIR) + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/python/${PROJECT_NAME}" ) IF(IS_ABSOLUTE ${PYTHON_SITELIB}) From 8075cd1bf7e851a94c8739b7f9594cca3272eb29 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 10 Jul 2024 17:37:39 +0200 Subject: [PATCH 39/57] cmake: Use variable to define hpp-fcl::hpp-fcl alias --- src/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a760e564f..ede3a1c54 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -187,7 +187,7 @@ add_library(${LIBRARY_NAME} ${PROJECT_HEADERS_FULL_PATH} ${${LIBRARY_NAME}_SOURCES} ) -add_library(hpp-fcl::hpp-fcl ALIAS ${LIBRARY_NAME}) +add_library(${LIBRARY_NAME}::${LIBRARY_NAME} ALIAS ${LIBRARY_NAME}) if(UNIX) get_relative_rpath(${CMAKE_INSTALL_LIBDIR} ${PROJECT_NAME}_INSTALL_RPATH) From be3a1bdfebbf658641dedf7a8993ceb5051ef049 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Fri, 12 Jul 2024 17:45:37 +0200 Subject: [PATCH 40/57] cmake: Prefix all test target by hpp-fcl- --- CMakeLists.txt | 1 + test/CMakeLists.txt | 31 +++++++++++++++++-------------- test/python_unit/CMakeLists.txt | 4 ++-- 3 files changed, 20 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f392f403..cd74a5a58 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,6 +87,7 @@ else() endif() include("${JRL_CMAKE_MODULES}/boost.cmake") +include("${JRL_CMAKE_MODULES}/python.cmake") include("${JRL_CMAKE_MODULES}/hpp.cmake") include("${JRL_CMAKE_MODULES}/apple.cmake") include("${JRL_CMAKE_MODULES}/ide.cmake") diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 39d2d31b2..f4a5b679c 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -3,26 +3,28 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework filesystem) config_files(fcl_resources/config.h) -macro(add_coal_test test_name source) +function(add_coal_test test_name source) + set(target_name ${PROJECT_NAME}-${test_name}) ADD_UNIT_TEST(${test_name} ${source}) - target_link_libraries(${test_name} + target_link_libraries(${target_name} PUBLIC ${LIBRARY_NAME} Boost::filesystem - utility + ${utility_target} ) IF(NOT WIN32) - target_compile_options(${test_name} PRIVATE "-Wno-c99-extensions") - ENDIF(NOT WIN32) + target_compile_options(${target_name} PRIVATE "-Wno-c99-extensions") + ENDIF() if(COAL_HAS_QHULL) - target_compile_options(${test_name} PRIVATE -DCOAL_HAS_QHULL) + target_compile_options(${target_name} PRIVATE -DCOAL_HAS_QHULL) endif() -endmacro(add_coal_test) +endfunction() include_directories(${CMAKE_CURRENT_BINARY_DIR}) -add_library(utility STATIC utility.cpp) -target_link_libraries(utility PUBLIC ${PROJECT_NAME}) +set(utility_target ${PROJECT_NAME}-utility) +add_library(${utility_target} STATIC utility.cpp) +target_link_libraries(${utility_target} PUBLIC ${PROJECT_NAME}) add_coal_test(math math.cpp) @@ -38,7 +40,7 @@ add_coal_test(shape_inflation shape_inflation.cpp) #add_coal_test(shape_mesh_consistency shape_mesh_consistency.cpp) add_coal_test(gjk_asserts gjk_asserts.cpp) add_coal_test(frontlist frontlist.cpp) -SET_TESTS_PROPERTIES(frontlist PROPERTIES TIMEOUT 7200) +SET_TESTS_PROPERTIES(${PROJECT_NAME}-frontlist PROPERTIES TIMEOUT 7200) # add_coal_test(sphere_capsule sphere_capsule.cpp) add_coal_test(capsule_capsule capsule_capsule.cpp) @@ -67,16 +69,17 @@ add_coal_test(serialization serialization.cpp) # Broadphase add_coal_test(broadphase broadphase.cpp) -set_tests_properties(broadphase PROPERTIES WILL_FAIL TRUE) +set_tests_properties(${PROJECT_NAME}-broadphase PROPERTIES WILL_FAIL TRUE) add_coal_test(broadphase_dynamic_AABB_tree broadphase_dynamic_AABB_tree.cpp) add_coal_test(broadphase_collision_1 broadphase_collision_1.cpp) add_coal_test(broadphase_collision_2 broadphase_collision_2.cpp) ## Benchmark -add_executable(test-benchmark benchmark.cpp) -target_link_libraries(test-benchmark +set(test_benchmark_target ${PROJECT_NAME}-test-benchmark) +add_executable(${test_benchmark_target} benchmark.cpp) +target_link_libraries(${test_benchmark_target} PUBLIC - utility + ${utility_target} Boost::filesystem ${PROJECT_NAME} ) diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt index 4c59925a0..1e3a95298 100644 --- a/test/python_unit/CMakeLists.txt +++ b/test/python_unit/CMakeLists.txt @@ -8,5 +8,5 @@ SET(${PROJECT_NAME}_PYTHON_TESTS ADD_DEPENDENCIES(build_tests ${PROJECT_NAME}_pywrap) FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) - ADD_PYTHON_UNIT_TEST("py-${TEST}" "test/python_unit/${TEST}.py" "python") -ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) + ADD_PYTHON_UNIT_TEST("${PROJECT_NAME}-py-${TEST}" "test/python_unit/${TEST}.py" "python") +ENDFOREACH() From 86fdcd17cc74dac912186cf8d6196e4771fbac93 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Mon, 15 Jul 2024 14:25:40 +0200 Subject: [PATCH 41/57] cmake: Setup PYTHON_COMPONENTs before calling FINDPYTHON --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index cd74a5a58..6c4d11c2e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,6 +124,7 @@ CMAKE_DEPENDENT_OPTION(GENERATE_PYTHON_STUBS "Generate the Python stubs associat ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.0") if(BUILD_PYTHON_INTERFACE) + SET(PYTHON_COMPONENTS Interpreter Development NumPy) FINDPYTHON(REQUIRED) ADD_PROJECT_PRIVATE_DEPENDENCY(eigenpy 2.9.2 REQUIRED) endif() From 43ee50d73f9d3e2b1f6466f3929a4208a145ec27 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Tue, 16 Jul 2024 15:15:59 +0200 Subject: [PATCH 42/57] cmake: Configure PROJECT_SOURCE_DIR --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c4d11c2e..cbbbe84dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,7 +41,10 @@ set(PROJECT_DESCRIPTION ) SET(PROJECT_USE_CMAKE_EXPORT TRUE) SET(PROJECT_COMPATIBILITY_VERSION AnyNewerVersion) +# To enable jrl-cmakemodules compatibility with workspace we must define the two +# following lines set(PROJECT_AUTO_RUN_FINALIZE FALSE) +set(PROJECT_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}) SET(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE) SET(DOXYGEN_USE_TEMPLATE_CSS TRUE) From db60cca1fce675e24a426e08990dc9b4e4773933 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Tue, 16 Jul 2024 15:17:08 +0200 Subject: [PATCH 43/57] cmake: Use topic/workspace jrl-cmakemodules --- .gitmodules | 2 +- cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 0226cac4c..c209f9f6f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "cmake"] path = cmake - url = https://github.com/jrl-umi3218/jrl-cmakemodules.git + url = https://github.com/jorisv/jrl-cmakemodules.git [submodule "third-parties/qhull"] path = third-parties/qhull url = https://github.com/qhull/qhull.git diff --git a/cmake b/cmake index baf48a3ec..b03972c2a 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit baf48a3ecb96f8dcad616aec006d43cce3307e19 +Subproject commit b03972c2aab5d4ed63bc7cc596d29328f388ef83 From 44468fd73dba9ed002d474043968458d59c7da3f Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Tue, 16 Jul 2024 15:19:22 +0200 Subject: [PATCH 44/57] changelog: Add changelog entry --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 04007a79a..db5ef76b1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,6 +23,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation. - Add `Serializable` trait to transform, collision data, collision geometries, bounding volumes, bvh models, hfields. Collision problems can now be serialized from C++ and sent to python and vice versa. - CMake: allow use of installed jrl-cmakemodules ([#564](https://github.com/humanoid-path-planner/hpp-fcl/pull/564)) +- CMake: Add compatibility with jrl-cmakemodules workspace ([#610](https://github.com/humanoid-path-planner/hpp-fcl/pull/610)) ### Fixed From 902d303c42f5693ecc2165f6c00c5b6aa4d7476a Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 24 Jul 2024 10:29:10 +0200 Subject: [PATCH 45/57] cmake: Switch to upstream jrl-cmakemodules --- .gitmodules | 2 +- cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index c209f9f6f..0226cac4c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "cmake"] path = cmake - url = https://github.com/jorisv/jrl-cmakemodules.git + url = https://github.com/jrl-umi3218/jrl-cmakemodules.git [submodule "third-parties/qhull"] path = third-parties/qhull url = https://github.com/qhull/qhull.git diff --git a/cmake b/cmake index b03972c2a..91b8f5f21 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit b03972c2aab5d4ed63bc7cc596d29328f388ef83 +Subproject commit 91b8f5f2168b123a198da079b8e6c09fd1f60285 From d3225747017d0ea7b4872d5fe7678344024dbdb9 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 24 Jul 2024 11:39:13 +0200 Subject: [PATCH 46/57] cmake: Fix rebase issue --- test/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f4a5b679c..c85077bb0 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -5,7 +5,7 @@ config_files(fcl_resources/config.h) function(add_coal_test test_name source) set(target_name ${PROJECT_NAME}-${test_name}) - ADD_UNIT_TEST(${test_name} ${source}) + ADD_UNIT_TEST(${target_name} ${source}) target_link_libraries(${target_name} PUBLIC ${LIBRARY_NAME} From 0928ea3e94e75c4ed937322860da7e77dc74596b Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 24 Jul 2024 11:56:07 +0200 Subject: [PATCH 47/57] cmake: Add coal backward compatibility support to workspace --- src/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ede3a1c54..f1dfb892a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -189,6 +189,11 @@ add_library(${LIBRARY_NAME} ) add_library(${LIBRARY_NAME}::${LIBRARY_NAME} ALIAS ${LIBRARY_NAME}) +if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) + add_library(hpp-fcl ALIAS ${LIBRARY_NAME}) + add_library(hpp-fcl::hpp-fcl ALIAS ${LIBRARY_NAME}) +endif() + if(UNIX) get_relative_rpath(${CMAKE_INSTALL_LIBDIR} ${PROJECT_NAME}_INSTALL_RPATH) set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH "${${PROJECT_NAME}_INSTALL_RPATH}") From 1b20de92160250e69c53e920975d351ec8939d71 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Tue, 23 Jul 2024 11:48:05 +0200 Subject: [PATCH 48/57] fix coal deprecated header warning --- include/hpp/fcl/coal.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/include/hpp/fcl/coal.hpp b/include/hpp/fcl/coal.hpp index e3c8845e2..e4fdb758c 100644 --- a/include/hpp/fcl/coal.hpp +++ b/include/hpp/fcl/coal.hpp @@ -1,15 +1,19 @@ -#ifndef HPP_FCL_COAL_HH -#define HPP_FCL_COAL_HH +#ifndef HPP_FCL_COAL_HPP +#define HPP_FCL_COAL_HPP #include #include #define COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL +#ifdef _MSC_VER #pragma message COAL_DEPRECATED_HEADER( \ "Please update your includes from 'hpp/fcl' to 'coal'") +#else +#warning "Please update your includes from 'hpp/fcl' to 'coal'" +#endif #define HPP_FCL_VERSION_AT_LEAST(major, minor, patch) \ COAL_VERSION_AT_LEAST(major, minor, patch) -#endif // COAL_FWD_HH +#endif // COAL_FWD_HPP From 3e7eaacddf019717739e734d37e64ccfa3980bcd Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Wed, 24 Jul 2024 22:03:17 +0200 Subject: [PATCH 49/57] CMake hpp-fcl compat: fix for CMake < 3.18 --- hpp-fclConfig.cmake | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/hpp-fclConfig.cmake b/hpp-fclConfig.cmake index 3c30e4845..c09ecf7ff 100644 --- a/hpp-fclConfig.cmake +++ b/hpp-fclConfig.cmake @@ -3,4 +3,15 @@ message(WARNING "Please update your CMake from 'hpp-fcl' to 'coal'") find_package(coal REQUIRED) -add_library(hpp-fcl::hpp-fcl ALIAS coal::coal) + +if(CMAKE_VERSION VERSION_LESS "3.18.0") + if(NOT TARGET hpp-fcl::hpp-fcl) + add_library(hpp-fcl::hpp-fcl SHARED IMPORTED) + target_link_libraries(hpp-fcl::hpp-fcl INTERFACE coal::coal) + get_property(_cfg TARGET coal::coal PROPERTY IMPORTED_CONFIGURATIONS) + get_property(_loc TARGET coal::coal PROPERTY "IMPORTED_LOCATION_${_cfg}") + set_property(TARGET hpp-fcl::hpp-fcl PROPERTY IMPORTED_LOCATION "${_loc}") + endif() +else() + add_library(hpp-fcl::hpp-fcl ALIAS coal::coal) +endif() From 3e481afd548acade039da900cf0a574825cbd094 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Mon, 22 Jul 2024 19:15:23 +0200 Subject: [PATCH 50/57] python: expose missing `Transform3s::Random` --- include/coal/math/transform.h | 12 +++++++----- python/math.cc | 4 ++++ 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h index d2cf8ed41..7d712fadc 100644 --- a/include/coal/math/transform.h +++ b/include/coal/math/transform.h @@ -202,10 +202,14 @@ class COAL_DLLAPI Transform3s { } /// @brief return a random transform - static Transform3s Random() { return Transform3s().setRandom(); } + static Transform3s Random() { + Transform3s tf = Transform3s(); + tf.setRandom(); + return tf; + } /// @brief set the transform to a random transform - Transform3s& setRandom(); + inline void setRandom(); bool operator==(const Transform3s& other) const { return (R == other.getRotation()) && (T == other.getTranslation()); @@ -247,12 +251,10 @@ inline Quatf uniformRandomQuaternion() { return q; } -inline Transform3s& Transform3s::setRandom() { +inline void Transform3s::setRandom() { const Quatf q = uniformRandomQuaternion(); this->rotation() = q.matrix(); this->translation().setRandom(); - - return *this; } /// @brief Construct othonormal basis from vector. diff --git a/python/math.cc b/python/math.cc index be6d13dc4..0261ae5f1 100644 --- a/python/math.cc +++ b/python/math.cc @@ -111,6 +111,10 @@ void exposeMaths() { .def(dv::member_func("Identity", &Transform3s::Identity)) .staticmethod("Identity") + .def(dv::member_func("setRandom", &Transform3s::setRandom)) + .def(dv::member_func("Random", &Transform3s::Random)) + .staticmethod("Random") + .def(dv::member_func("transform", &Transform3s::transform)) .def("inverseInPlace", &Transform3s::inverseInPlace, return_internal_reference<>(), From 60ad7d2bf26b36bc3762cb5d48a830d6cfb450c7 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 19 Sep 2024 15:15:58 +0200 Subject: [PATCH 51/57] cmake: Create hpp-fcl-compatibility component to allow to only install the compatibility layer --- .gitmodules | 2 +- CMakeLists.txt | 10 ++++++++-- cmake | 2 +- hpp-fclConfig.cmake | 5 ++++- python/CMakeLists.txt | 4 ++-- 5 files changed, 16 insertions(+), 7 deletions(-) diff --git a/.gitmodules b/.gitmodules index 0226cac4c..c209f9f6f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "cmake"] path = cmake - url = https://github.com/jrl-umi3218/jrl-cmakemodules.git + url = https://github.com/jorisv/jrl-cmakemodules.git [submodule "third-parties/qhull"] path = third-parties/qhull url = https://github.com/qhull/qhull.git diff --git a/CMakeLists.txt b/CMakeLists.txt index cbbbe84dc..72f69ead4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -317,7 +317,7 @@ SET(${PROJECT_NAME}_HEADERS ) if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) - LIST(APPEND ${PROJECT_NAME}_HEADERS + SET(HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS include/hpp/fcl/broadphase/broadphase_bruteforce.h include/hpp/fcl/broadphase/broadphase_callbacks.h include/hpp/fcl/broadphase/broadphase_collision_manager.h @@ -444,6 +444,8 @@ if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) include/hpp/fcl/timings.h include/hpp/fcl/warning.hh ) + LIST(APPEND ${PROJECT_NAME}_HEADERS ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS}) + HEADER_INSTALL(COMPONENT hpp-fcl-compatibility ${HPP_FCL_BACKWARD_COMPATIBILITY_HEADERS}) endif() IF(COAL_HAS_OCTOMAP) @@ -485,7 +487,11 @@ if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) write_basic_package_version_file(hpp-fclConfigVersion.cmake VERSION 3.0.0 COMPATIBILITY AnyNewerVersion) - install(FILES hpp-fclConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/hpp-fclConfigVersion.cmake DESTINATION lib/cmake/hpp-fcl) + install(FILES hpp-fclConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/hpp-fclConfigVersion.cmake + DESTINATION lib/cmake/hpp-fcl + COMPONENT hpp-fcl-compatibility) + include("${JRL_CMAKE_MODULES}/install-helpers.cmake") + add_install_target(NAME hpp-fcl-compatibility COMPONENT hpp-fcl-compatibility) endif() setup_project_finalize() diff --git a/cmake b/cmake index 91b8f5f21..053b36a9a 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 91b8f5f2168b123a198da079b8e6c09fd1f60285 +Subproject commit 053b36a9a584b5ce8baf363a63bdd494999962c7 diff --git a/hpp-fclConfig.cmake b/hpp-fclConfig.cmake index c09ecf7ff..326135bfe 100644 --- a/hpp-fclConfig.cmake +++ b/hpp-fclConfig.cmake @@ -11,7 +11,10 @@ if(CMAKE_VERSION VERSION_LESS "3.18.0") get_property(_cfg TARGET coal::coal PROPERTY IMPORTED_CONFIGURATIONS) get_property(_loc TARGET coal::coal PROPERTY "IMPORTED_LOCATION_${_cfg}") set_property(TARGET hpp-fcl::hpp-fcl PROPERTY IMPORTED_LOCATION "${_loc}") + target_compile_definitions(hpp-fcl::hpp-fcl PUBLIC COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) endif() else() - add_library(hpp-fcl::hpp-fcl ALIAS coal::coal) + add_library(hpp-fcl::hpp-fcl INTERFACE) + target_link_libraries(hpp-fcl::hpp-fcl INTERFACE coal::coal) + target_compile_definitions(hpp-fcl::hpp-fcl INTERFACE COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) endif() diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index d28714c59..38b98031b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -191,6 +191,6 @@ ENDFOREACH(python) if(COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) - python_install_on_site(hppfcl __init__.py) - python_install_on_site(hppfcl viewer.py) + python_install_on_site(hppfcl __init__.py COMPONENT hpp-fcl-compatibility) + python_install_on_site(hppfcl viewer.py COMPONENT hpp-fcl-compatibility) endif() From 50ee6b9036648e7c228e8e08f46adc134805a4b3 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Mon, 23 Sep 2024 15:30:18 +0200 Subject: [PATCH 52/57] cmake: Always use the IMPORTED target --- hpp-fclConfig.cmake | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/hpp-fclConfig.cmake b/hpp-fclConfig.cmake index 326135bfe..1e2664285 100644 --- a/hpp-fclConfig.cmake +++ b/hpp-fclConfig.cmake @@ -4,17 +4,11 @@ message(WARNING "Please update your CMake from 'hpp-fcl' to 'coal'") find_package(coal REQUIRED) -if(CMAKE_VERSION VERSION_LESS "3.18.0") - if(NOT TARGET hpp-fcl::hpp-fcl) - add_library(hpp-fcl::hpp-fcl SHARED IMPORTED) - target_link_libraries(hpp-fcl::hpp-fcl INTERFACE coal::coal) - get_property(_cfg TARGET coal::coal PROPERTY IMPORTED_CONFIGURATIONS) - get_property(_loc TARGET coal::coal PROPERTY "IMPORTED_LOCATION_${_cfg}") - set_property(TARGET hpp-fcl::hpp-fcl PROPERTY IMPORTED_LOCATION "${_loc}") - target_compile_definitions(hpp-fcl::hpp-fcl PUBLIC COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) - endif() -else() - add_library(hpp-fcl::hpp-fcl INTERFACE) - target_link_libraries(hpp-fcl::hpp-fcl INTERFACE coal::coal) - target_compile_definitions(hpp-fcl::hpp-fcl INTERFACE COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) +if(NOT TARGET hpp-fcl::hpp-fcl) + add_library(hpp-fcl::hpp-fcl SHARED IMPORTED) + target_link_libraries(hpp-fcl::hpp-fcl INTERFACE coal::coal) + get_property(_cfg TARGET coal::coal PROPERTY IMPORTED_CONFIGURATIONS) + get_property(_loc TARGET coal::coal PROPERTY "IMPORTED_LOCATION_${_cfg}") + set_property(TARGET hpp-fcl::hpp-fcl PROPERTY IMPORTED_LOCATION "${_loc}") + target_compile_definitions(hpp-fcl::hpp-fcl INTERFACE COAL_BACKWARD_COMPATIBILITY_WITH_HPP_FCL) endif() From 93660b8041ab5ae5cdf13d25c443b1369f4cf2a4 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Mon, 23 Sep 2024 15:31:10 +0200 Subject: [PATCH 53/57] cmake: Update jrl-cmakemodule --- cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake b/cmake index 053b36a9a..ba4e0bd96 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 053b36a9a584b5ce8baf363a63bdd494999962c7 +Subproject commit ba4e0bd964d21d1b92f67d327ca943718c8a3280 From 1b6b198e17bffeead5e60c902c7d7a82fa31b267 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 20 Nov 2024 11:23:30 +0100 Subject: [PATCH 54/57] cmake: Update jrl-cmakemodules --- .gitmodules | 2 +- cmake | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index c209f9f6f..0226cac4c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "cmake"] path = cmake - url = https://github.com/jorisv/jrl-cmakemodules.git + url = https://github.com/jrl-umi3218/jrl-cmakemodules.git [submodule "third-parties/qhull"] path = third-parties/qhull url = https://github.com/qhull/qhull.git diff --git a/cmake b/cmake index ba4e0bd96..f1f95f942 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit ba4e0bd964d21d1b92f67d327ca943718c8a3280 +Subproject commit f1f95f942fabd3d4dc7e39aa7f1c20a5b0735165 From cfd085815205c7a9f4523ad78f3ef21ca455f39e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 7 Oct 2024 17:45:48 +0000 Subject: [PATCH 55/57] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.5.1 → v0.6.9](https://github.com/astral-sh/ruff-pre-commit/compare/v0.5.1...v0.6.9) - [github.com/pre-commit/mirrors-clang-format: v18.1.8 → v19.1.1](https://github.com/pre-commit/mirrors-clang-format/compare/v18.1.8...v19.1.1) - [github.com/pre-commit/pre-commit-hooks: v4.6.0 → v5.0.0](https://github.com/pre-commit/pre-commit-hooks/compare/v4.6.0...v5.0.0) --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a731840ef..d81c0fba4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ ci: autoupdate_schedule: quarterly repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.5.1 + rev: v0.6.9 hooks: - id: ruff args: @@ -12,12 +12,12 @@ repos: - --exit-non-zero-on-fix - id: ruff-format - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.1 hooks: - id: clang-format args: - '--style={BasedOnStyle: Google, SortIncludes: false}' - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: trailing-whitespace From 02c9cf0ebaecb296717059effff2874eaebcd265 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 20 Nov 2024 11:30:30 +0100 Subject: [PATCH 56/57] pre-commit: run clang-format --- include/coal/collision_data.h | 10 +-- python/collision-geometries.cc | 7 +-- src/BV/OBB.cpp | 43 +++++++------ src/collision_func_matrix.cpp | 95 ++++++++++++++--------------- src/distance_func_matrix.cpp | 91 +++++++++++++-------------- src/traits_traversal.h | 4 +- src/traversal/traversal_recurse.cpp | 10 +-- 7 files changed, 125 insertions(+), 135 deletions(-) diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h index 6e17f0485..e885e66a6 100644 --- a/include/coal/collision_data.h +++ b/include/coal/collision_data.h @@ -987,12 +987,12 @@ struct COAL_DLLAPI DistanceRequest : QueryRequest { /// Nearest points are always computed and are the points of the shapes that /// achieve a distance of `DistanceResult::min_distance`. COAL_DEPRECATED_MESSAGE( - Nearest points are always computed - : they are the points of the shapes that achieve a distance of + Nearest points are always computed : they are the points of the shapes + that achieve a distance of `DistanceResult::min_distance` - .\n Use `enable_signed_distance` if you want to compute a signed - minimum distance(and thus its corresponding nearest points) - .) + .\n Use `enable_signed_distance` if you want to compute a signed + minimum distance(and thus its corresponding nearest points) + .) bool enable_nearest_points; /// @brief whether to compute the penetration depth when objects are in diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 9d246b48e..b3afc5594 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -577,8 +577,7 @@ void exposeCollisionGeometries() { "Check whether two AABB are overlaping and return the overloaping " "part if true.") - .def("distance", - (CoalScalar(AABB::*)(const AABB&) const) & AABB::distance, + .def("distance", (CoalScalar(AABB::*)(const AABB&) const)&AABB::distance, bp::args("self", "other"), "Distance between two AABBs.") // .def("distance", // AABB_distance_proxy, @@ -643,10 +642,10 @@ void exposeCollisionGeometries() { #endif ; - def("translate", (AABB(*)(const AABB&, const Vec3s&)) & translate, + def("translate", (AABB(*)(const AABB&, const Vec3s&))&translate, bp::args("aabb", "t"), "Translate the center of AABB by t"); - def("rotate", (AABB(*)(const AABB&, const Matrix3s&)) & rotate, + def("rotate", (AABB(*)(const AABB&, const Matrix3s&))&rotate, bp::args("aabb", "R"), "Rotate the AABB object by R"); if (!eigenpy::register_symbolic_link_to_registered_type< diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 6ae95bcf7..b956fe144 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -310,29 +310,28 @@ inline CoalScalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T, } template -struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi { - static inline bool run(int ia, int ja, int ka, const Matrix3s& B, - const Vec3s& T, const Vec3s& a, const Vec3s& b, - const Matrix3s& Bf, const CoalScalar& breakDistance2, - CoalScalar& squaredLowerBoundDistance) { - CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); - if (sinus2 < 1e-6) return false; - - const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); - - const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + - b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); - // We need to divide by the norm || Aia x Bib || - // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 - if (diff > 0) { - squaredLowerBoundDistance = diff * diff / sinus2; - if (squaredLowerBoundDistance > breakDistance2) { - return true; - } - } - return false; +struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run( + int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a, + const Vec3s& b, const Matrix3s& Bf, const CoalScalar& breakDistance2, + CoalScalar& squaredLowerBoundDistance){CoalScalar sinus2 = + 1 - Bf(ia, ib) * Bf(ia, ib); +if (sinus2 < 1e-6) return false; + +const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); + +const CoalScalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + + b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); +// We need to divide by the norm || Aia x Bib || +// As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 +if (diff > 0) { + squaredLowerBoundDistance = diff * diff / sinus2; + if (squaredLowerBoundDistance > breakDistance2) { + return true; } -}; +} +return false; +} // namespace internal +}; // namespace coal } // namespace internal // B, T orientation and position of 2nd OBB in frame of 1st OBB, diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index e30a2c510..b654cfbda 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -97,66 +97,61 @@ BVH_SHAPE_DEFAULT_TO_ORIENTED(OBBRSS); /// - 0 if the query should be made with non-aligned object frames. template ::Options> -struct COAL_LOCAL BVHShapeCollider { - static std::size_t collide(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { +struct COAL_LOCAL BVHShapeCollider{static std::size_t collide( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, const CollisionRequest& request, + CollisionResult& result){ if (request.isSatisfied(result)) return result.numContacts(); - if (request.security_margin < 0) - COAL_THROW_PRETTY( - "Negative security margin are not handled yet for BVHModel", - std::invalid_argument); +if (request.security_margin < 0) + COAL_THROW_PRETTY("Negative security margin are not handled yet for BVHModel", + std::invalid_argument); - if (_Options & RelativeTransformationIsIdentity) - return aligned(o1, tf1, o2, tf2, nsolver, request, result); - else - return oriented(o1, tf1, o2, tf2, nsolver, request, result); - } +if (_Options & RelativeTransformationIsIdentity) + return aligned(o1, tf1, o2, tf2, nsolver, request, result); +else + return oriented(o1, tf1, o2, tf2, nsolver, request, result); +} // namespace coal - static std::size_t aligned(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - if (request.isSatisfied(result)) return result.numContacts(); +static std::size_t aligned(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); - MeshShapeCollisionTraversalNode - node(request); - const BVHModel* obj1 = static_cast*>(o1); - BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3s tf1_tmp = tf1; - const T_SH* obj2 = static_cast(o2); + MeshShapeCollisionTraversalNode + node(request); + const BVHModel* obj1 = static_cast*>(o1); + BVHModel* obj1_tmp = new BVHModel(*obj1); + Transform3s tf1_tmp = tf1; + const T_SH* obj2 = static_cast(o2); - initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); - coal::collide(&node, request, result); + initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, result); + coal::collide(&node, request, result); - delete obj1_tmp; - return result.numContacts(); - } + delete obj1_tmp; + return result.numContacts(); +} - static std::size_t oriented(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - if (request.isSatisfied(result)) return result.numContacts(); +static std::size_t oriented(const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); - MeshShapeCollisionTraversalNode node(request); - const BVHModel* obj1 = static_cast*>(o1); - const T_SH* obj2 = static_cast(o2); + MeshShapeCollisionTraversalNode node(request); + const BVHModel* obj1 = static_cast*>(o1); + const T_SH* obj2 = static_cast(o2); - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); - coal::collide(&node, request, result); - return result.numContacts(); - } -}; + initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); + coal::collide(&node, request, result); + return result.numContacts(); +} +} +; /// @brief Collider functor for HeightField data structure /// \tparam _Options takes two values. diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp index fbfeb6640..d9aae3ba3 100644 --- a/src/distance_func_matrix.cpp +++ b/src/distance_func_matrix.cpp @@ -83,27 +83,26 @@ COAL_LOCAL CoalScalar distance_function_not_implemented( } template -struct COAL_LOCAL BVHShapeDistancer { - static CoalScalar distance(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { +struct COAL_LOCAL BVHShapeDistancer{static CoalScalar distance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result){ if (request.isSatisfied(result)) return result.min_distance; - MeshShapeDistanceTraversalNode node; - const BVHModel* obj1 = static_cast*>(o1); - BVHModel* obj1_tmp = new BVHModel(*obj1); - Transform3s tf1_tmp = tf1; - const T_SH* obj2 = static_cast(o2); +MeshShapeDistanceTraversalNode node; +const BVHModel* obj1 = static_cast*>(o1); +BVHModel* obj1_tmp = new BVHModel(*obj1); +Transform3s tf1_tmp = tf1; +const T_SH* obj2 = static_cast(o2); - initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); - ::coal::distance(&node); +initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); +::coal::distance(&node); - delete obj1_tmp; - return result.min_distance; - } -}; +delete obj1_tmp; +return result.min_distance; +} // namespace coal +} +; namespace details { @@ -172,35 +171,33 @@ struct COAL_LOCAL BVHShapeDistancer { }; template -struct COAL_LOCAL HeightFieldShapeDistancer { - static CoalScalar distance(const CollisionGeometry* o1, - const Transform3s& tf1, - const CollisionGeometry* o2, - const Transform3s& tf2, const GJKSolver* nsolver, - const DistanceRequest& request, - DistanceResult& result) { - COAL_UNUSED_VARIABLE(o1); - COAL_UNUSED_VARIABLE(tf1); - COAL_UNUSED_VARIABLE(o2); - COAL_UNUSED_VARIABLE(tf2); - COAL_UNUSED_VARIABLE(nsolver); - COAL_UNUSED_VARIABLE(request); - // TODO(jcarpent) - COAL_THROW_PRETTY( - "Distance between a height field and a shape is not implemented", - std::invalid_argument); - // if(request.isSatisfied(result)) return result.min_distance; - // HeightFieldShapeDistanceTraversalNode node; - // - // const HeightField* obj1 = static_cast* - // >(o1); const T_SH* obj2 = static_cast(o2); - // - // initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); - // fcl::distance(&node); - - return result.min_distance; - } -}; +struct COAL_LOCAL HeightFieldShapeDistancer{static CoalScalar distance( + const CollisionGeometry* o1, const Transform3s& tf1, + const CollisionGeometry* o2, const Transform3s& tf2, + const GJKSolver* nsolver, const DistanceRequest& request, + DistanceResult& result){COAL_UNUSED_VARIABLE(o1); +COAL_UNUSED_VARIABLE(tf1); +COAL_UNUSED_VARIABLE(o2); +COAL_UNUSED_VARIABLE(tf2); +COAL_UNUSED_VARIABLE(nsolver); +COAL_UNUSED_VARIABLE(request); +// TODO(jcarpent) +COAL_THROW_PRETTY( + "Distance between a height field and a shape is not implemented", + std::invalid_argument); +// if(request.isSatisfied(result)) return result.min_distance; +// HeightFieldShapeDistanceTraversalNode node; +// +// const HeightField* obj1 = static_cast* +// >(o1); const T_SH* obj2 = static_cast(o2); +// +// initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); +// fcl::distance(&node); + +return result.min_distance; +} +} +; template CoalScalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1, diff --git a/src/traits_traversal.h b/src/traits_traversal.h index 8666d34a2..5667a69c6 100644 --- a/src/traits_traversal.h +++ b/src/traits_traversal.h @@ -18,7 +18,7 @@ namespace coal { // TraversalTraitsCollision for collision_func_matrix.cpp template -struct COAL_LOCAL TraversalTraitsCollision {}; +struct COAL_LOCAL TraversalTraitsCollision{}; #ifdef COAL_HAS_OCTOMAP @@ -62,7 +62,7 @@ struct COAL_LOCAL TraversalTraitsCollision, OcTree> { // TraversalTraitsDistance for distance_func_matrix.cpp template -struct COAL_LOCAL TraversalTraitsDistance {}; +struct COAL_LOCAL TraversalTraitsDistance{}; #ifdef COAL_HAS_OCTOMAP diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp index 909e2979b..2b133c5a6 100644 --- a/src/traversal/traversal_recurse.cpp +++ b/src/traversal/traversal_recurse.cpp @@ -211,11 +211,11 @@ struct COAL_LOCAL BVT { }; /** @brief Comparer between two BVT */ -struct COAL_LOCAL BVT_Comparer { - bool operator()(const BVT& lhs, const BVT& rhs) const { - return lhs.d > rhs.d; - } -}; +struct COAL_LOCAL BVT_Comparer{bool operator()(const BVT& lhs, const BVT& rhs) + const {return lhs.d > rhs.d; +} // namespace coal +} +; struct COAL_LOCAL BVTQ { BVTQ() : qsize(2) {} From 630f495c7b80fd968daba0d9b42a3045a9bb44d9 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Nov 2024 13:51:24 +0100 Subject: [PATCH 57/57] readme: change hppfcl to coal where needed todo: change the badges once the migration is done --- README.md | 46 +++++++++--------- ...performances.jpg => coal-performances.jpg} | Bin ....pdf => coal-vs-the-rest-of-the-world.pdf} | Bin ....png => coal-vs-the-rest-of-the-world.png} | Bin 4 files changed, 24 insertions(+), 22 deletions(-) rename doc/images/{hpp-fcl-performances.jpg => coal-performances.jpg} (100%) rename doc/images/{hpp-fcl-vs-the-rest-of-the-world.pdf => coal-vs-the-rest-of-the-world.pdf} (100%) rename doc/images/{hpp-fcl-vs-the-rest-of-the-world.png => coal-vs-the-rest-of-the-world.png} (100%) diff --git a/README.md b/README.md index 79f0114c3..88e27ccac 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# HPP-FCL — An extension of the Flexible Collision Library +# Coal — An extension of the Flexible Collision Library

Pipeline status @@ -11,16 +11,16 @@ ruff

-[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015. -Since then, a large part of the code has been rewritten or removed (for the unused and untested part). -The broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022 based on the FCL version 0.7.0. +[FCL](https://github.com/flexible-collision-library/fcl) was forked in 2015, creating a new project called HPP-FCL. +Since then, a large part of the code has been rewritten or removed (for the unused and untested part), and new features have been developped (see below). +Due to these major changes, it was decided in 2024 to rename the HPP-FCL project to **Coal**. -If you use **HPP-FCL** in your projects and research papers, we would appreciate it if you'd [cite it](https://raw.githubusercontent.com/humanoid-path-planner/hpp-fcl/devel/CITATION.bib). +If you use **Coal** in your projects and research papers, we would appreciate it if you'd [cite it](https://raw.githubusercontent.com/coal-library/coal/devel/CITATION.bib). ## New features Compared to the original [FCL](https://github.com/flexible-collision-library/fcl) library, the main new features are: -- a dedicated and efficient implementation of the GJK algorithm (we do not rely anymore on [libccd](https://github.com/danfis/libccd)) +- dedicated and efficient implementations of the GJK and the EPA algorithms (we do not rely on [libccd](https://github.com/danfis/libccd)) - the support of safety margins for collision detection - an accelerated version of collision detection *à la Nesterov*, which leads to increased performances (up to a factor of 2). More details are available in this [paper](https://hal.archives-ouvertes.fr/hal-03662157/) - the computation of a lower bound of the distance between two objects when collision checking is performed, and no collision is found @@ -30,13 +30,15 @@ Compared to the original [FCL](https://github.com/flexible-collision-library/fcl - efficient computation of **contact points** and **contact patches** between objects - full support of object serialization via Boost.Serialization -This project is now used in many robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source software that implements efficient and versatile rigid body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source software for Motion and Manipulation Planning. **HPP-FCL** has also been recently used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond. +Note: the broad phase was reintroduced by [Justin Carpentier](https://github.com/jcarpent) in 2022, based on the FCL version 0.7.0. + +This project is now used in many robotics frameworks such as [Pinocchio](https://github.com/stack-of-tasks/pinocchio), an open-source software that implements efficient and versatile rigid body dynamics algorithms, the [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc), an open-source software for Motion and Manipulation Planning. **Coal** has also been recently used to develop [Simple](https://github.com/Simple-Robotics/Simple), a new (differentiable) and efficient simulator for robotics and beyond. ## A high-performance library -Unlike the original FCL library, HPP-FCL implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performances, as depicted by the figures below. +Unlike the original FCL library, Coal implements the well-established [GJK algorithm](https://en.wikipedia.org/wiki/Gilbert%E2%80%93Johnson%E2%80%93Keerthi_distance_algorithm) and [its variants](https://hal.archives-ouvertes.fr/hal-03662157/) for collision detection and distance computation. These implementations lead to state-of-the-art performances, as depicted by the figures below. -On the one hand, we have benchmarked HPP-FCL against major software alternatives of the state of the art: +On the one hand, we have benchmarked Coal against major software alternatives of the state of the art: 1. the [Bullet simulator](https://github.com/bulletphysics/bullet3), 2. the original [FCL library](https://github.com/flexible-collision-library/fcl) (used in the [Drake framework]()), 3. the [libccd library](https://github.com/danfis/libccd) (used in [MuJoCo](http://mujoco.org/)). @@ -45,13 +47,13 @@ The results are depicted in the following figure, which notably shows that the a Please notice that the y-axis is in log scale.

- HPP-FCL vs the rest of the world + Coal vs the rest of the world

-On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why. +On the other hand, why do we care about dedicated collision detection solvers like GJK for the narrow phase? Why can't we simply formulate the collision detection problem as a quadratic problem and call an off-the-shelf optimization solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite))? Here is why:

- HPP-FCL vs generic QP solvers + Coal vs generic QP solvers

One can observe that GJK-based approaches largely outperform solutions based on classic optimization solvers (e.g., QP solver like [ProxQP](https://github.com/Simple-Robotics/proxsuite)), notably for large geometries composed of tens or hundreds of vertices. @@ -67,16 +69,16 @@ One can observe that GJK-based approaches largely outperform solutions based on - [ocs2](https://github.com/leggedrobotics/ocs2) A toolbox for Optimal Control for Switched Systems (OCS2) ## C++ example -Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install hpp-fcl`. +Both the C++ library and the python bindings can be installed as simply as `conda -c conda-forge install coal`. The `.so` library, include files and python bindings will then be installed under `$CONDA_PREFIX/lib`, `$CONDA_PREFIX/include` and `$CONDA_PREFIX/lib/python3.XX/site-packages`. -Here is an example of using HPP-FCL in C++: +Here is an example of using Coal in C++: ```cpp -#include "hpp/fcl/math/transform.h" -#include "hpp/fcl/mesh_loader/loader.h" -#include "hpp/fcl/BVH/BVH_model.h" -#include "hpp/fcl/collision.h" -#include "hpp/fcl/collision_data.h" +#include "coal/math/transform.h" +#include "coal/mesh_loader/loader.h" +#include "coal/BVH/BVH_model.h" +#include "coal/collision.h" +#include "coal/collision_data.h" #include #include @@ -158,14 +160,14 @@ int main() { ``` ## Python example -Here is the C++ example from above translated in python using HPP-FCL's python bindings: +Here is the C++ example from above translated in python using the python bindings of Coal: ```python import numpy as np import coal # Optional: # The Pinocchio library is a rigid body algorithms library and has a handy SE3 module. # It can be installed as simply as `conda -c conda-forge install pinocchio`. -# Installing pinocchio also installs hpp-fcl. +# Installing pinocchio also installs coal. import pinocchio as pin def loadConvexMesh(file_name: str): @@ -211,4 +213,4 @@ if __name__ == "__main__": ## Acknowledgments -The development of **HPP-FCL** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extend, [Eureka Robotics](https://eurekarobotics.com/). +The development of **Coal** is actively supported by the [Gepetto team](http://projects.laas.fr/gepetto/) [@LAAS-CNRS](http://www.laas.fr), the [Willow team](https://www.di.ens.fr/willow/) [@INRIA](http://www.inria.fr) and, to some extend, [Eureka Robotics](https://eurekarobotics.com/). diff --git a/doc/images/hpp-fcl-performances.jpg b/doc/images/coal-performances.jpg similarity index 100% rename from doc/images/hpp-fcl-performances.jpg rename to doc/images/coal-performances.jpg diff --git a/doc/images/hpp-fcl-vs-the-rest-of-the-world.pdf b/doc/images/coal-vs-the-rest-of-the-world.pdf similarity index 100% rename from doc/images/hpp-fcl-vs-the-rest-of-the-world.pdf rename to doc/images/coal-vs-the-rest-of-the-world.pdf diff --git a/doc/images/hpp-fcl-vs-the-rest-of-the-world.png b/doc/images/coal-vs-the-rest-of-the-world.png similarity index 100% rename from doc/images/hpp-fcl-vs-the-rest-of-the-world.png rename to doc/images/coal-vs-the-rest-of-the-world.png