From e78aa64e1cf5331e5a056004cd6b771a069717d5 Mon Sep 17 00:00:00 2001
From: Louis Montaut <louismontaut@gmail.com>
Date: Sun, 23 Jun 2024 17:18:01 +0200
Subject: [PATCH] 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 b69bcc2b..2b7019f9 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 55c45548..8c91a382 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<T_BVH>::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<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box]
"void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" [shape = box]
- "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" [shape = box]
+ "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" [shape = box]
"void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* 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<T_BVH>::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<T_BVH>::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<T_BVH>* 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<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"-> "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)"
- "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)"
- "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result"
- "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)" -> "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, FCL_REAL& sqrDistLowerBound) const\n -request\n - result"
- "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)"
- "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode<T_BVH>::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<T_BVH>::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<T_BVH>::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<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+ "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result"
+ "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)" -> "MeshCollisionTraversalNode<T_BVH>::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result"
+ "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "void collisionRecurse(MeshCollisionTraversalNode<T_BVH>* node,\n int b1, int b2, BVHFrontList* front_list, CoalScalar& sqrDistLowerBound)"
+ "void propagateBVHFrontListCollisionRecurse(MeshCollisionTraversalNode<T_BVH>* node\n, BVHFrontList* front_list, const CollisionRequest& request, CollisionResult& result)" -> "bool MeshCollisionTraversalNode<T_BVH>::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<T_BVH>::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<T_BVH>::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 839f575a..c08fbdfe 100644
--- a/doc/shape-mesh-collision-call.dot
+++ b/doc/shape-mesh-collision-call.dot
@@ -8,29 +8,29 @@ digraph CD {
"details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, 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 <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* 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<Shape>\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 <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* 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<Shape>\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<OBBRSS, Shape, NarrowPhaseSolver>::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<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, 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<MeshShapeCollisionTraversalNodeOBBRSS\n<T_SH, NarrowPhaseSolver>, 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 <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* 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 <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* 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<Shape>\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 <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* 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 <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* 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<Shape>\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 6690f4eb..be01df25 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<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\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<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\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<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\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<typename S1, typename S2> 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<typename S1, typename S2> 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<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\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<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nFCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)"
- "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\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<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\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<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\nCoalScalar ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,\nconst CollisionGeometry* o2, const Transform3f& tf2,\nconst NarrowPhaseSolver* nsolver, const DistanceRequest& request,\nDistanceResult& result)"
+ "template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>\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<typename S1, typename S2> 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<typename S1, typename S2> 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 c0aa81b6..22ae0ab0 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 7cca2048..d6c6a7e0 100644
--- a/include/coal/BV/BV.h
+++ b/include/coal/BV/BV.h
@@ -65,7 +65,7 @@ template <>
struct Converter<AABB, AABB> {
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 <typename BV1>
struct Converter<BV1, AABB> {
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<BV1, AABB> {
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<AABB, RSS> {
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 9e7adfad..43f33863 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 d9bd0c50..b16f3d01 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 5e1da89f..003b396e 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 e0916fe2..8d9cfba5 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<FCL_REAL>() * radius * radius *
+ 4 * boost::math::constants::pi<CoalScalar>() * 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 cb35f783..4803cd4b 100644
--- a/include/coal/BV/kDOP.h
+++ b/include/coal/BV/kDOP.h
@@ -91,7 +91,7 @@ template <short N>
class COAL_DLLAPI KDOP {
protected:
/// @brief Origin's distances to N KDOP planes
- Eigen::Array<FCL_REAL, N, 1> dist_;
+ Eigen::Array<CoalScalar, N, 1> 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<N>& other, const CollisionRequest& request,
- FCL_REAL& sqrDistLowerBound) const;
+ CoalScalar& sqrDistLowerBound) const;
/// @brief The distance between two KDOP<N>. Not implemented.
- FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL,
- Vec3f* Q = NULL) const;
+ CoalScalar distance(const KDOP<N>& other, Vec3f* P = NULL,
+ Vec3f* Q = NULL) const;
/// @brief Merge the point and the KDOP
KDOP<N>& operator+=(const Vec3f& p);
@@ -139,7 +139,7 @@ class COAL_DLLAPI KDOP {
KDOP<N> operator+(const KDOP<N>& 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<N>& /*b1*/,
template <short N>
bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
const KDOP<N>& /*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 36eaee61..c663bd2c 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 71b83018..de80f330 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<Triangle>& 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 21434736..a3687ab4 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 fdb46cb2..9ca05050 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<CollisionObject*>::const_iterator pos_start,
typename std::vector<CollisionObject*>::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<CollisionObject*>& objs_x,
diff --git a/include/coal/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h
index 209ad6af..28c06e57 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<CollisionObject*, SaPAABB*> 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 56fe4f1c..5a132d10 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 c2cec955..c2e36e74 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<float>;
using BroadPhaseContinuousCollisionManagerd =
- BroadPhaseContinuousCollisionManager<FCL_REAL>;
+ BroadPhaseContinuousCollisionManager<CoalScalar>;
} // 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 05d8881a..870e39b0 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<Derived>& 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 caafe009..29655516 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<Derived>& 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 a97799c2..9950cf82 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<detail::SimpleInterval*>::const_iterator pos_start,
typename std::deque<detail::SimpleInterval*>::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<EndPoint> endpoints[3];
diff --git a/include/coal/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h
index fd4ffa0b..1509bcd7 100644
--- a/include/coal/broadphase/broadphase_spatialhash-inl.h
+++ b/include/coal/broadphase/broadphase_spatialhash-inl.h
@@ -45,7 +45,7 @@ namespace coal {
//==============================================================================
template <typename HashTable>
SpatialHashingCollisionManager<HashTable>::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 <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(
CollisionObject* obj, DistanceCallBackBase* callback) const {
if (size() == 0) return;
- FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
distance_(obj, callback, min_dist);
}
@@ -309,10 +309,10 @@ bool SpatialHashingCollisionManager<HashTable>::collide_(
template <typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::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<FCL_REAL>::max)()) {
+ if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
}
@@ -320,7 +320,7 @@ bool SpatialHashingCollisionManager<HashTable>::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<HashTable>::distance_(
}
if (status == 1) {
- if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)()) {
+ if (old_min_distance < (std::numeric_limits<CoalScalar>::max)()) {
break;
} else {
if (min_dist < old_min_distance) {
@@ -422,7 +422,7 @@ void SpatialHashingCollisionManager<HashTable>::distance(
this->enable_tested_set_ = true;
this->tested_set.clear();
- FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
for (const auto& obj : objs) {
if (distance_(obj, callback, min_dist)) break;
@@ -473,7 +473,7 @@ void SpatialHashingCollisionManager<HashTable>::distance(
return;
}
- FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
if (this->size() < other_manager->size()) {
for (const auto& obj : objs)
@@ -512,7 +512,7 @@ template <typename HashTable>
template <typename Container>
bool SpatialHashingCollisionManager<HashTable>::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 17f37e05..e2828a53 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<CollisionObject*> objs;
@@ -157,7 +157,7 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager {
template <typename Container>
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 a124cd8d..711b6c70 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 85d0c889..b771e77e 100644
--- a/include/coal/broadphase/detail/hierarchy_tree-inl.h
+++ b/include/coal/broadphase/detail/hierarchy_tree-inl.h
@@ -159,7 +159,7 @@ template <typename S, typename BV>
struct UpdateImpl {
static bool run(const HierarchyTree<BV>& tree,
typename HierarchyTree<BV>::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 <typename BV>
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel,
- FCL_REAL margin) {
- return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel, margin);
+ CoalScalar margin) {
+ return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel, margin);
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel) {
- return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel);
+ return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel);
}
//==============================================================================
@@ -302,10 +302,10 @@ void HierarchyTree<BV>::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<FCL_REAL>::max)();
+ CoalScalar min_size = (std::numeric_limits<CoalScalar>::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<BV>::Node* HierarchyTree<BV>::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<BV>::Node* HierarchyTree<BV>::topdown_1(
split_p += (*it)->bv.center();
vol += (*it)->bv;
}
- split_p /= static_cast<FCL_REAL>(num_leaves);
+ split_p /= static_cast<CoalScalar>(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<BV>::Node* HierarchyTree<BV>::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<BV>::init_1(std::vector<Node*>& 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<FCL_REAL, uint32_t> coder(bound_bv);
+ morton_functor<CoalScalar, uint32_t> 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<BV>::init_2(std::vector<Node*>& 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<FCL_REAL, uint32_t> coder(bound_bv);
+ morton_functor<CoalScalar, uint32_t> 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<BV>::init_3(std::vector<Node*>& 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<FCL_REAL, uint32_t> coder(bound_bv);
+ morton_functor<CoalScalar, uint32_t> 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 <typename BV>
size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
const NodeBase<BV>& node2) {
- return SelectImpl<FCL_REAL, BV>::run(query, node1, node2);
+ return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
}
//==============================================================================
template <typename BV>
size_t select(const BV& query, const NodeBase<BV>& node1,
const NodeBase<BV>& node2) {
- return SelectImpl<FCL_REAL, BV>::run(query, node1, node2);
+ return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
}
//==============================================================================
@@ -973,8 +973,8 @@ struct SelectImpl<S, AABB> {
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<S, AABB> {
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 8eb2f46d..67a6b7ec 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 7fafa128..9ac67f89 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<BV>::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<FCL_REAL, uint32_t> coder(bound_bv);
+ morton_functor<CoalScalar, uint32_t> 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<BV>::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<FCL_REAL, uint32_t> coder(bound_bv);
+ morton_functor<CoalScalar, uint32_t> 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<BV>::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<FCL_REAL, uint32_t> coder(bound_bv);
+ morton_functor<CoalScalar, uint32_t> 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<BV>::update(size_t leaf, const BV& bv) {
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::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<BV>::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<FCL_REAL>::max)();
+ CoalScalar min_size = (std::numeric_limits<CoalScalar>::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<BV>::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<BV>::topdown_1(size_t* lbeg, size_t* lend) {
split_p += nodes[*i].bv.center();
vol += nodes[*i].bv;
}
- split_p /= static_cast<FCL_REAL>(num_leaves);
+ split_p /= static_cast<CoalScalar>(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<BV>::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 <typename BV>
size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) {
- return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes);
+ return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
}
//==============================================================================
template <typename BV>
size_t select(const BV& query, size_t node1, size_t node2,
NodeBase<BV>* nodes) {
- return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes);
+ return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
}
//==============================================================================
@@ -952,8 +952,8 @@ struct SelectImpl<S, AABB> {
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<S, AABB> {
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 23e2c2bc..a7edf618 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 b17c8b23..e2f8485c 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<SimpleInterval*> query(FCL_REAL low, FCL_REAL high);
+ std::deque<SimpleInterval*> 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 b053a2f9..01c5de4b 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 549e7df0..f2e8073b 100644
--- a/include/coal/broadphase/detail/morton.h
+++ b/include/coal/broadphase/detail/morton.h
@@ -80,7 +80,7 @@ struct morton_functor<S, uint32_t> {
};
using morton_functoru32f = morton_functor<float, uint32_t>;
-using morton_functoru32d = morton_functor<FCL_REAL, uint32_t>;
+using morton_functoru32d = morton_functor<CoalScalar, uint32_t>;
/// @brief Functor to compute 60 bit morton code for a given AABB
template <typename S>
diff --git a/include/coal/broadphase/detail/simple_interval.h b/include/coal/broadphase/detail/simple_interval.h
index 7b654048..efd9cbe5 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 499aee84..cb9d6bd9 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 a5301a79..6d8646e3 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<unsigned int> 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 863372d5..488211f6 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<FCL_REAL>::max)();
+ penetration_depth = (std::numeric_limits<CoalScalar>::max)();
nearest_points[0] = nearest_points[1] = normal = pos =
- Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ penetration_depth = (std::numeric_limits<CoalScalar>::max)();
nearest_points[0] = nearest_points[1] = normal = pos =
- Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::dummy_precision()) {}
+ Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::max)()) {}
+ distance_upper_bound((std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)()) {}
+ distance_upper_bound((std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)()) {
+ : distance_lower_bound((std::numeric_limits<CoalScalar>::max)()) {
nearest_points[0] = nearest_points[1] = normal =
- Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ distance_lower_bound = (std::numeric_limits<CoalScalar>::max)();
contacts.clear();
timings.clear();
nearest_points[0] = nearest_points[1] = normal =
- Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::dummy_precision()) const {
+ const CoalScalar tol =
+ Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::dummy_precision();
+ this->m_patch_tolerance = Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::max)())
+ CoalScalar min_distance_ = (std::numeric_limits<CoalScalar>::max)())
: min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
const Vec3f nan(
- Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::quiet_NaN()));
- min_distance = (std::numeric_limits<FCL_REAL>::max)();
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+ min_distance = (std::numeric_limits<CoalScalar>::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 5e7900ff..1088ed62 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<FCL_REAL>::max)())),
+ : aabb_center(Vec3f::Constant((std::numeric_limits<CoalScalar>::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 2d230c14..09ae43c1 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 9eecdd07..f730b79d 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<FCL_REAL>::dummy_precision();
+ const CoalScalar eps = Eigen::NumTraits<CoalScalar>::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<double>::epsilon()) {
return d;
}
- const FCL_REAL nominator = n.dot(a - d);
- FCL_REAL alpha = nominator / denominator;
- alpha = std::min<double>(1.0, std::max<FCL_REAL>(0.0, alpha));
+ const CoalScalar nominator = n.dot(a - d);
+ CoalScalar alpha = nominator / denominator;
+ alpha = std::min<double>(1.0, std::max<CoalScalar>(0.0, alpha));
return alpha * c + (1 - alpha) * d;
}
diff --git a/include/coal/data_types.h b/include/coal/data_types.h
index 0f69930e..b9b64bb7 100644
--- a/include/coal/data_types.h
+++ b/include/coal/data_types.h
@@ -67,9 +67,19 @@ typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf;
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3, Eigen::RowMajor> Matrixx3f;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 2, Eigen::RowMajor> Matrixx2f;
+typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
+
+typedef double CoalScalar;
+typedef Eigen::Matrix<CoalScalar, 3, 1> Vec3s;
+typedef Eigen::Matrix<CoalScalar, 2, 1> Vec2s;
+typedef Eigen::Matrix<CoalScalar, 6, 1> Vec6s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 1> VecXs;
+typedef Eigen::Matrix<CoalScalar, 3, 3> Matrix3s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3, Eigen::RowMajor> Matrixx3s;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 2, Eigen::RowMajor> Matrixx2s;
typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor>
Matrixx3i;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, Eigen::Dynamic> 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 bf4beef7..21da6487 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 d6bd5b7e..febcc663 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 0e3269bf..4204efc2 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<FCL_REAL>::lowest()),
+ max_height(std::numeric_limits<CoalScalar>::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<FCL_REAL>::min)()),
- max_height((std::numeric_limits<FCL_REAL>::max)()) {}
+ min_height((std::numeric_limits<CoalScalar>::min)()),
+ max_height((std::numeric_limits<CoalScalar>::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<BV>* 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>& 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>& 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 01fabf1c..6106a203 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<FCL_REAL> proj((size_t)num_primitives);
+ std::vector<CoalScalar> 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 0353df05..acbf532f 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 64b11d3b..07a4a97e 100644
--- a/include/coal/internal/shape_shape_func.h
+++ b/include/coal/internal/shape_shape_func.h
@@ -49,18 +49,20 @@ namespace coal {
template <typename ShapeType1, typename ShapeType2>
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<ShapeType1, ShapeType2>::run(
- o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
- normal);
+ const CoalScalar distance =
+ ShapeShapeDistancer<ShapeType1, ShapeType2>::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<const ShapeType1*>(o1);
const ShapeType2* obj2 = static_cast<const ShapeType2*>(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 <typename ShapeType1, typename ShapeType2>
-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<ShapeType1, ShapeType2>::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 <typename ShapeType1, typename ShapeType2>
-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<ShapeType1, ShapeType2>::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<ShapeType1, ShapeType2>(
+ CoalScalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
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<T1, T2>( \
+ COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T1, T2>( \
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<T2, T1>( \
+ COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T2, T1>( \
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<T1, T2>( \
+ inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T1, T2>( \
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<T2, T1>( \
+ inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T2, T1>( \
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<T, T>( \
+ COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<T, T>( \
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<T, T>( \
+ inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T, T>( \
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 02cf695e..ab26633d 100644
--- a/include/coal/internal/tools.h
+++ b/include/coal/internal/tools.h
@@ -203,8 +203,8 @@ void eigen(const Eigen::MatrixBase<Derived>& m,
template <typename Derived, typename OtherDerived>
bool isEqual(const Eigen::MatrixBase<Derived>& lhs,
const Eigen::MatrixBase<OtherDerived>& rhs,
- const FCL_REAL tol = std::numeric_limits<FCL_REAL>::epsilon() *
- 100) {
+ const CoalScalar tol = std::numeric_limits<CoalScalar>::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 3dc33f32..238dfc1a 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<FCL_REAL>::max)();
+ virtual CoalScalar BVDistanceLowerBound(unsigned int /*b1*/,
+ unsigned int /*b2*/) const {
+ return (std::numeric_limits<CoalScalar>::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 c9c1c659..ad2d203d 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<BV1>& 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<OBBRSS, 0>
namespace details {
template <typename BV>
struct DistanceTraversalBVDistanceLowerBound_impl {
- static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
+ static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
return b1.distance(b2);
}
- static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
- const BVNode<BV>& b2) {
+ static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
+ const BVNode<BV>& b2) {
return distance(R, T, b1.bv, b2.bv);
}
};
template <>
struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
- static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const BVNode<OBB>& b1, const BVNode<OBB>& 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<OBB> {
}
return sqrt(sqrDistLowerBound);
}
- static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
- const BVNode<OBB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ const BVNode<OBB>& b1, const BVNode<OBB>& 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<OBB> {
template <>
struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
- static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const BVNode<AABB>& b1, const BVNode<AABB>& 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<AABB> {
}
return sqrt(sqrDistLowerBound);
}
- static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
- const BVNode<AABB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ const BVNode<AABB>& b1, const BVNode<AABB>& 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<BV> {
}
/// @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<BV>::run(
@@ -450,21 +450,21 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
// 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<BV> {
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<!bool(RTIsIdentity)> RT;
@@ -501,7 +501,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
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 3b5d09ce..ec68e528 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<BV>& 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<TriangleP, S>(
+ const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
&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<TriangleP, S>(
+ const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
&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<TriangleP, S>(
+ const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
&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 2f6fd1c9..99a442f3 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<BV> {
/// @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<BV> {
/// 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<BV>& node1 = this->model1->getBV(b1);
@@ -210,11 +210,11 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
this->request.enable_contact || (this->request.security_margin < 0);
Vec3f p1, p2, normal;
DistanceResult distanceResult;
- FCL_REAL distance = internal::ShapeShapeDistance<TriangleP, TriangleP>(
+ CoalScalar distance = internal::ShapeShapeDistance<TriangleP, TriangleP>(
&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<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
namespace details {
template <typename BV>
struct DistanceTraversalBVDistanceLowerBound_impl {
- static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
+ static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
return b1.distance(b2);
}
- static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
- const BVNode<BV>& b2) {
+ static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
+ const BVNode<BV>& b2) {
return distance(R, T, b1.bv, b2.bv);
}
};
template <>
struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
- static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const BVNode<OBB>& b1, const BVNode<OBB>& 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<OBB> {
}
return sqrt(sqrDistLowerBound);
}
- static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
- const BVNode<OBB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ const BVNode<OBB>& b1, const BVNode<OBB>& 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<OBB> {
template <>
struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
- static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const BVNode<AABB>& b1, const BVNode<AABB>& 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<AABB> {
}
return sqrt(sqrDistLowerBound);
}
- static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
- const BVNode<AABB>& b2) {
- FCL_REAL sqrDistLowerBound;
+ static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ const BVNode<AABB>& b1, const BVNode<AABB>& 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<BV> {
}
/// @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<BV>::run(
@@ -452,21 +452,21 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
// 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<BV> {
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<!bool(RTIsIdentity)> RT;
@@ -503,7 +503,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
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 e15c00e1..1b5f6b1e 100644
--- a/include/coal/internal/traversal_node_hfield_shape.h
+++ b/include/coal/internal/traversal_node_hfield_shape.h
@@ -63,10 +63,10 @@ Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& 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<const MatrixXf, 2, 2> cell =
heights.block<2, 2>(node.y_id, node.x_id);
@@ -126,11 +126,11 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& 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<const MatrixXf, 2, 2> 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<Vec3f>& 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<Triangle>& convex,
- size_t& closest_face_id) {
+inline CoalScalar distanceContactPointToFace(const size_t face_id,
+ const Vec3f& contact_point,
+ const Convex<Triangle>& convex,
+ size_t& closest_face_id) {
assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]");
const std::vector<Vec3f>& 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 <typename Polygone, typename Shape>
bool binCorrection(const Convex<Polygone>& 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<Vec3f>& points = *(convex.points);
bool hfield_witness_is_on_bin_side = true;
@@ -341,11 +341,12 @@ bool binCorrection(const Convex<Polygone>& convex,
if (convex_active_faces & 8) active_faces.push_back(6);
Triangle face_triangle;
- FCL_REAL shortest_distance_to_face = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar shortest_distance_to_face =
+ (std::numeric_limits<CoalScalar>::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<Polygone>& 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<Polygone>& 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<Convex<Polygone>, Shape>(
@@ -515,7 +516,7 @@ class HeightFieldShapeCollisionTraversalNode
: public CollisionTraversalNodeBase {
public:
typedef CollisionTraversalNodeBase Base;
- typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;
+ typedef Eigen::Array<CoalScalar, 1, 2> 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<BV>& 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<ConvexQuadrilateral, S>(
&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 33bd2e1f..29cefe59 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<BV>* 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<Box, S>(
+ const CoalScalar distance = internal::ShapeShapeDistance<Box, S>(
&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, TriangleP>(
- &box, box_tf, &tri, tf2, this->solver,
- this->drequest->enable_signed_distance, p1, p2, normal);
+ const CoalScalar distance =
+ internal::ShapeShapeDistance<Box, TriangleP>(
+ &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<Box, TriangleP>(
+ const CoalScalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
&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<BV>* 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<Triangle, Box, 0>(
@@ -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<BV>* 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<Triangle, Box, 0>(
@@ -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<Box, Box>(
+ const CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
&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<Box, Box>(
+ CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
&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 b34eaad5..a10d8b71 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<S1, S2>(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 ad28bcd2..c0626b51 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 0795da1f..3bfcd0c3 100644
--- a/include/coal/math/transform.h
+++ b/include/coal/math/transform.h
@@ -43,8 +43,8 @@
namespace coal {
-COAL_DEPRECATED typedef Eigen::Quaternion<FCL_REAL> Quaternion3f;
-typedef Eigen::Quaternion<FCL_REAL> Quatf;
+COAL_DEPRECATED typedef Eigen::Quaternion<CoalScalar> Quaternion3f;
+typedef Eigen::Quaternion<CoalScalar> 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<FCL_REAL>::dummy_precision()) const {
+ const CoalScalar& prec =
+ Eigen::NumTraits<CoalScalar>::dummy_precision()) const {
return R.isIdentity(prec) && T.isZero(prec);
}
@@ -218,26 +218,26 @@ class COAL_DLLAPI Transform3f {
template <typename Derived>
inline Quatf fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
- FCL_REAL angle) {
- return Quatf(Eigen::AngleAxis<FCL_REAL>(angle, axis));
+ CoalScalar angle) {
+ return Quatf(Eigen::AngleAxis<CoalScalar>(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<FCL_REAL>(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<CoalScalar>(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 f72292b2..ae92c05e 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<SimplexVertex> sv_store;
std::vector<SimplexFace> 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 677373e5..c0af9424 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<FCL_REAL, 1, 2> Array2d;
+ typedef Eigen::Array<CoalScalar, 1, 2> 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 90bc04b1..261917cc 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<FCL_REAL>::max)()),
+ distance_upper_bound((std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ this->distance_upper_bound = (std::numeric_limits<CoalScalar>::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 <typename S1, typename S2>
- 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 <typename S1>
- 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 <typename S2>
- 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<S2>(
+ 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>(
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<FCL_REAL>::max)();
+ distance = -(std::numeric_limits<CoalScalar>::max)();
p1 = p2 = normal =
- Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ distance = -(std::numeric_limits<CoalScalar>::max)();
p1 = p2 = normal =
- Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
}
};
diff --git a/include/coal/narrowphase/narrowphase_defaults.h b/include/coal/narrowphase/narrowphase_defaults.h
index 54ce7532..1fd2775a 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 5bc3facb..8dac64d4 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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 <int _SupportOptions = SupportOptions::NoSweptSphere>
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 c3635c64..7dee68df 100644
--- a/include/coal/octree.h
+++ b/include/coal/octree.h
@@ -54,16 +54,16 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
protected:
shared_ptr<const octomap::OcTree> 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<const octomap::OcTree>(
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<FCL_REAL>(), max_extent.cast<FCL_REAL>());
+ aabb_local =
+ AABB(min_extent.cast<CoalScalar>(), max_extent.cast<CoalScalar>());
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<uint8_t> tobytes() const {
typedef Eigen::Matrix<float, 3, 1> 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<uint8_t> 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<Vec3float>(&it.getCoordinate().x()).cast<FCL_REAL>();
+ Eigen::Map<Vec3float>(&it.getCoordinate().x()).cast<CoalScalar>();
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<FCL_REAL, Eigen::Dynamic, 3>& point_cloud,
- const FCL_REAL resolution);
+makeOctree(const Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3>& point_cloud,
+ const CoalScalar resolution);
} // namespace coal
diff --git a/include/coal/serialization/contact_patch.h b/include/coal/serialization/contact_patch.h
index cdad6047..aae049da 100644
--- a/include/coal/serialization/contact_patch.h
+++ b/include/coal/serialization/contact_patch.h
@@ -16,7 +16,7 @@ template <class Archive>
void serialize(Archive& ar, coal::ContactPatch& contact_patch,
const unsigned int /*version*/) {
using namespace coal;
- typedef Eigen::Matrix<FCL_REAL, 2, Eigen::Dynamic> PolygonPoints;
+ typedef Eigen::Matrix<CoalScalar, 2, Eigen::Dynamic> 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<PolygonPoints> points_map(
- reinterpret_cast<FCL_REAL*>(contact_patch.points().data()), 2,
+ reinterpret_cast<CoalScalar*>(contact_patch.points().data()), 2,
static_cast<Eigen::Index>(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 2aefe25f..52999cee 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<Vec3f>(convex_base.num_normals_and_offsets));
convex_base.offsets.reset(
- new std::vector<FCL_REAL>(convex_base.num_normals_and_offsets));
+ new std::vector<CoalScalar>(convex_base.num_normals_and_offsets));
}
}
@@ -73,23 +73,23 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base,
}
}
- typedef Eigen::Matrix<FCL_REAL, 3, Eigen::Dynamic> MatrixPoints;
+ typedef Eigen::Matrix<CoalScalar, 3, Eigen::Dynamic> MatrixPoints;
if (convex_base.num_points > 0) {
Eigen::Map<MatrixPoints> points_map(
- reinterpret_cast<FCL_REAL*>(convex_base.points->data()), 3,
+ reinterpret_cast<CoalScalar*>(convex_base.points->data()), 3,
convex_base.num_points);
ar& make_nvp("points", points_map);
}
- typedef Eigen::Matrix<FCL_REAL, 1, Eigen::Dynamic> VecOfReals;
+ typedef Eigen::Matrix<CoalScalar, 1, Eigen::Dynamic> VecOfReals;
if (convex_base.num_normals_and_offsets > 0) {
Eigen::Map<MatrixPoints> normals_map(
- reinterpret_cast<FCL_REAL*>(convex_base.normals->data()), 3,
+ reinterpret_cast<CoalScalar*>(convex_base.normals->data()), 3,
convex_base.num_normals_and_offsets);
ar& make_nvp("normals", normals_map);
Eigen::Map<VecOfReals> offsets_map(
- reinterpret_cast<FCL_REAL*>(convex_base.offsets->data()), 1,
+ reinterpret_cast<CoalScalar*>(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<int, 1, Eigen::Dynamic> VecOfInts;
if (num_warm_start_supports > 0) {
Eigen::Map<MatrixPoints> warm_start_support_points_map(
- reinterpret_cast<FCL_REAL*>(
+ reinterpret_cast<CoalScalar*>(
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 661bfa53..2a14614c 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<coal::CollisionGeometry>(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 2a024e05..14c6457f 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<coal::Vec3f, coal::kIOS::max_num_spheres> centers{};
- std::array<coal::FCL_REAL, coal::kIOS::max_num_spheres> radii;
+ std::array<coal::CoalScalar, coal::kIOS::max_num_spheres> 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<coal::Vec3f, coal::kIOS::max_num_spheres> centers;
- std::array<coal::FCL_REAL, coal::kIOS::max_num_spheres> radii;
+ std::array<coal::CoalScalar, coal::kIOS::max_num_spheres> 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 067160ad..916fdfda 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 e77a6a09..566af023 100644
--- a/include/coal/shape/details/convex.hxx
+++ b/include/coal/shape/details/convex.hxx
@@ -144,7 +144,7 @@ Vec3f Convex<PolygonT>::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<PolygonT>::computeCOM() const {
polygon[static_cast<index_type>((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<PolygonT>::computeCOM() const {
}
template <typename PolygonT>
-FCL_REAL Convex<PolygonT>::computeVolume() const {
+CoalScalar Convex<PolygonT>::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<PolygonT>::computeVolume() const {
polygon[static_cast<index_type>((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 0dd17626..48516cd7 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 <typename BV>
void generateBVHModel(BVHModel<BV>& 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<Vec3f> points(8);
std::vector<Triangle> tri_indices(12);
points[0] = Vec3f(a, -b, c);
@@ -94,18 +94,18 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
- FCL_REAL r = shape.radius;
- FCL_REAL phi, phid;
- const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+ CoalScalar r = shape.radius;
+ CoalScalar phi, phid;
+ const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
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 <typename BV>
void generateBVHModel(BVHModel<BV>& 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<BV>& model, const Cylinder& shape,
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
- FCL_REAL r = shape.radius;
- FCL_REAL h = shape.halfLength;
- FCL_REAL phi, phid;
- const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+ CoalScalar r = shape.radius;
+ CoalScalar h = shape.halfLength;
+ CoalScalar phi, phid;
+ const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
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 <typename BV>
void generateBVHModel(BVHModel<BV>& 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<FCL_REAL>();
+ const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
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<BV>& model, const Cone& shape,
std::vector<Vec3f> points;
std::vector<Triangle> 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<FCL_REAL>();
+ CoalScalar phi, phid;
+ const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
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<BV>& model, const Cone& shape,
template <typename BV>
void generateBVHModel(BVHModel<BV>& 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<FCL_REAL>();
+ const CoalScalar pi = boost::math::constants::pi<CoalScalar>();
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 9d6a62f2..d21d5c21 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<ShapeBase*, Transform3f> inflated(const FCL_REAL value) const {
+ // std::pair<ShapeBase*, Transform3f> 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<FCL_REAL>::max)(); // TODO(jcarpent):
+ // return (std::numeric_limits<CoalScalar>::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<Box, Transform3f> inflated(const FCL_REAL value) const {
+ std::pair<Box, Transform3f> 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<FCL_REAL>() * radius * radius *
+ CoalScalar computeVolume() const {
+ return 4 * boost::math::constants::pi<CoalScalar>() * 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<Sphere, Transform3f> inflated(const FCL_REAL value) const {
+ std::pair<Sphere, Transform3f> 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<FCL_REAL>() * radii[0] * radii[1] *
+ CoalScalar computeVolume() const {
+ return 4 * boost::math::constants::pi<CoalScalar>() * 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<Ellipsoid, Transform3f> inflated(const FCL_REAL value) const {
+ std::pair<Ellipsoid, Transform3f> 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<FCL_REAL>() * radius * radius *
+ CoalScalar computeVolume() const {
+ return boost::math::constants::pi<CoalScalar>() * 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>();
- FCL_REAL v_sph = radius * radius * radius *
- boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
+ CoalScalar v_cyl = radius * radius * (halfLength * 2) *
+ boost::math::constants::pi<CoalScalar>();
+ CoalScalar v_sph = radius * radius * radius *
+ boost::math::constants::pi<CoalScalar>() * 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<Capsule, Transform3f> inflated(const FCL_REAL value) const {
+ std::pair<Capsule, Transform3f> 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<FCL_REAL>() * radius * radius *
+ CoalScalar computeVolume() const {
+ return boost::math::constants::pi<CoalScalar>() * 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<Cone, Transform3f> inflated(const FCL_REAL value) const {
+ std::pair<Cone, Transform3f> 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<FCL_REAL>() * radius * radius *
+ CoalScalar computeVolume() const {
+ return boost::math::constants::pi<CoalScalar>() * 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<Cylinder, Transform3f> inflated(const FCL_REAL value) const {
+ std::pair<Cylinder, Transform3f> 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<FCL_REAL>::lowest();
+ CoalScalar minInflationValue() const {
+ return std::numeric_limits<CoalScalar>::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<Halfspace, Transform3f> inflated(const FCL_REAL value) const {
+ std::pair<Halfspace, Transform3f> 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 788fb73e..583e611c 100644
--- a/python/broadphase/broadphase.cc
+++ b/python/broadphase/broadphase.cc
@@ -135,7 +135,7 @@ void exposeBroadPhase() {
typedef SpatialHashingCollisionManager<HashTable> Derived;
bp::class_<Derived, bp::bases<BroadPhaseCollisionManager>>(
"SpatialHashingCollisionManager", bp::no_init)
- .def(dv::init<Derived, FCL_REAL, const Vec3f &, const Vec3f &,
+ .def(dv::init<Derived, CoalScalar, const Vec3f &, const Vec3f &,
bp::optional<unsigned int>>());
}
}
diff --git a/python/broadphase/broadphase_callbacks.hh b/python/broadphase/broadphase_callbacks.hh
index 03790438..b0ae02d3 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 32aa74d5..2adacda2 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<Geometry>(), no_init)
.def(dv::init<HeightField<BV>>())
.def(dv::init<HeightField<BV>, const HeightField<BV>&>())
- .def(dv::init<HeightField<BV>, FCL_REAL, FCL_REAL, const MatrixXf&,
- bp::optional<FCL_REAL>>())
+ .def(dv::init<HeightField<BV>, CoalScalar, CoalScalar, const MatrixXf&,
+ bp::optional<CoalScalar>>())
.DEF_CLASS_FUNC(Geometry, getXDim)
.DEF_CLASS_FUNC(Geometry, getYDim)
@@ -278,7 +278,7 @@ void exposeShapes() {
"Box", doxygen::class_doc<ShapeBase>(), no_init)
.def(dv::init<Box>())
.def(dv::init<Box, const Box&>())
- .def(dv::init<Box, FCL_REAL, FCL_REAL, FCL_REAL>())
+ .def(dv::init<Box, CoalScalar, CoalScalar, CoalScalar>())
.def(dv::init<Box, const Vec3f&>())
.DEF_RW_CLASS_ATTRIB(Box, halfSide)
.def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone),
@@ -289,7 +289,7 @@ void exposeShapes() {
class_<Capsule, bases<ShapeBase>, shared_ptr<Capsule>>(
"Capsule", doxygen::class_doc<Capsule>(), no_init)
.def(dv::init<Capsule>())
- .def(dv::init<Capsule, FCL_REAL, FCL_REAL>())
+ .def(dv::init<Capsule, CoalScalar, CoalScalar>())
.def(dv::init<Capsule, const Capsule&>())
.DEF_RW_CLASS_ATTRIB(Capsule, radius)
.DEF_RW_CLASS_ATTRIB(Capsule, halfLength)
@@ -301,7 +301,7 @@ void exposeShapes() {
class_<Cone, bases<ShapeBase>, shared_ptr<Cone>>(
"Cone", doxygen::class_doc<Cone>(), no_init)
.def(dv::init<Cone>())
- .def(dv::init<Cone, FCL_REAL, FCL_REAL>())
+ .def(dv::init<Cone, CoalScalar, CoalScalar>())
.def(dv::init<Cone, const Cone&>())
.DEF_RW_CLASS_ATTRIB(Cone, radius)
.DEF_RW_CLASS_ATTRIB(Cone, halfLength)
@@ -360,7 +360,7 @@ void exposeShapes() {
class_<Cylinder, bases<ShapeBase>, shared_ptr<Cylinder>>(
"Cylinder", doxygen::class_doc<Cylinder>(), no_init)
.def(dv::init<Cylinder>())
- .def(dv::init<Cylinder, FCL_REAL, FCL_REAL>())
+ .def(dv::init<Cylinder, CoalScalar, CoalScalar>())
.def(dv::init<Cylinder, const Cylinder&>())
.DEF_RW_CLASS_ATTRIB(Cylinder, radius)
.DEF_RW_CLASS_ATTRIB(Cylinder, halfLength)
@@ -372,9 +372,10 @@ void exposeShapes() {
class_<Halfspace, bases<ShapeBase>, shared_ptr<Halfspace>>(
"Halfspace", doxygen::class_doc<Halfspace>(), no_init)
- .def(dv::init<Halfspace, const Vec3f&, FCL_REAL>())
+ .def(dv::init<Halfspace, const Vec3f&, CoalScalar>())
.def(dv::init<Halfspace, const Halfspace&>())
- .def(dv::init<Halfspace, FCL_REAL, FCL_REAL, FCL_REAL, FCL_REAL>())
+ .def(
+ dv::init<Halfspace, CoalScalar, CoalScalar, CoalScalar, CoalScalar>())
.def(dv::init<Halfspace>())
.DEF_RW_CLASS_ATTRIB(Halfspace, n)
.DEF_RW_CLASS_ATTRIB(Halfspace, d)
@@ -386,9 +387,9 @@ void exposeShapes() {
class_<Plane, bases<ShapeBase>, shared_ptr<Plane>>(
"Plane", doxygen::class_doc<Plane>(), no_init)
- .def(dv::init<Plane, const Vec3f&, FCL_REAL>())
+ .def(dv::init<Plane, const Vec3f&, CoalScalar>())
.def(dv::init<Plane, const Plane&>())
- .def(dv::init<Plane, FCL_REAL, FCL_REAL, FCL_REAL, FCL_REAL>())
+ .def(dv::init<Plane, CoalScalar, CoalScalar, CoalScalar, CoalScalar>())
.def(dv::init<Plane>())
.DEF_RW_CLASS_ATTRIB(Plane, n)
.DEF_RW_CLASS_ATTRIB(Plane, d)
@@ -401,7 +402,7 @@ void exposeShapes() {
"Sphere", doxygen::class_doc<Sphere>(), no_init)
.def(dv::init<Sphere>())
.def(dv::init<Sphere, const Sphere&>())
- .def(dv::init<Sphere, FCL_REAL>())
+ .def(dv::init<Sphere, CoalScalar>())
.DEF_RW_CLASS_ATTRIB(Sphere, radius)
.def("clone", &Sphere::clone, doxygen::member_func_doc(&Sphere::clone),
return_value_policy<manage_new_object>())
@@ -411,7 +412,7 @@ void exposeShapes() {
class_<Ellipsoid, bases<ShapeBase>, shared_ptr<Ellipsoid>>(
"Ellipsoid", doxygen::class_doc<Ellipsoid>(), no_init)
.def(dv::init<Ellipsoid>())
- .def(dv::init<Ellipsoid, FCL_REAL, FCL_REAL, FCL_REAL>())
+ .def(dv::init<Ellipsoid, CoalScalar, CoalScalar, CoalScalar>())
.def(dv::init<Ellipsoid, Vec3f>())
.def(dv::init<Ellipsoid, const Ellipsoid&>())
.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& (AABB::*)(const AABB&, FCL_REAL)>(&AABB::expand),
+ static_cast<AABB& (AABB::*)(const AABB&, CoalScalar)>(&AABB::expand),
// doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
- // AABB &, FCL_REAL)>(&AABB::expand)),
+ // AABB &, CoalScalar)>(&AABB::expand)),
// doxygen::member_func_args(static_cast<AABB&
- // (AABB::*)(const AABB &, FCL_REAL)>(&AABB::expand)),
+ // (AABB::*)(const AABB &, CoalScalar)>(&AABB::expand)),
bp::return_internal_reference<>())
.def("expand",
- static_cast<AABB& (AABB::*)(const FCL_REAL)>(&AABB::expand),
+ static_cast<AABB& (AABB::*)(const CoalScalar)>(&AABB::expand),
// doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
- // FCL_REAL)>(&AABB::expand)),
+ // CoalScalar)>(&AABB::expand)),
// doxygen::member_func_args(static_cast<AABB&
- // (AABB::*)(const FCL_REAL)>(&AABB::expand)),
+ // (AABB::*)(const CoalScalar)>(&AABB::expand)),
bp::return_internal_reference<>())
.def("expand", static_cast<AABB& (AABB::*)(const Vec3f&)>(&AABB::expand),
// doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
diff --git a/python/collision.cc b/python/collision.cc
index 71b29268..dac1c2db 100644
--- a/python/collision.cc
+++ b/python/collision.cc
@@ -183,7 +183,7 @@ void exposeCollisionAPI() {
const CollisionGeometry*, int, int>())
.def(dv::init<Contact, const CollisionGeometry*,
const CollisionGeometry*, int, int, const Vec3f&,
- const Vec3f&, FCL_REAL>())
+ const Vec3f&, CoalScalar>())
.add_property(
"o1",
make_function(&geto<1>,
diff --git a/python/contact_patch.cc b/python/contact_patch.cc
index e7972112..4721e875 100644
--- a/python/contact_patch.cc
+++ b/python/contact_patch.cc
@@ -93,12 +93,12 @@ void exposeContactPatchAPI() {
ContactPatchRequest>()) {
class_<ContactPatchRequest>(
"ContactPatchRequest", doxygen::class_doc<ContactPatchRequest>(),
- init<optional<size_t, size_t, FCL_REAL>>(
+ init<optional<size_t, size_t, CoalScalar>>(
(arg("self"), arg("max_num_patch"),
arg("num_samples_curved_shapes"), arg("patch_tolerance")),
"ContactPatchRequest constructor."))
.def(dv::init<ContactPatchRequest, const CollisionRequest&,
- bp::optional<size_t, FCL_REAL>>())
+ bp::optional<size_t, CoalScalar>>())
.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 e6080a8b..0e0032d9 100644
--- a/python/distance.cc
+++ b/python/distance.cc
@@ -73,7 +73,7 @@ void exposeDistanceAPI() {
if (!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>()) {
class_<DistanceRequest, bases<QueryRequest> >(
"DistanceRequest", doxygen::class_doc<DistanceRequest>(),
- init<optional<bool, FCL_REAL, FCL_REAL> >(
+ init<optional<bool, CoalScalar, CoalScalar> >(
(arg("self"), arg("enable_nearest_points"), arg("rel_err"),
arg("abs_err")),
"Constructor"))
@@ -149,14 +149,14 @@ void exposeDistanceAPI() {
doxygen::def(
"distance",
- static_cast<FCL_REAL (*)(const CollisionObject*, const CollisionObject*,
- const DistanceRequest&, DistanceResult&)>(
+ static_cast<CoalScalar (*)(const CollisionObject*, const CollisionObject*,
+ const DistanceRequest&, DistanceResult&)>(
&distance));
doxygen::def(
"distance",
- static_cast<FCL_REAL (*)(const CollisionGeometry*, const Transform3f&,
- const CollisionGeometry*, const Transform3f&,
- const DistanceRequest&, DistanceResult&)>(
+ static_cast<CoalScalar (*)(const CollisionGeometry*, const Transform3f&,
+ const CollisionGeometry*, const Transform3f&,
+ const DistanceRequest&, DistanceResult&)>(
&distance));
class_<ComputeDistance>("ComputeDistance",
@@ -164,7 +164,7 @@ void exposeDistanceAPI() {
.def(dv::init<ComputeDistance, const CollisionGeometry*,
const CollisionGeometry*>())
.def("__call__",
- static_cast<FCL_REAL (ComputeDistance::*)(
+ static_cast<CoalScalar (ComputeDistance::*)(
const Transform3f&, const Transform3f&, const DistanceRequest&,
DistanceResult&) const>(&ComputeDistance::operator()));
}
diff --git a/python/gjk.cc b/python/gjk.cc
index c5dbb0d1..9f705fb9 100644
--- a/python/gjk.cc
+++ b/python/gjk.cc
@@ -178,7 +178,7 @@ void exposeGJK() {
if (!eigenpy::register_symbolic_link_to_registered_type<GJK>()) {
class_<GJK>("GJK", doxygen::class_doc<GJK>(), no_init)
- .def(doxygen::visitor::init<GJK, unsigned int, FCL_REAL>())
+ .def(doxygen::visitor::init<GJK, unsigned int, CoalScalar>())
.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 1622eb34..85c365d4 100644
--- a/python/math.cc
+++ b/python/math.cc
@@ -95,7 +95,7 @@ void exposeMaths() {
return_value_policy<copy_const_reference>())
.def("isIdentity", &Transform3f::isIdentity,
(bp::arg("self"),
- bp::arg("prec") = Eigen::NumTraits<FCL_REAL>::dummy_precision()),
+ bp::arg("prec") = Eigen::NumTraits<CoalScalar>::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 c18c7d64..a569bba5 100644
--- a/python/octree.cc
+++ b/python/octree.cc
@@ -35,7 +35,7 @@ void exposeOctree() {
bp::class_<OcTree, bp::bases<CollisionGeometry>, shared_ptr<OcTree> >(
"OcTree", doxygen::class_doc<OcTree>(), bp::no_init)
- .def(dv::init<OcTree, FCL_REAL>())
+ .def(dv::init<OcTree, CoalScalar>())
.def("clone", &OcTree::clone, doxygen::member_func_doc(&OcTree::clone),
bp::return_value_policy<bp::manage_new_object>())
.def(dv::member_func("getTreeDepth", &OcTree::getTreeDepth))
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index c7b427a7..de3d582d 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<FCL_REAL>::max)())),
- max_(Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)())) {}
+ : min_(Vec3f::Constant((std::numeric_limits<CoalScalar>::max)())),
+ max_(Vec3f::Constant(-(std::numeric_limits<CoalScalar>::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 e69b49ed..621b059c 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<FCL_REAL>::max)();
+ CoalScalar real_max = (std::numeric_limits<CoalScalar>::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 <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
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<FCL_REAL>::epsilon() &&
+ 10 * Eigen::NumTraits<CoalScalar>::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 051a9af7..907272a5 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 1738f8ab..15c3043a 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 80e94990..cc581a84 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 <short N>
-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 <short N>
KDOP<N>::KDOP() {
- FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
dist_.template head<N / 2>().setConstant(real_max);
dist_.template tail<N / 2>().setConstant(-real_max);
}
@@ -110,7 +111,7 @@ KDOP<N>::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<N>::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<N>::overlap(const KDOP<N>& other) const {
template <short N>
bool KDOP<N>::overlap(const KDOP<N>& 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<N / 2>() - other.dist_.template tail<N / 2>())
.minCoeff();
if (a > breakDistance) {
@@ -154,7 +155,7 @@ bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
return false;
}
- FCL_REAL b =
+ CoalScalar b =
(other.dist_.template head<N / 2>() - dist_.template tail<N / 2>())
.minCoeff();
if (b > breakDistance) {
@@ -172,7 +173,7 @@ bool KDOP<N>::inside(const Vec3f& p) const {
if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false;
enum { P = ((N - 6) / 2) };
- Eigen::Array<FCL_REAL, P, 1> d;
+ Eigen::Array<CoalScalar, P, 1> d;
getDistances<P>(p, d.data());
if ((d < dist_.template segment<P>(3)).any()) return false;
@@ -187,7 +188,7 @@ KDOP<N>& KDOP<N>::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<N> KDOP<N>::operator+(const KDOP<N>& other) const {
}
template <short N>
-FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/,
- Vec3f* /*Q*/) const {
+CoalScalar KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/,
+ Vec3f* /*Q*/) const {
std::cerr << "KDOP distance not implemented!" << std::endl;
return 0.0;
}
@@ -226,7 +227,7 @@ KDOP<N> translate(const KDOP<N>& 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 2b50f4b6..10e9d7fd 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 cd45cabf..807549e6 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 2072921b..9a919431 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -88,7 +88,7 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
const DistanceRequest distanceRequest(enable_nearest_points,
compute_penetration);
DistanceResult distanceResult;
- const FCL_REAL distance = ShapeShapeDistance<Box, TriangleP>(
+ const CoalScalar distance = ShapeShapeDistance<Box, TriangleP>(
&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<FCL_REAL>(radsqr - dz * dz, 0));
+ minx = P[minindex][0] + std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
dz = P[maxindex][2] - cz;
- maxx = P[maxindex][0] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+ maxx = P[maxindex][0] - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - dz * dz, 0));
+ x = P[i][0] + std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - dz * dz, 0));
+ x = P[i][0] - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - dz * dz, 0));
+ miny = P[minindex][1] + std::sqrt(std::max<CoalScalar>(radsqr - dz * dz, 0));
dz = P[maxindex][2] - cz;
- maxy = P[maxindex][1] - std::sqrt(std::max<FCL_REAL>(radsqr - dz * dz, 0));
+ maxy = P[maxindex][1] - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - dz * dz, 0));
+ y = P[i][1] + std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - dz * dz, 0));
+ y = P[i][1] - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - t, 0));
+ u = u - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - t, 0));
+ u = u - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - t, 0));
+ u = u - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(radsqr - t, 0));
+ u = u - std::sqrt(std::max<CoalScalar>(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<FCL_REAL>(maxx - minx, 0);
- l[1] = std::max<FCL_REAL>(maxy - miny, 0);
+ l[0] = std::max<CoalScalar>(maxx - minx, 0);
+ l[1] = std::max<CoalScalar>(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<FCL_REAL>::max)();
+ CoalScalar real_max = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar real_max = (std::numeric_limits<CoalScalar>::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 61006a38..deb74700 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<OBB>::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<OBBRSS>::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<OBBRSS>::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<RSS>::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<RSS>::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<kIOS>::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<kIOS>::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<kIOS>::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<kIOS>::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<kIOS>::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 1a78e332..48c28433 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -83,7 +83,7 @@ void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3f& split_vector) {
}
template <typename BV>
-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 <typename BV>
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<FCL_REAL> proj(num_primitives);
+ CoalScalar& split_value) {
+ std::vector<CoalScalar> 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 124f8c67..ca1ba324 100644
--- a/src/broadphase/broadphase_SSaP.cpp
+++ b/src/broadphase/broadphase_SSaP.cpp
@@ -183,7 +183,7 @@ bool SSaPCollisionManager::checkDis(
typename std::vector<CollisionObject*>::const_iterator pos_start,
typename std::vector<CollisionObject*>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)())
+ if (min_dist < (std::numeric_limits<CoalScalar>::max)())
dummy_vector += Vec3f(min_dist, min_dist, min_dist);
typename std::vector<CollisionObject*>::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<FCL_REAL>::max)())
+ if (old_min_distance < (std::numeric_limits<CoalScalar>::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<FCL_REAL>::epsilon() * 100))
+ std::numeric_limits<CoalScalar>::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<CollisionObject*>::const_iterator& it_beg,
typename std::vector<CollisionObject*>::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<CollisionObject*>::const_iterator it, it_end;
selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
- FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
typename std::vector<CollisionObject*>::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 61f039d5..9e0d64c7 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<FCL_REAL>(),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+ std::bind(std::less<CoalScalar>(),
+ std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_1, coord),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+ std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
&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<FCL_REAL>(),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+ std::bind(std::less<CoalScalar>(),
+ std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_1, axis),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+ std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
&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<FCL_REAL>::max)()) {
+ if (min_dist < (std::numeric_limits<CoalScalar>::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<FCL_REAL>(),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+ std::bind(std::less<CoalScalar>(),
+ std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
&EndPoint::getVal),
std::placeholders::_1, axis),
- std::bind(static_cast<FCL_REAL (EndPoint::*)(int) const>(
+ std::bind(static_cast<CoalScalar (EndPoint::*)(int) const>(
&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<FCL_REAL>::max)())
+ if (old_min_distance < (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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 f1a241d7..7d83629b 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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
for (typename std::list<CollisionObject*>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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 afb765e1..05ac3151 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<CollisionObject*>(root1->data);
CollisionObject* root2_obj = static_cast<CollisionObject*>(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<CollisionObject*>(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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback,
min_dist);
}
@@ -749,7 +749,7 @@ void DynamicAABBTreeCollisionManager::distance(
DynamicAABBTreeCollisionManager* other_manager =
static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
if ((size() == 0) || (other_manager->size() == 0)) return;
- FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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 d4f44f20..95f440c6 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<CollisionObject*>(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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<DynamicAABBTreeArrayCollisionManager*>(other_manager_);
if ((size() == 0) || (other_manager->size() == 0)) return;
- FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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 9107335d..8a431315 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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)()) {
+ if (min_dist < (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)())
+ if (old_min_distance < (std::numeric_limits<CoalScalar>::max)())
break;
else {
if (min_dist < old_min_distance) {
@@ -455,9 +455,9 @@ void IntervalTreeCollisionManager::collide(
std::set<CollisionObject*> active;
std::set<std::pair<CollisionObject*, CollisionObject*> > 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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_dist = (std::numeric_limits<CoalScalar>::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<detail::SimpleInterval*>::const_iterator pos_start,
typename std::deque<detail::SimpleInterval*>::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<SAPInterval*>(*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 222e77b8..8e54a47e 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<DistanceData*>(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 63fdd8d7..34a2326c 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<FCL_REAL>::max)();
+ -(std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ (std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ y->max_high = -(std::numeric_limits<CoalScalar>::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<SimpleInterval*> IntervalTree::query(FCL_REAL low, FCL_REAL high) {
+std::deque<SimpleInterval*> IntervalTree::query(CoalScalar low,
+ CoalScalar high) {
std::deque<SimpleInterval*> 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 ba45f0e9..dd92fb7c 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<unsigned int>(std::ceil(scene_limit.width() / cell_size));
diff --git a/src/collision.cpp b/src/collision.cpp
index 5b597f45..57b92304 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<FCL_REAL>::infinity()) {
+ if (request.security_margin == -std::numeric_limits<CoalScalar>::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<FCL_REAL>::infinity()) {
+ if (request.security_margin == -std::numeric_limits<CoalScalar>::infinity()) {
result.clear();
return false;
}
diff --git a/src/collision_node.cpp b/src/collision_node.cpp
index 93caa018..77c6adb0 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<FCL_REAL>::epsilon());
+ const CoalScalar dummy_precision =
+ std::sqrt(Eigen::NumTraits<CoalScalar>::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 541915bf..bf6418d5 100644
--- a/src/contact_patch/contact_patch_solver.cpp
+++ b/src/contact_patch/contact_patch_solver.cpp
@@ -46,7 +46,7 @@ template <typename ShapeType,
void getShapeSupportSetTpl(const ShapeBase* shape, SupportSet& support_set,
int& hint, ShapeSupportData& support_data,
size_t num_sampled_supports = 6,
- FCL_REAL tol = 1e-3) {
+ CoalScalar tol = 1e-3) {
const ShapeType* shape_ = static_cast<const ShapeType*>(shape);
getShapeSupportSet<_SupportOptions>(shape_, support_set, hint, support_data,
num_sampled_supports, tol);
diff --git a/src/distance.cpp b/src/distance.cpp
index a54efa3c..97dc1f5e 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<FCL_REAL>::max)();
+ CoalScalar res = (std::numeric_limits<CoalScalar>::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 39f8a792..f4dbc806 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<Box, Halfspace>(
+CoalScalar ShapeShapeDistance<Box, Halfspace>(
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<const Box&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, Box>(
+CoalScalar ShapeShapeDistance<Halfspace, Box>(
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 f0c45e3f..abce82fb 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<Box, Plane>(const CollisionGeometry* o1,
- const Transform3f& tf1,
- const CollisionGeometry* o2,
- const Transform3f& tf2,
- const GJKSolver*, const bool, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Box, Plane>(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<const Box&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, Box>(const CollisionGeometry* o1,
- const Transform3f& tf1,
- const CollisionGeometry* o2,
- const Transform3f& tf2,
- const GJKSolver*, const bool, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Box>(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<const Plane&>(*o1);
const Box& s2 = static_cast<const Box&>(*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 220c20eb..aafda9c1 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<Box, Sphere>(const CollisionGeometry* o1,
- const Transform3f& tf1,
- const CollisionGeometry* o2,
- const Transform3f& tf2,
- const GJKSolver*, const bool,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Box, Sphere>(
+ 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<const Box&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*o2);
return details::boxSphereDistance(s1, tf1, s2, tf2, p1, p2, normal);
}
template <>
-FCL_REAL ShapeShapeDistance<Sphere, Box>(const CollisionGeometry* o1,
- const Transform3f& tf1,
- const CollisionGeometry* o2,
- const Transform3f& tf2,
- const GJKSolver*, const bool,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Sphere, Box>(
+ 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<const Sphere&>(*o1);
const Box& s2 = static_cast<const Box&>(*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 c229f80e..b09a3522 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<Capsule, Capsule>(
+CoalScalar ShapeShapeDistance<Capsule, Capsule>(
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<const Capsule*>(o1);
const Capsule* capsule2 = static_cast<const Capsule*>(o2);
- FCL_REAL EPSILON = std::numeric_limits<FCL_REAL>::epsilon() * 100;
+ CoalScalar EPSILON = std::numeric_limits<CoalScalar>::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<Capsule, Capsule>(
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<Capsule, Capsule>(
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<Capsule, Capsule>(
}
// 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 15daa03d..3d794ba2 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<Capsule, Halfspace>(
+CoalScalar ShapeShapeDistance<Capsule, Halfspace>(
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<const Capsule&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, Capsule>(
+CoalScalar ShapeShapeDistance<Halfspace, Capsule>(
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 9644e43e..35c1b8ee 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<Capsule, Plane>(
+CoalScalar ShapeShapeDistance<Capsule, Plane>(
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<const Capsule&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, Capsule>(
+CoalScalar ShapeShapeDistance<Plane, Capsule>(
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 39649095..6a3430d1 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<Cone, Halfspace>(
+CoalScalar ShapeShapeDistance<Cone, Halfspace>(
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<const Cone&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, Cone>(
+CoalScalar ShapeShapeDistance<Halfspace, Cone>(
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 ddd863ff..350552fe 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<Cone, Plane>(const CollisionGeometry* o1,
- const Transform3f& tf1,
- const CollisionGeometry* o2,
- const Transform3f& tf2,
- const GJKSolver*, const bool,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Cone, Plane>(
+ 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<const Cone&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, Cone>(const CollisionGeometry* o1,
- const Transform3f& tf1,
- const CollisionGeometry* o2,
- const Transform3f& tf2,
- const GJKSolver*, const bool,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Cone>(
+ 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<const Plane&>(*o1);
const Cone& s2 = static_cast<const Cone&>(*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 22fc88ad..55acbfa7 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<ConvexBase, Halfspace>(
+CoalScalar ShapeShapeDistance<ConvexBase, Halfspace>(
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<const ConvexBase&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, ConvexBase>(
+CoalScalar ShapeShapeDistance<Halfspace, ConvexBase>(
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 e2ece56e..b0514489 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<ConvexBase, Plane>(
+CoalScalar ShapeShapeDistance<ConvexBase, Plane>(
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<const ConvexBase&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, ConvexBase>(
+CoalScalar ShapeShapeDistance<Plane, ConvexBase>(
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 17b59990..3c06d8c8 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<Cylinder, Halfspace>(
+CoalScalar ShapeShapeDistance<Cylinder, Halfspace>(
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<const Cylinder&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, Cylinder>(
+CoalScalar ShapeShapeDistance<Halfspace, Cylinder>(
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 fd1e31b7..125d3d4e 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<Cylinder, Plane>(
+CoalScalar ShapeShapeDistance<Cylinder, Plane>(
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<const Cylinder&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, Cylinder>(
+CoalScalar ShapeShapeDistance<Plane, Cylinder>(
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 836eb493..5dc3e8b7 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<Ellipsoid, Halfspace>(
+CoalScalar ShapeShapeDistance<Ellipsoid, Halfspace>(
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<const Ellipsoid&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, Ellipsoid>(
+CoalScalar ShapeShapeDistance<Halfspace, Ellipsoid>(
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 77d7cd2e..16947904 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<Ellipsoid, Plane>(
+CoalScalar ShapeShapeDistance<Ellipsoid, Plane>(
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<const Ellipsoid&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, Ellipsoid>(
+CoalScalar ShapeShapeDistance<Plane, Ellipsoid>(
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 260b12ec..07648d70 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<Halfspace, Halfspace>(
+CoalScalar ShapeShapeDistance<Halfspace, Halfspace>(
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 096b8ea4..abea962c 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<Halfspace, Plane>(
+CoalScalar ShapeShapeDistance<Halfspace, Plane>(
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<Halfspace, Plane>(
}
template <>
-FCL_REAL ShapeShapeDistance<Plane, Halfspace>(
+CoalScalar ShapeShapeDistance<Plane, Halfspace>(
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<const Plane&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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 a716999e..98b23b76 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<Plane, Plane>(const CollisionGeometry* o1,
- const Transform3f& tf1,
- const CollisionGeometry* o2,
- const Transform3f& tf2,
- const GJKSolver*, const bool,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+CoalScalar ShapeShapeDistance<Plane, Plane>(
+ 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<const Plane&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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 d0210117..2724700f 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<Sphere, Capsule>(
+CoalScalar ShapeShapeDistance<Sphere, Capsule>(
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<Sphere, Capsule>(
}
template <>
-FCL_REAL ShapeShapeDistance<Capsule, Sphere>(
+CoalScalar ShapeShapeDistance<Capsule, Sphere>(
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<const Capsule&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*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 c97bec1d..93281080 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<Sphere, Cylinder>(
+CoalScalar ShapeShapeDistance<Sphere, Cylinder>(
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<Sphere, Cylinder>(
}
template <>
-FCL_REAL ShapeShapeDistance<Cylinder, Sphere>(
+CoalScalar ShapeShapeDistance<Cylinder, Sphere>(
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<const Cylinder&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*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 3f3d580c..e0fdbc64 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<Sphere, Halfspace>(
+CoalScalar ShapeShapeDistance<Sphere, Halfspace>(
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<const Sphere&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, Sphere>(
+CoalScalar ShapeShapeDistance<Halfspace, Sphere>(
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 a3d5a262..c28df4d3 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<Sphere, Plane>(
+CoalScalar ShapeShapeDistance<Sphere, Plane>(
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<const Sphere&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, Sphere>(
+CoalScalar ShapeShapeDistance<Plane, Sphere>(
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 ea60685a..365f8af5 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<Sphere, Sphere>(
+CoalScalar ShapeShapeDistance<Sphere, Sphere>(
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 a74cdc8b..f4220b25 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<TriangleP, Halfspace>(
+CoalScalar ShapeShapeDistance<TriangleP, Halfspace>(
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<const TriangleP&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*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<Halfspace, TriangleP>(
+CoalScalar ShapeShapeDistance<Halfspace, TriangleP>(
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 89b60f7a..8e3ba7e2 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<TriangleP, Plane>(
+CoalScalar ShapeShapeDistance<TriangleP, Plane>(
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<const TriangleP&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*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<Plane, TriangleP>(
+CoalScalar ShapeShapeDistance<Plane, TriangleP>(
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 aed68996..8e7e1ce7 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<TriangleP, Sphere>(
+CoalScalar ShapeShapeDistance<TriangleP, Sphere>(
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<const TriangleP&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*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<Sphere, TriangleP>(
+CoalScalar ShapeShapeDistance<Sphere, TriangleP>(
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 602e9efb..ae737e6d 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<TriangleP, TriangleP>(
+CoalScalar ShapeShapeDistance<TriangleP, TriangleP>(
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<TriangleP, TriangleP>(
// 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 4f37096b..5b482f29 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -48,10 +48,10 @@ namespace coal {
#ifdef COAL_HAS_OCTOMAP
template <typename TypeA, typename TypeB>
-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<TypeA, TypeB>::CollisionTraversal_t node;
const TypeA* obj1 = static_cast<const TypeA*>(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 <typename T_BVH, typename T_SH>
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<T_BVH, T_SH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
@@ -108,13 +109,13 @@ namespace details {
template <typename OrientedMeshShapeDistanceTraversalNode, typename T_BVH,
typename T_SH>
-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<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
@@ -130,11 +131,12 @@ FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1,
template <typename T_SH>
struct COAL_LOCAL BVHShapeDistancer<RSS, T_SH> {
- 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<T_SH>, RSS, T_SH>(
o1, tf1, o2, tf2, nsolver, request, result);
@@ -143,11 +145,12 @@ struct COAL_LOCAL BVHShapeDistancer<RSS, T_SH> {
template <typename T_SH>
struct COAL_LOCAL BVHShapeDistancer<kIOS, T_SH> {
- 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<T_SH>, kIOS, T_SH>(
o1, tf1, o2, tf2, nsolver, request, result);
@@ -156,11 +159,12 @@ struct COAL_LOCAL BVHShapeDistancer<kIOS, T_SH> {
template <typename T_SH>
struct COAL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> {
- 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<T_SH>, OBBRSS, T_SH>(
o1, tf1, o2, tf2, nsolver, request, result);
@@ -169,11 +173,12 @@ struct COAL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> {
template <typename T_HF, typename T_SH>
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 <typename T_BVH>
-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<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
@@ -220,12 +225,12 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
namespace details {
template <typename OrientedMeshDistanceTraversalNode, typename T_BVH>
-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<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
@@ -240,39 +245,41 @@ FCL_REAL orientedMeshDistance(const CollisionGeometry* o1,
} // namespace details
template <>
-FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1,
- const CollisionGeometry* o2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result) {
+CoalScalar BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const DistanceRequest& request,
+ DistanceResult& result) {
return details::orientedMeshDistance<MeshDistanceTraversalNodeRSS, RSS>(
o1, tf1, o2, tf2, request, result);
}
template <>
-FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1,
- const CollisionGeometry* o2, const Transform3f& tf2,
- const DistanceRequest& request,
- DistanceResult& result) {
- return details::orientedMeshDistance<MeshDistanceTraversalNodekIOS, kIOS>(
- o1, tf1, o2, tf2, request, result);
-}
-
-template <>
-FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1,
+CoalScalar BVHDistance<kIOS>(const CollisionGeometry* o1,
const Transform3f& tf1,
const CollisionGeometry* o2,
const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result) {
+ return details::orientedMeshDistance<MeshDistanceTraversalNodekIOS, kIOS>(
+ o1, tf1, o2, tf2, request, result);
+}
+
+template <>
+CoalScalar BVHDistance<OBBRSS>(const CollisionGeometry* o1,
+ const Transform3f& tf1,
+ const CollisionGeometry* o2,
+ const Transform3f& tf2,
+ const DistanceRequest& request,
+ DistanceResult& result) {
return details::orientedMeshDistance<MeshDistanceTraversalNodeOBBRSS, OBBRSS>(
o1, tf1, o2, tf2, request, result);
}
template <typename T_BVH>
-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<T_BVH>(o1, tf1, o2, tf2, request, result);
}
diff --git a/src/intersect.cpp b/src/intersect.cpp
index 6feb74e1..feca0843 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 804e6c89..4cc1601e 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<FCL_REAL>::epsilon());
+ static const CoalScalar eps(std::numeric_limits<CoalScalar>::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<FCL_REAL>::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<CoalScalar>::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<FCL_REAL>::epsilon()) unit = c1c2 / cdist;
- FCL_REAL dist = cdist - r1 - r2;
+ if (cdist > Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::quiet_NaN()));
- FCL_REAL min_distance_sqr, distance_sqr;
+ Vec3f::Constant(std::numeric_limits<CoalScalar>::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<details::SupportOptions::WithSweptSphere>(&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<FCL_REAL>::dummy_precision());
+ const CoalScalar dummy_precision =
+ std::sqrt(Eigen::NumTraits<CoalScalar>::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<details::SupportOptions::WithSweptSphere>(&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<FCL_REAL>::dummy_precision());
+ const CoalScalar dummy_precision =
+ std::sqrt(Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::max)();
+ CoalScalar min_d = (std::numeric_limits<CoalScalar>::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<FCL_REAL>::epsilon()) // parallel
+ if (dir_sq_norm < std::numeric_limits<CoalScalar>::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<FCL_REAL>::max)();
+ distance = -(std::numeric_limits<CoalScalar>::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<FCL_REAL>::dummy_precision());
+ Eigen::NumTraits<CoalScalar>::dummy_precision());
} else {
normal = -new_s1.n;
p1 << new_s1.n * new_s1.d;
p2 = -(normal * distance);
assert(new_s1.distance(p1) <=
- Eigen::NumTraits<FCL_REAL>::dummy_precision());
+ Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::max)();
+ distance = -(std::numeric_limits<CoalScalar>::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<FCL_REAL>::epsilon()) // parallel
+ if (dir_sq_norm < std::numeric_limits<CoalScalar>::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<FCL_REAL>::dummy_precision());
+ Eigen::NumTraits<CoalScalar>::dummy_precision());
assert(new_s2.distance(p2) <=
- Eigen::NumTraits<FCL_REAL>::dummy_precision());
+ Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::max)();
+ distance = -(std::numeric_limits<CoalScalar>::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<FCL_REAL>::epsilon()) // parallel
+ if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon()) // parallel
{
p1 = new_s1.n * new_s1.d;
p2 = new_s2.n * new_s2.d;
assert(new_s1.distance(p1) <=
- Eigen::NumTraits<FCL_REAL>::dummy_precision());
+ Eigen::NumTraits<CoalScalar>::dummy_precision());
assert(new_s2.distance(p2) <=
- Eigen::NumTraits<FCL_REAL>::dummy_precision());
+ Eigen::NumTraits<CoalScalar>::dummy_precision());
distance = (p1 - p2).norm();
- if (distance > Eigen::NumTraits<FCL_REAL>::dummy_precision()) {
+ if (distance > Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::max)();
+ distance = -(std::numeric_limits<CoalScalar>::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 f6959bbd..e8093de6 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<FCL_REAL>::max)();
+ distance_upper_bound = (std::numeric_limits<CoalScalar>::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 <bool Separated>
void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0,
Vec3f& w1) {
#ifndef NDEBUG
- const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
+ const CoalScalar dummy_precision =
+ Eigen::NumTraits<CoalScalar>::dummy_precision();
assert((normal.norm() - 1) < dummy_precision);
#endif
- const Eigen::Array<FCL_REAL, 1, 2>& I(shape.swept_sphere_radius);
+ const Eigen::Array<CoalScalar, 1, 2>& I(shape.swept_sphere_radius);
Eigen::Array<bool, 1, 2> 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<FCL_REAL>::dummy_precision()) {
+ if ((w1 - w0).norm() > Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::epsilon()));
+ const CoalScalar dummy_precision(
+ 3 * std::sqrt(std::numeric_limits<CoalScalar>::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<FCL_REAL>::epsilon()) {
+ if (face->n.norm() > Eigen::NumTraits<CoalScalar>::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<FCL_REAL>::max();
+ face->d = std::numeric_limits<CoalScalar>::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<FCL_REAL>::max();
+ CoalScalar mind = std::numeric_limits<CoalScalar>::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<FCL_REAL>::epsilon()));
+ const CoalScalar dummy_precision(
+ 3 * std::sqrt(std::numeric_limits<CoalScalar>::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<FCL_REAL>::dummy_precision()) {
+ if ((w0 - w1).norm() > Eigen::NumTraits<CoalScalar>::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 52fbb09f..73307e12 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<FCL_REAL>::epsilon());
+ assert(dir.norm() > Eigen::NumTraits<CoalScalar>::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 <typename Shape0, int _SupportOptions>
MinkowskiDiff::GetSupportFunction makeGetSupportFunction1(
const ShapeBase* s1, bool identity,
- Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
+ Eigen::Array<CoalScalar, 1, 2>& 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 <int _SupportOptions>
MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
const ShapeBase* s0, const ShapeBase* s1, bool identity,
- Eigen::Array<FCL_REAL, 1, 2>& swept_sphere_radius,
+ Eigen::Array<CoalScalar, 1, 2>& 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 ef9edf2f..9c7563f4 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 <int _SupportOptions>
- inline void getShapeSupport(const Box* box, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData& /*unused*/) {
+// ============================================================================
+template <int _SupportOptions>
+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 <int _SupportOptions>
- inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData& /*unused*/) {
+// ============================================================================
+template <int _SupportOptions>
+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 <int _SupportOptions>
- inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData& /*unused*/) {
+// ============================================================================
+template <int _SupportOptions>
+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 <int _SupportOptions>
- inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData& /*unused*/) {
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir,
+ Vec3f& support, int& /*unused*/,
+ ShapeSupportData& /*unused*/) {
static const FCL_REAL dummy_precision =
Eigen::NumTraits<FCL_REAL>::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 <int _SupportOptions>
- void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
- int& /*unused*/, ShapeSupportData& /*unused*/) {
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
+ int& /*unused*/, ShapeSupportData& /*unused*/) {
static const FCL_REAL dummy_precision =
Eigen::NumTraits<FCL_REAL>::dummy_precision();
@@ -270,15 +259,12 @@ getShapeSupportTplInstantiation(Capsule)
support += cone->getSweptSphereRadius() * dir.normalized();
}
}
-// clang-format off
-getShapeSupportTplInstantiation(Cone)
- // clang-format on
+getShapeSupportTplInstantiation(Cone);
- // ============================================================================
- template <int _SupportOptions>
- void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
- ShapeSupportData& /*unused*/) {
+// ============================================================================
+template <int _SupportOptions>
+void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
+ int& /*unused*/, ShapeSupportData& /*unused*/) {
static const FCL_REAL dummy_precision =
Eigen::NumTraits<FCL_REAL>::dummy_precision();
@@ -313,15 +299,13 @@ getShapeSupportTplInstantiation(Cone)
support += cylinder->getSweptSphereRadius() * dir.normalized();
}
}
-// clang-format off
-getShapeSupportTplInstantiation(Cylinder)
- // clang-format on
+getShapeSupportTplInstantiation(Cylinder);
- // ============================================================================
- template <int _SupportOptions>
- void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
- Vec3f& support, int& hint,
- ShapeSupportData& support_data) {
+// ============================================================================
+template <int _SupportOptions>
+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<const ConvexBase*>(convex), dir, support, hint,
support_data);
}
-// clang-format off
-getShapeSupportTplInstantiation(SmallConvex)
- // clang-format on
+getShapeSupportTplInstantiation(SmallConvex);
- // ============================================================================
- template <int _SupportOptions>
- inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
- Vec3f& support, int& hint,
- ShapeSupportData& support_data) {
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
+ Vec3f& support, int& hint,
+ ShapeSupportData& support_data) {
getShapeSupportLog<_SupportOptions>(
reinterpret_cast<const ConvexBase*>(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<const ShapeType*>(shape), \
support_set, hint, support_data, \
max_num_supports, tol)
- template <int _SupportOptions>
- void getSupportSet(const ShapeBase* shape, SupportSet& support_set,
- int& hint, size_t max_num_supports, FCL_REAL tol) {
+template <int _SupportOptions>
+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 <int _SupportOptions>
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 <int _SupportOptions>
@@ -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 <int _SupportOptions>
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 <int _SupportOptions>
@@ -691,8 +664,7 @@ void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
}
}
}
-// clang-format off
-getShapeSupportSetTplInstantiation(Capsule)
+getShapeSupportSetTplInstantiation(Capsule);
// ============================================================================
template <int _SupportOptions>
@@ -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 <int _SupportOptions>
@@ -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 <int _SupportOptions>
@@ -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<int8_t>& 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 <int _SupportOptions>
@@ -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<const ConvexBase*>(convex), support_set, hint,
support_data, num_sampled_supports, tol);
}
-// clang-format off
-getShapeSupportSetTplInstantiation(SmallConvex)
+getShapeSupportSetTplInstantiation(SmallConvex);
// ============================================================================
template <int _SupportOptions>
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<const ConvexBase*>(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 7c8abed2..6ca93eff 100644
--- a/src/octree.cpp
+++ b/src/octree.cpp
@@ -58,15 +58,15 @@ struct Neighbors {
void computeNeighbors(const std::vector<Vec6f>& boxes,
std::vector<Neighbors>& neighbors) {
typedef std::vector<Vec6f> 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<Vec6f>& 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<FCL_REAL, Eigen::Dynamic, 3>& point_cloud,
- const FCL_REAL resolution) {
- typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> InputType;
+ const Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3>& point_cloud,
+ const CoalScalar resolution) {
+ typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 3> InputType;
typedef InputType::ConstRowXpr RowType;
shared_ptr<octomap::OcTree> octree(new octomap::OcTree(resolution));
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index b0b68e45..ba7a2078 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<AABB>(*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<AABB>(*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<AABB>(*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<AABB>(*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<AABB>(*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<AABB>(*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<AABB>(*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<AABB>(*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<AABB>(*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<AABB>(*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 c03335b8..b5d4bf2f 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -45,9 +45,9 @@ namespace details {
std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf) {
std::vector<Vec3f> 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<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf) {
std::vector<Vec3f> getBoundVertices(const Sphere& sphere,
const Transform3f& tf) {
std::vector<Vec3f> 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<Vec3f> getBoundVertices(const Sphere& sphere,
std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
const Transform3f& tf) {
std::vector<Vec3f> 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<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
const Transform3f& tf) {
std::vector<Vec3f> 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<Vec3f> 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<Vec3f> getBoundVertices(const Capsule& capsule,
std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf) {
std::vector<Vec3f> 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<Vec3f> getBoundVertices(const Cylinder& cylinder,
const Transform3f& tf) {
std::vector<Vec3f> 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<Halfspace, 2> 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<Halfspace, 2> result = {Halfspace(n, d), Halfspace(-n, -d)};
result[0].setSweptSphereRadius(a.getSweptSphereRadius());
result[1].setSweptSphereRadius(a.getSweptSphereRadius());
@@ -334,12 +334,12 @@ void computeBV<AABB, Cone>(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<AABB, Cylinder>(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<AABB, Halfspace>(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<FCL_REAL>::max)());
- bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
- if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+ bv_.min_ = Vec3f::Constant(-(std::numeric_limits<CoalScalar>::max)());
+ bv_.max_ = Vec3f::Constant((std::numeric_limits<CoalScalar>::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<AABB, Plane>(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<FCL_REAL>::max)());
- bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)());
- if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) {
+ bv_.min_ = Vec3f::Constant(-(std::numeric_limits<CoalScalar>::max)());
+ bv_.max_ = Vec3f::Constant((std::numeric_limits<CoalScalar>::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<OBB, Halfspace>(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<FCL_REAL>::max)()));
+ bv.extent.setConstant(((std::numeric_limits<CoalScalar>::max)()));
}
template <>
@@ -565,7 +565,7 @@ void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f&,
bv.axes.setIdentity();
bv.Tr.setZero();
bv.length[0] = bv.length[1] = bv.radius =
- (std::numeric_limits<FCL_REAL>::max)();
+ (std::numeric_limits<CoalScalar>::max)();
}
template <>
@@ -589,7 +589,7 @@ void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf,
bv.num_spheres = 1;
computeBV<OBB, Halfspace>(s, tf, bv.obb);
bv.spheres[0].o = Vec3f();
- bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
+ bv.spheres[0].r = (std::numeric_limits<CoalScalar>::max)();
}
template <>
@@ -601,50 +601,50 @@ void computeBV<KDOP<16>, 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<FCL_REAL>::max)();
+ bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
for (short i = D; i < 2 * D; ++i)
- bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+ bv.dist(i) = (std::numeric_limits<CoalScalar>::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<KDOP<18>, 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<FCL_REAL>::max)();
+ bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
for (short i = D; i < 2 * D; ++i)
- bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+ bv.dist(i) = (std::numeric_limits<CoalScalar>::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<KDOP<24>, 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<FCL_REAL>::max)();
+ bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
for (short i = D; i < 2 * D; ++i)
- bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+ bv.dist(i) = (std::numeric_limits<CoalScalar>::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<OBB, Plane>(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<FCL_REAL>::max)(),
- (std::numeric_limits<FCL_REAL>::max)();
+ bv.extent << 0, (std::numeric_limits<CoalScalar>::max)(),
+ (std::numeric_limits<CoalScalar>::max)();
Vec3f p = s.n * s.d;
bv.To =
@@ -828,8 +828,8 @@ void computeBV<RSS, Plane>(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<FCL_REAL>::max)();
- bv.length[1] = (std::numeric_limits<FCL_REAL>::max)();
+ bv.length[0] = (std::numeric_limits<CoalScalar>::max)();
+ bv.length[1] = (std::numeric_limits<CoalScalar>::max)();
bv.radius = 0;
@@ -857,7 +857,7 @@ void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv) {
bv.num_spheres = 1;
computeBV<OBB, Plane>(s, tf, bv.obb);
bv.spheres[0].o = Vec3f();
- bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)();
+ bv.spheres[0].r = (std::numeric_limits<CoalScalar>::max)();
}
template <>
@@ -869,39 +869,39 @@ void computeBV<KDOP<16>, 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<FCL_REAL>::max)();
+ bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
for (short i = D; i < 2 * D; ++i)
- bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+ bv.dist(i) = (std::numeric_limits<CoalScalar>::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<KDOP<18>, 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<FCL_REAL>::max)();
+ bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
for (short i = D; i < 2 * D; ++i)
- bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+ bv.dist(i) = (std::numeric_limits<CoalScalar>::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<KDOP<24>, 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<FCL_REAL>::max)();
+ bv.dist(i) = -(std::numeric_limits<CoalScalar>::max)();
for (short i = D; i < 2 * D; ++i)
- bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)();
+ bv.dist(i) = (std::numeric_limits<CoalScalar>::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 01ed51e1..909e2979 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<unsigned int, unsigned int> BVPair_t;
// typedef std::stack<BVPair_t, std::vector<BVPair_t> > Stack_t;
typedef std::vector<BVPair_t> Stack_t;
Stack_t pairs;
pairs.reserve(1000);
- sqrDistLowerBound = std::numeric_limits<FCL_REAL>::infinity();
- FCL_REAL sdlb = std::numeric_limits<FCL_REAL>::infinity();
+ sqrDistLowerBound = std::numeric_limits<CoalScalar>::infinity();
+ CoalScalar sdlb = std::numeric_limits<CoalScalar>::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 ed744b01..0526e499 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<Transform3f> transforms;
generateRandomTransforms(extents, transforms, n);
Transform3f identity = Transform3f::Identity();
diff --git a/test/benchmark.cpp b/test/benchmark.cpp
index 6cca3275..584ceb64 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 <typename BV>
void makeModel(const std::vector<Vec3f>& vertices,
@@ -208,7 +208,7 @@ int main(int, char*[]) {
makeModel(p2, t2, SPLIT_METHOD_MEDIAN, ms_obbrss[1][SPLIT_METHOD_MEDIAN]);
std::vector<Transform3f> 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 fdbb4dda..2633febd 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 a7d18d07..4bb9b642 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 <typename U, typename V>
@@ -146,9 +146,9 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
double env_scale, std::size_t n) {
int n_edge = static_cast<int>(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<CollisionObject*>& env,
double env_scale, std::size_t n) {
int n_edge = static_cast<int>(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 1fc94036..2655390f 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<TStruct> ts;
std::vector<BenchTimer> 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>();
- FCL_REAL delta_trans_max = 0.01 * env_scale;
+ CoalScalar delta_angle_max =
+ 10 / 360.0 * 2 * boost::math::constants::pi<CoalScalar>();
+ 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>();
- FCL_REAL delta_trans_max = 0.01 * env_scale;
+ CoalScalar delta_angle_max =
+ 10 / 360.0 * 2 * boost::math::constants::pi<CoalScalar>();
+ 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 7f6f5ca5..329ebbed 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 25c64d43..f13c73e8 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<CallBackData*>(callback_data);
const std::vector<CollisionObject*>& objects = *(data->objects);
diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp
index 2d3f5e50..4b1f8aed 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 1f429914..a20f0fe8 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 de61e1a8..76bd8824 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<int>::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<Transform3f> 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<Transform3f> 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<Transform3f> 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<Transform3f> 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<Transform3f> transforms;
@@ -624,7 +624,7 @@ struct mesh_mesh_run_test {
//
BOOST_AUTO_TEST_CASE(mesh_mesh) {
std::vector<Transform3f> 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<Transform3f> 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 57506e59..d1dbc9e3 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<FCL_REAL>();
+constexpr CoalScalar pi = boost::math::constants::pi<CoalScalar>();
double DegToRad(const double& deg) {
static double degToRad = pi / 180.;
diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp
index 36725fd0..0a02bbe9 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<Vec3f, ContactPatch::default_preallocated_size> 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<Vec3f, ContactPatch::default_preallocated_size> 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<Quadrilateral> box1(buildBox(halfside, halfside, halfside));
const Convex<Quadrilateral> 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 ebcc4a09..bc48ca8d 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<Quadrilateral> box(buildBox(l, w, d));
// Check neighbors
@@ -89,7 +89,7 @@ BOOST_AUTO_TEST_CASE(convex) {
template <typename Sa, typename Sb>
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 <typename Sa, typename Sb>
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<Quadrilateral> convex_box(buildBox(l, w, d));
diff --git a/test/distance.cpp b/test/distance.cpp
index 8066dfa7..603d57ba 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 <typename BV>
void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
@@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE(mesh_distance) {
loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
std::vector<Transform3f> 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 50695a27..7c034f69 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<Transform3f> 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<Transform3f> 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<Transform3f> 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<Transform3f> 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 0728f6bb..2ed4dd76 100644
--- a/test/frontlist.cpp
+++ b/test/frontlist.cpp
@@ -86,8 +86,8 @@ BOOST_AUTO_TEST_CASE(front_list) {
std::vector<Transform3f> transforms; // t0
std::vector<Transform3f> 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 ec07c2d5..c391e6a2 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 <typename S1, typename S2>
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 <typename S1, typename S2>
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 <typename S1, typename S2>
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<FCL_REAL>::epsilon();
+ CoalScalar eps = Eigen::NumTraits<CoalScalar>::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 <typename S1, typename S2>
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 ee54597b..cdfb4d5e 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<FCL_REAL, Eigen::Dynamic, 1> vector_t;
-typedef Eigen::Matrix<FCL_REAL, 6, 1> vector6_t;
-typedef Eigen::Matrix<FCL_REAL, 4, 1> vector4_t;
-typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 1> vector_t;
+typedef Eigen::Matrix<CoalScalar, 6, 1> vector6_t;
+typedef Eigen::Matrix<CoalScalar, 4, 1> vector4_t;
+typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, Eigen::Dynamic> 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<FCL_REAL, 4, 1> Vec4f;
+ typedef Eigen::Matrix<CoalScalar, 4, 1> 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 926ed28a..4028b869 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<FCL_REAL>();
+constexpr CoalScalar pi = boost::math::constants::pi<CoalScalar>();
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 6fccdb20..5283e451 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<Transform3f> transforms;
generateRandomTransforms(extents, transforms, n);
Transform3f identity = Transform3f::Identity();
diff --git a/test/hfields.cpp b/test/hfields.cpp
index 7492bd9d..0042cd80 100644
--- a/test/hfields.cpp
+++ b/test/hfields.cpp
@@ -58,9 +58,9 @@ using namespace coal;
template <typename BV>
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<BV> 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<OBBRSS>(2, 2, min_altitude,
max_altitude); // Simple case
@@ -272,9 +272,9 @@ BOOST_AUTO_TEST_CASE(building_constant_hfields) {
template <typename BV>
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<BV> 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<OBBRSS>(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<bool, Eigen::Dynamic, Eigen::Dynamic> 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<bool, Eigen::Dynamic, Eigen::Dynamic> 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 03827c6f..ad44b43f 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 81176f5e..e440afdc 100644
--- a/test/normal_and_nearest_points.cpp
+++ b/test/normal_and_nearest_points.cpp
@@ -75,9 +75,9 @@ template <typename ShapeType1, typename ShapeType2>
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<Transform3f> transforms;
generateRandomTransforms(extents, transforms, n);
Transform3f tf1 = Transform3f::Identity();
CollisionRequest colreq;
- colreq.distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
+ colreq.distance_upper_bound = std::numeric_limits<CoalScalar>::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<FCL_REAL>::epsilon());
+ const CoalScalar dummy_precision(
+ 100 * std::numeric_limits<CoalScalar>::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 <int VecSize>
-Eigen::Matrix<FCL_REAL, VecSize, 1> generateRandomVector(FCL_REAL min,
- FCL_REAL max) {
- typedef Eigen::Matrix<FCL_REAL, VecSize, 1> VecType;
+Eigen::Matrix<CoalScalar, VecSize, 1> generateRandomVector(CoalScalar min,
+ CoalScalar max) {
+ typedef Eigen::Matrix<CoalScalar, VecSize, 1> 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<FCL_REAL>(rand()) / static_cast<FCL_REAL>(RAND_MAX);
+CoalScalar generateRandomNumber(CoalScalar min, CoalScalar max) {
+ CoalScalar r =
+ static_cast<CoalScalar>(rand()) / static_cast<CoalScalar>(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<Sphere> o1(new Sphere(radii(0)));
shared_ptr<Capsule> 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<Ellipsoid> 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<Ellipsoid> 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<Box> 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<Plane> 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<Box> 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<Halfspace> 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<Capsule> 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<Halfspace> 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<Sphere> 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<Halfspace> 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<Sphere> 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<Plane> 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<Convex<Triangle>> o1(new Convex<Triangle>(
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<Halfspace> o2(new Halfspace(n, offset));
@@ -458,9 +459,9 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_cylinder) {
shared_ptr<Cylinder> 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<Cylinder> 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<Cone> 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<Halfspace> 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<Cylinder> 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<Halfspace> 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<Cone> 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<Plane> 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<Cylinder> 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<Plane> 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<Capsule> 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<Plane> 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<Sphere> o1(new Sphere(r(0)));
shared_ptr<Cylinder> 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<Ellipsoid> 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<Ellipsoid> o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
@@ -623,14 +624,14 @@ void test_normal_and_nearest_points(const BVHModel<OBBRSS>& 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<Transform3f> transforms;
generateRandomTransforms(extents, transforms, n);
Transform3f tf1 = Transform3f::Identity();
transforms[0] = Transform3f::Identity();
CollisionRequest colreq;
- colreq.distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
+ colreq.distance_upper_bound = std::numeric_limits<CoalScalar>::max();
colreq.num_max_contacts = 100;
CollisionResult colres;
DistanceRequest distreq;
@@ -641,7 +642,7 @@ void test_normal_and_nearest_points(const BVHModel<OBBRSS>& 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<Halfspace> o2(new Halfspace(n, offset));
diff --git a/test/obb.cpp b/test/obb.cpp
index 8405484b..d8db1dd8 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 <int ia, int ib, int ja = (ia + 1) % 3, int ka = (ia + 2) % 3,
struct loop_case_1 {
static inline bool run(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 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) {
@@ -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 <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
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 87bf39c6..894c2803 100644
--- a/test/octree.cpp
+++ b/test/octree.cpp
@@ -67,7 +67,7 @@ void makeMesh(const std::vector<Vec3f>& vertices,
}
coal::OcTree makeOctree(const BVHModel<OBBRSS>& 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<OBBRSS>& 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<OBBRSS>& mesh,
BOOST_AUTO_TEST_CASE(octree_mesh) {
Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
"", "", "(", ")");
- FCL_REAL resolution(10.);
+ CoalScalar resolution(10.);
std::vector<Vec3f> pRob, pEnv;
std::vector<Triangle> tRob, tEnv;
boost::filesystem::path path(TEST_RESOURCES_DIR);
@@ -136,11 +136,11 @@ BOOST_AUTO_TEST_CASE(octree_mesh) {
{
const std::vector<uint8_t> bytes = envOctree.tobytes();
BOOST_CHECK(bytes.size() > 0 && bytes.size() <= envOctree.toBoxes().size() *
- 3 * sizeof(FCL_REAL));
+ 3 * sizeof(CoalScalar));
}
std::vector<Transform3f> 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<Vec3f> pEnv;
std::vector<Triangle> 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<AABB> hfield(x_dim, y_dim, heights, min_altitude);
hfield.computeLocalAABB();
std::vector<Transform3f> 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 5ac74771..bdc5eb15 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 59ef596f..1f05503f 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 <typename ShapeType1, typename ShapeType2>
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 58e1ac88..3d0769f8 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 33f285da..3a32cb6c 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 <typename Shape>
-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<FCL_REAL, 1, 1> Matrix;
+bool isApprox(const CoalScalar &v1, const CoalScalar &v2,
+ const CoalScalar tol) {
+ typedef Eigen::Matrix<CoalScalar, 1, 1> 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 <typename Shape>
-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 <typename Shape>
-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 <typename Shape>
-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 2bfd3fa9..9dfd5db8 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 94527919..cbeceab0 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 6d2c3364..a89c8937 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 <typename S1, typename S2>
- 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, S2, details::SupportOptions::WithSweptSphere>(
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, S2, details::SupportOptions::NoSweptSphere>(
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<Transform3f> tf1s;
std::vector<Transform3f> tf2s;
generateRandomTransforms(extents, tf1s, n);
generateRandomTransforms(extents, tf2s, n);
- const std::array<FCL_REAL, 4> swept_sphere_radius = {0, 0.1, 1., 10.};
+ const std::array<CoalScalar, 4> 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<FCL_REAL, 2> distance;
+ std::array<CoalScalar, 2> distance;
std::array<Vec3f, 2> p1;
std::array<Vec3f, 2> p2;
std::array<Vec3f, 2> 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<Triangle> 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<Transform3f> tf1s;
std::vector<Transform3f> tf2s;
generateRandomTransforms(extents, tf1s, n);
generateRandomTransforms(extents, tf2s, n);
- const std::array<FCL_REAL, 4> swept_sphere_radius = {0, 0.1, 1., 10.};
- for (const FCL_REAL& ssr1 : swept_sphere_radius) {
+ const std::array<CoalScalar, 4> 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<FCL_REAL>::max();
- const FCL_REAL tol = 1e-6;
+ request.security_margin = std::numeric_limits<CoalScalar>::max();
+ const CoalScalar tol = 1e-6;
request.gjk_tolerance = tol;
request.epa_tolerance = tol;
std::array<CollisionResult, 2> 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 11390269..bddb9b4d 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<Vec3f>& 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<Vec3f>& 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<Transform3f>& 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<Transform3f>& transforms,
std::vector<Transform3f>& 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<DistanceData*>(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<CollisionObject*>& 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<Transform3f> transforms(n);
generateRandomTransforms(extents, transforms, n);
@@ -420,9 +421,9 @@ void generateEnvironments(std::vector<CollisionObject*>& env,
}
void generateEnvironmentsMesh(std::vector<CollisionObject*>& 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<Transform3f> transforms;
generateRandomTransforms(extents, transforms, n);
@@ -456,7 +457,7 @@ void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
}
}
-Convex<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d) {
+Convex<Quadrilateral> buildBox(CoalScalar l, CoalScalar w, CoalScalar d) {
std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>(
{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<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
- FCL_REAL PHI = (1 + std::sqrt(5)) / 2;
+ CoalScalar PHI = (1 + std::sqrt(5)) / 2;
// vertices
std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
@@ -555,50 +556,50 @@ Convex<Triangle> 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<FCL_REAL, 2> min_size,
- std::array<FCL_REAL, 2> max_size) {
+Capsule makeRandomCapsule(std::array<CoalScalar, 2> min_size,
+ std::array<CoalScalar, 2> max_size) {
return Capsule(rand_interval(min_size[0], max_size[0]),
rand_interval(min_size[1], max_size[1]));
}
-Cone makeRandomCone(std::array<FCL_REAL, 2> min_size,
- std::array<FCL_REAL, 2> max_size) {
+Cone makeRandomCone(std::array<CoalScalar, 2> min_size,
+ std::array<CoalScalar, 2> max_size) {
return Cone(rand_interval(min_size[0], max_size[0]),
rand_interval(min_size[1], max_size[1]));
}
-Cylinder makeRandomCylinder(std::array<FCL_REAL, 2> min_size,
- std::array<FCL_REAL, 2> max_size) {
+Cylinder makeRandomCylinder(std::array<CoalScalar, 2> min_size,
+ std::array<CoalScalar, 2> max_size) {
return Cylinder(rand_interval(min_size[0], max_size[0]),
rand_interval(min_size[1], max_size[1]));
}
-Convex<Triangle> makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size) {
+Convex<Triangle> 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 78f4b05b..70c86809 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<FCL_REAL> AngleAxis;
+typedef Eigen::AngleAxis<CoalScalar> AngleAxis;
extern const Vec3f UnitX;
extern const Vec3f UnitY;
extern const Vec3f UnitZ;
@@ -139,26 +139,26 @@ void saveOBJFile(const char* filename, std::vector<Vec3f>& 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<Transform3f>& 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<Transform3f>& transforms,
std::vector<Transform3f>& 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<CollisionObject*>& env,
- FCL_REAL env_scale, std::size_t n);
+ CoalScalar env_scale, std::size_t n);
void generateEnvironmentsMesh(std::vector<CollisionObject*>& 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<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d);
+Convex<Quadrilateral> 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<Quadrilateral> buildBox(FCL_REAL l, FCL_REAL w, FCL_REAL d);
/// ellipsoid tranformation to each vertex of the icosahedron.
Convex<Triangle> 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<FCL_REAL, 2> min_size,
- std::array<FCL_REAL, 2> max_size);
+Capsule makeRandomCapsule(std::array<CoalScalar, 2> min_size,
+ std::array<CoalScalar, 2> max_size);
-Cone makeRandomCone(std::array<FCL_REAL, 2> min_size,
- std::array<FCL_REAL, 2> max_size);
+Cone makeRandomCone(std::array<CoalScalar, 2> min_size,
+ std::array<CoalScalar, 2> max_size);
-Cylinder makeRandomCylinder(std::array<FCL_REAL, 2> min_size,
- std::array<FCL_REAL, 2> max_size);
+Cylinder makeRandomCylinder(std::array<CoalScalar, 2> min_size,
+ std::array<CoalScalar, 2> max_size);
-Convex<Triangle> makeRandomConvex(FCL_REAL min_size, FCL_REAL max_size);
+Convex<Triangle> 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<ShapeBase> makeRandomGeometry(NODE_TYPE node_type);
--
GitLab