From 120757884a855edfa20d7ac7b1eaa6b7aa8ffb02 Mon Sep 17 00:00:00 2001
From: Louis Montaut <louismontaut@gmail.com>
Date: Sun, 23 Jun 2024 17:30:22 +0200
Subject: [PATCH] all: rename `Matrix/Vec[..]f` typedefs to `Matrix/Vec[...]s`
We keep the old typedefs for backward compatibility
---
README.md | 4 +-
doc/gjk.py | 4 +-
doc/mesh-mesh-collision-call.dot | 14 +-
doc/shape-mesh-collision-call.dot | 20 +-
doc/shape-shape-collision-call.dot | 4 +-
include/coal/BV/AABB.h | 34 +-
include/coal/BV/BV.h | 34 +-
include/coal/BV/BV_node.h | 16 +-
include/coal/BV/OBB.h | 26 +-
include/coal/BV/OBBRSS.h | 20 +-
include/coal/BV/RSS.h | 24 +-
include/coal/BV/kDOP.h | 20 +-
include/coal/BV/kIOS.h | 26 +-
include/coal/BVH/BVH_model.h | 72 +-
include/coal/BVH/BVH_utility.h | 22 +-
include/coal/broadphase/broadphase_SaP.h | 4 +-
.../broadphase/broadphase_spatialhash-inl.h | 8 +-
.../coal/broadphase/broadphase_spatialhash.h | 8 +-
.../broadphase/detail/hierarchy_tree-inl.h | 24 +-
.../coal/broadphase/detail/hierarchy_tree.h | 4 +-
.../detail/hierarchy_tree_array-inl.h | 20 +-
.../broadphase/detail/hierarchy_tree_array.h | 4 +-
include/coal/broadphase/detail/morton-inl.h | 6 +-
include/coal/broadphase/detail/morton.h | 18 +-
include/coal/collision_data.h | 76 +-
include/coal/collision_object.h | 32 +-
.../coal/contact_patch/contact_patch_solver.h | 4 +-
.../contact_patch/contact_patch_solver.hxx | 54 +-
include/coal/data_types.h | 8 +-
include/coal/hfield.h | 50 +-
include/coal/internal/BV_fitter.h | 20 +-
include/coal/internal/BV_splitter.h | 18 +-
include/coal/internal/intersect.h | 72 +-
include/coal/internal/shape_shape_func.h | 24 +-
include/coal/internal/traversal.h | 14 +-
.../coal/internal/traversal_node_bvh_hfield.h | 48 +-
.../coal/internal/traversal_node_bvh_shape.h | 16 +-
include/coal/internal/traversal_node_bvhs.h | 52 +-
.../internal/traversal_node_hfield_shape.h | 142 +-
include/coal/internal/traversal_node_octree.h | 22 +-
include/coal/internal/traversal_node_setup.h | 48 +-
include/coal/math/transform.h | 32 +-
include/coal/mesh_loader/assimp.h | 8 +-
include/coal/mesh_loader/loader.h | 8 +-
include/coal/narrowphase/gjk.h | 32 +-
.../coal/narrowphase/minkowski_difference.h | 14 +-
include/coal/narrowphase/narrowphase.h | 64 +-
include/coal/narrowphase/support_functions.h | 38 +-
include/coal/octree.h | 22 +-
include/coal/serialization/collision_data.h | 6 +-
include/coal/serialization/convex.h | 4 +-
include/coal/serialization/kIOS.h | 4 +-
include/coal/shape/convex.h | 8 +-
include/coal/shape/details/convex.hxx | 48 +-
.../coal/shape/geometric_shape_to_BVH_model.h | 48 +-
include/coal/shape/geometric_shapes.h | 96 +-
include/coal/shape/geometric_shapes_utility.h | 18 +-
python/broadphase/broadphase.cc | 2 +-
python/collision-geometries.cc | 68 +-
python/collision.cc | 8 +-
python/distance.cc | 4 +-
python/gjk.cc | 4 +-
python/math.cc | 30 +-
python/octree.cc | 4 +-
src/BV/AABB.cpp | 28 +-
src/BV/OBB.cpp | 114 +-
src/BV/OBB.h | 8 +-
src/BV/OBBRSS.cpp | 2 +-
src/BV/RSS.cpp | 94 +-
src/BV/kDOP.cpp | 28 +-
src/BV/kIOS.cpp | 18 +-
src/BVH/BVH_model.cpp | 124 +-
src/BVH/BVH_utility.cpp | 122 +-
src/BVH/BV_fitter.cpp | 150 +-
src/BVH/BV_splitter.cpp | 60 +-
src/broadphase/broadphase_SSaP.cpp | 8 +-
src/broadphase/broadphase_SaP.cpp | 14 +-
src/broadphase/broadphase_interval_tree.cpp | 6 +-
src/broadphase/detail/morton-inl.h | 6 +-
src/distance/box_halfspace.cpp | 4 +-
src/distance/box_plane.cpp | 4 +-
src/distance/box_sphere.cpp | 4 +-
src/distance/capsule_capsule.cpp | 22 +-
src/distance/capsule_halfspace.cpp | 4 +-
src/distance/capsule_plane.cpp | 4 +-
src/distance/cone_halfspace.cpp | 4 +-
src/distance/cone_plane.cpp | 4 +-
src/distance/convex_halfspace.cpp | 4 +-
src/distance/convex_plane.cpp | 4 +-
src/distance/cylinder_halfspace.cpp | 4 +-
src/distance/cylinder_plane.cpp | 4 +-
src/distance/ellipsoid_halfspace.cpp | 4 +-
src/distance/ellipsoid_plane.cpp | 4 +-
src/distance/halfspace_halfspace.cpp | 2 +-
src/distance/halfspace_plane.cpp | 4 +-
src/distance/plane_plane.cpp | 2 +-
src/distance/sphere_capsule.cpp | 4 +-
src/distance/sphere_cylinder.cpp | 4 +-
src/distance/sphere_halfspace.cpp | 4 +-
src/distance/sphere_plane.cpp | 4 +-
src/distance/sphere_sphere.cpp | 2 +-
src/distance/triangle_halfspace.cpp | 4 +-
src/distance/triangle_plane.cpp | 4 +-
src/distance/triangle_sphere.cpp | 4 +-
src/distance/triangle_triangle.cpp | 4 +-
src/intersect.cpp | 162 +--
src/math/transform.cpp | 2 +-
src/mesh_loader/assimp.cpp | 6 +-
src/mesh_loader/loader.cpp | 6 +-
src/narrowphase/details.h | 180 +--
src/narrowphase/gjk.cpp | 100 +-
src/narrowphase/minkowski_difference.cpp | 18 +-
src/narrowphase/support_functions.cpp | 385 ++---
src/octree.cpp | 38 +-
src/shape/convex.cpp | 22 +-
src/shape/geometric_shapes.cpp | 50 +-
src/shape/geometric_shapes_utility.cpp | 318 ++---
test/accelerated_gjk.cpp | 10 +-
test/benchmark.cpp | 6 +-
test/box_box_collision.cpp | 6 +-
test/box_box_distance.cpp | 40 +-
test/broadphase.cpp | 20 +-
test/broadphase_collision_1.cpp | 28 +-
test/broadphase_collision_2.cpp | 2 +-
test/bvh_models.cpp | 32 +-
test/capsule_box_1.cpp | 10 +-
test/capsule_box_2.cpp | 6 +-
test/capsule_capsule.cpp | 12 +-
test/collision.cpp | 16 +-
test/collision_node_asserts.cpp | 16 +-
test/contact_patch.cpp | 226 +--
test/convex.cpp | 88 +-
test/distance.cpp | 32 +-
test/distance_lower_bound.cpp | 6 +-
test/frontlist.cpp | 28 +-
test/geometric_shapes.cpp | 1250 ++++++++---------
test/gjk.cpp | 130 +-
test/gjk_asserts.cpp | 12 +-
test/gjk_convergence_criterion.cpp | 10 +-
test/hfields.cpp | 162 +--
test/math.cpp | 42 +-
test/normal_and_nearest_points.cpp | 58 +-
test/obb.cpp | 114 +-
test/octree.cpp | 14 +-
test/profiling.cpp | 14 +-
test/python_unit/collision.py | 2 +-
test/python_unit/geometric_shapes.py | 2 +-
test/python_unit/pickling.py | 2 +-
test/security_margin.cpp | 40 +-
test/serialization.cpp | 34 +-
test/shape_inflation.cpp | 8 +-
test/shape_mesh_consistency.cpp | 212 +--
test/simple.cpp | 68 +-
test/swept_sphere_radius.cpp | 10 +-
test/utility.cpp | 82 +-
test/utility.h | 14 +-
156 files changed, 3379 insertions(+), 3376 deletions(-)
diff --git a/README.md b/README.md
index d8a6d1af..7cb262f3 100644
--- a/README.md
+++ b/README.md
@@ -113,10 +113,10 @@ int main() {
// Define the shapes' placement in 3D space
hpp::fcl::Transform3f T1;
T1.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom());
- T1.setTranslation(hpp::fcl::Vec3f::Random());
+ T1.setTranslation(hpp::fcl::Vec3s::Random());
hpp::fcl::Transform3f T2 = hpp::fcl::Transform3f::Identity();
T2.setQuatRotation(hpp::fcl::Quaternion3f::UnitRandom());
- T2.setTranslation(hpp::fcl::Vec3f::Random());
+ T2.setTranslation(hpp::fcl::Vec3s::Random());
// Define collision requests and results.
//
diff --git a/doc/gjk.py b/doc/gjk.py
index 2b7019f9..bee0d547 100644
--- a/doc/gjk.py
+++ b/doc/gjk.py
@@ -444,7 +444,7 @@ def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]):
for v in "abcd":
print(
indent
- + "const Vec3f& {} (current.vertex[{}]->w);".format(v.upper(), v),
+ + "const Vec3s& {} (current.vertex[{}]->w);".format(v.upper(), v),
file=file,
)
print(indent + "const CoalScalar aa = A.squaredNorm();".format(), file=file)
@@ -471,7 +471,7 @@ def printOrder(order, indent="", start=True, file=sys.stdout, curTests=[]):
)
for v in "bc":
print(
- indent + "const Vec3f a_cross_{0} = A.cross({1});".format(v, v.upper()),
+ indent + "const Vec3s a_cross_{0} = A.cross({1});".format(v, v.upper()),
file=file,
)
print("", file=file)
diff --git a/doc/mesh-mesh-collision-call.dot b/doc/mesh-mesh-collision-call.dot
index 8c91a382..5069bbc4 100644
--- a/doc/mesh-mesh-collision-call.dot
+++ b/doc/mesh-mesh-collision-call.dot
@@ -7,8 +7,8 @@ digraph CD {
"std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" [shape = box]
"bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
"bool OBBRSS::overlap(const OBBRSS& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
- "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
- "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" [shape = box]
"bool BVNode::overlap(const BVNode& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" [shape = box]
"bool BVHCollisionTraversalNode::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box]
"bool MeshCollisionTraversalNode<T_BVH>::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" [shape = box]
@@ -17,19 +17,19 @@ digraph CD {
"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, 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]
+ "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)" [shape = box]
"std::size_t BVHCollide(const CollisionGeometry* o1,\nconst Transform3f& tf1, const CollisionGeometry* o2,\nconst Transform3f& tf2, const CollisionRequest& request,\nCollisionResult& result)" -> "void collide(MeshCollisionTraversalNode<T_BVH>* node,\n const CollisionRequest& request, CollisionResult& result,\nBVHFrontList* front_list)"
- "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)"
- "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,\n CoalScalar& squaredLowerBoundDistance)"
- "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
+ "bool OBB::overlap(const OBB& other,\nconst CollisionRequest& request,\nCoalScalar& sqrDistLowerBound) const" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool obbDisjointAndLowerBoundDistance\n(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,\n CoalScalar& squaredLowerBoundDistance)"
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBB& b1, const OBB& b2,\n const CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
"void collide(MeshCollisionTraversalNode<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, 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 MeshCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\n -request\n - result" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0,\n const OBBRSS& b1, const OBBRSS& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)"
"bool MeshCollisionTraversalNode<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"
diff --git a/doc/shape-mesh-collision-call.dot b/doc/shape-mesh-collision-call.dot
index c08fbdfe..3eb29364 100644
--- a/doc/shape-mesh-collision-call.dot
+++ b/doc/shape-mesh-collision-call.dot
@@ -13,11 +13,11 @@ digraph CD {
"virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" [shape = box]
"bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
"bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [shape = box]
- "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box]
- "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box]
- "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box]
- "void details::meshShapeCollisionOrientedNodeLeafTesting <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]
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" [shape = box]
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" [shape = box]
+ "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp" [shape = box]
+ "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" [shape = box]
+ "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3f& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156" [shape = box]
"BVHShapeCollider<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"
@@ -28,9 +28,9 @@ digraph CD {
"void collisionRecurse(CollisionTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_recurse.cpp" -> "virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h"
"virtual bool CollisionTraversalNodeBase::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const = 0\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color=red]
"virtual void CollisionTraversalNodeBase::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_base.h" -> "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" [color = red]
- "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp"
- "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp"
- "bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,\nconst Vec3f& a, const Vec3f& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp"
- "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting <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"
+ "bool MeshShapeCollisionTraversalNodeOBBRSS::BVTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp"
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,\nconst OBBRSS& b2, const CollisionRequest& request,\nCoalScalar& sqrDistLowerBound)\nBV/OBBRSS.cpp" -> "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp"
+ "bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,\nconst CollisionRequest& request, CoalScalar& sqrDistLowerBound)\nBV/OBB.cpp" -> "bool obbDisjointAndLowerBoundDistance (const Matrix3s& B, const Vec3s& T,\nconst Vec3s& a, const Vec3s& b,\nconst CollisionRequest& request,\nCoalScalar& squaredLowerBoundDistance)\nBV/OBB.cpp"
+ "bool MeshShapeCollisionTraversalNodeOBBRSS::leafTesting\n(int b1, int b2, CoalScalar& sqrDistLowerBound) const\ntraversal/traversal_node_bvh_shape.h" -> "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293"
+ "void details::meshShapeCollisionOrientedNodeLeafTesting <OBBRSS, Shape, NarrowPhaseSolver>\n(int b1, int b2, const BVHModel<BV>* model1, const S& model2,\nVec3s* vertices, Triangle* tri_indices, const Transform3f& tf1,\nconst Transform3f& tf2, const NarrowPhaseSolver* nsolver,\nbool enable_statistics, int& num_leaf_tests,\nconst CollisionRequest& request, CollisionResult& result,\nCoalScalar& sqrDistLowerBound)\ntraversal/traversal_node_bvh_shape.h:293" -> "bool GJKSolver_indep::shapeTriangleIntersect<Shape>\n(const S& s, const Transform3f& tf,\nconst Vec3s& P1, const Vec3s& P2, const Vec3s& P3,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h:156"
}
diff --git a/doc/shape-shape-collision-call.dot b/doc/shape-shape-collision-call.dot
index be01df25..59e8467e 100644
--- a/doc/shape-shape-collision-call.dot
+++ b/doc/shape-shape-collision-call.dot
@@ -9,11 +9,11 @@ digraph CD {
"void distance(DistanceTraversalNodeBase* node,\nBVHFrontList* front_list, int qsize)" [shape = box]
"void distanceRecurse(DistanceTraversalNodeBase* node,\nint b1, int b2, BVHFrontList* front_list)" [shape = box]
"void ShapeDistanceTraversalNode::leafTesting(int, int) const\ntraversal/traversal_node_shapes.h" [shape = box]
- "template<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 S1, typename S2> bool GJKSolver_indep::shapeDistance\n(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2,\nCoalScalar* distance, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h" [shape = box]
"template<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,\nCoalScalar* 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, Vec3s* p1, Vec3s* p2) const\nnarrowphase/narrowphase.h"
}
diff --git a/include/coal/BV/AABB.h b/include/coal/BV/AABB.h
index 22ae0ab0..8d46a532 100644
--- a/include/coal/BV/AABB.h
+++ b/include/coal/BV/AABB.h
@@ -55,33 +55,33 @@ class Halfspace;
class COAL_DLLAPI AABB {
public:
/// @brief The min point in the AABB
- Vec3f min_;
+ Vec3s min_;
/// @brief The max point in the AABB
- Vec3f max_;
+ Vec3s max_;
/// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf)
AABB();
/// @brief Creating an AABB at position v with zero size
- AABB(const Vec3f& v) : min_(v), max_(v) {}
+ AABB(const Vec3s& v) : min_(v), max_(v) {}
/// @brief Creating an AABB with two endpoints a and b
- AABB(const Vec3f& a, const Vec3f& b)
+ AABB(const Vec3s& a, const Vec3s& b)
: min_(a.cwiseMin(b)), max_(a.cwiseMax(b)) {}
/// @brief Creating an AABB centered as core and is of half-dimension delta
- AABB(const AABB& core, const Vec3f& delta)
+ AABB(const AABB& core, const Vec3s& delta)
: min_(core.min_ - delta), max_(core.max_ + delta) {}
/// @brief Creating an AABB contains three points
- AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c)
+ AABB(const Vec3s& a, const Vec3s& b, const Vec3s& c)
: min_(a.cwiseMin(b).cwiseMin(c)), max_(a.cwiseMax(b).cwiseMax(c)) {}
AABB(const AABB& other) = default;
AABB& operator=(const AABB& other) = default;
- AABB& update(const Vec3f& a, const Vec3f& b) {
+ AABB& update(const Vec3s& a, const Vec3s& b) {
min_ = a.cwiseMin(b);
max_ = a.cwiseMax(b);
return *this;
@@ -99,7 +99,7 @@ class COAL_DLLAPI AABB {
/// @{
/// @brief Check whether the AABB contains a point
- inline bool contain(const Vec3f& p) const {
+ inline bool contain(const Vec3s& p) const {
if (p[0] < min_[0] || p[0] > max_[0]) return false;
if (p[1] < min_[1] || p[1] > max_[1]) return false;
if (p[2] < min_[2] || p[2] > max_[2]) return false;
@@ -135,10 +135,10 @@ class COAL_DLLAPI AABB {
/// @brief Distance between two AABBs; P and Q, should not be NULL, return the
/// nearest points
- CoalScalar distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
+ CoalScalar distance(const AABB& other, Vec3s* P, Vec3s* Q) const;
/// @brief Merge the AABB and a point
- inline AABB& operator+=(const Vec3f& p) {
+ inline AABB& operator+=(const Vec3s& p) {
min_ = min_.cwiseMin(p);
max_ = max_.cwiseMax(p);
return *this;
@@ -161,7 +161,7 @@ class COAL_DLLAPI AABB {
inline CoalScalar size() const { return (max_ - min_).squaredNorm(); }
/// @brief Center of the AABB
- inline Vec3f center() const { return (min_ + max_) * 0.5; }
+ inline Vec3s center() const { return (min_ + max_) * 0.5; }
/// @brief Width of the AABB
inline CoalScalar width() const { return max_[0] - min_[0]; }
@@ -205,7 +205,7 @@ class COAL_DLLAPI AABB {
/// @brief expand the half size of the AABB by delta, and keep the center
/// unchanged.
- inline AABB& expand(const Vec3f& delta) {
+ inline AABB& expand(const Vec3s& delta) {
min_ -= delta;
max_ += delta;
return *this;
@@ -230,16 +230,16 @@ class COAL_DLLAPI AABB {
};
/// @brief translate the center of AABB by t
-static inline AABB translate(const AABB& aabb, const Vec3f& t) {
+static inline AABB translate(const AABB& aabb, const Vec3s& t) {
AABB res(aabb);
res.min_ += t;
res.max_ += t;
return res;
}
-static inline AABB rotate(const AABB& aabb, const Matrix3f& R) {
+static inline AABB rotate(const AABB& aabb, const Matrix3s& R) {
AABB res(R * aabb.min_);
- Vec3f corner(aabb.min_);
+ Vec3s corner(aabb.min_);
const Eigen::DenseIndex bit[3] = {1, 2, 4};
for (Eigen::DenseIndex ic = 1; ic < 8;
++ic) { // ic = 0 corresponds to aabb.min_. Skip it.
@@ -253,12 +253,12 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& R) {
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
/// and b2 is in identity.
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
const AABB& b2);
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0)
/// and b2 is in identity.
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound);
} // namespace coal
diff --git a/include/coal/BV/BV.h b/include/coal/BV/BV.h
index d6c6a7e0..aba2d1d4 100644
--- a/include/coal/BV/BV.h
+++ b/include/coal/BV/BV.h
@@ -64,11 +64,11 @@ struct Converter {
template <>
struct Converter<AABB, AABB> {
static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) {
- const Vec3f& center = bv1.center();
+ const Vec3s& center = bv1.center();
CoalScalar r = (bv1.max_ - bv1.min_).norm() * 0.5;
- const Vec3f center2 = tf1.transform(center);
- bv2.min_ = center2 - Vec3f::Constant(r);
- bv2.max_ = center2 + Vec3f::Constant(r);
+ const Vec3s center2 = tf1.transform(center);
+ bv2.min_ = center2 - Vec3s::Constant(r);
+ bv2.max_ = center2 + Vec3s::Constant(r);
}
static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
@@ -114,14 +114,14 @@ struct Converter<OBBRSS, OBB> {
template <>
struct Converter<RSS, OBB> {
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) {
- bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
+ bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
static void convert(const RSS& bv1, OBB& bv2) {
- bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
+ bv2.extent = Vec3s(bv1.length[0] * 0.5 + bv1.radius,
bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To = bv1.Tr;
bv2.axes = bv1.axes;
@@ -131,18 +131,18 @@ struct Converter<RSS, OBB> {
template <typename BV1>
struct Converter<BV1, AABB> {
static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) {
- const Vec3f& center = bv1.center();
- CoalScalar r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
- const Vec3f center2 = tf1.transform(center);
- bv2.min_ = center2 - Vec3f::Constant(r);
- bv2.max_ = center2 + Vec3f::Constant(r);
+ const Vec3s& center = bv1.center();
+ CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
+ const Vec3s center2 = tf1.transform(center);
+ bv2.min_ = center2 - Vec3s::Constant(r);
+ bv2.max_ = center2 + Vec3s::Constant(r);
}
static void convert(const BV1& bv1, AABB& bv2) {
- const Vec3f& center = bv1.center();
- CoalScalar r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
- bv2.min_ = center - Vec3f::Constant(r);
- bv2.max_ = center + Vec3f::Constant(r);
+ const Vec3s& center = bv1.center();
+ CoalScalar r = Vec3s(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
+ bv2.min_ = center - Vec3s::Constant(r);
+ bv2.max_ = center + Vec3s::Constant(r);
}
};
@@ -233,12 +233,12 @@ struct Converter<AABB, RSS> {
}
}
- const Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
+ const Vec3s extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.radius = extent[id[2]];
bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
- const Matrix3f& R = tf1.getRotation();
+ const Matrix3s& R = tf1.getRotation();
const bool left_hand = (id[0] == (id[1] + 1) % 3);
if (left_hand)
bv2.axes.col(0) = -R.col(id[0]);
diff --git a/include/coal/BV/BV_node.h b/include/coal/BV/BV_node.h
index 43f33863..b858308c 100644
--- a/include/coal/BV/BV_node.h
+++ b/include/coal/BV/BV_node.h
@@ -127,17 +127,17 @@ struct COAL_DLLAPI BVNode : public BVNodeBase {
/// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
/// the underlying BV supports distance, return the nearest points.
- CoalScalar distance(const BVNode& other, Vec3f* P1 = NULL,
- Vec3f* P2 = NULL) const {
+ CoalScalar distance(const BVNode& other, Vec3s* P1 = NULL,
+ Vec3s* P2 = NULL) const {
return bv.distance(other.bv, P1, P2);
}
/// @brief Access to the center of the BV
- Vec3f getCenter() const { return bv.center(); }
+ Vec3s getCenter() const { return bv.center(); }
/// @brief Access to the orientation of the BV
- const Matrix3f& getOrientation() const {
- static const Matrix3f id3 = Matrix3f::Identity();
+ const Matrix3s& getOrientation() const {
+ static const Matrix3s id3 = Matrix3s::Identity();
return id3;
}
@@ -147,17 +147,17 @@ struct COAL_DLLAPI BVNode : public BVNodeBase {
};
template <>
-inline const Matrix3f& BVNode<OBB>::getOrientation() const {
+inline const Matrix3s& BVNode<OBB>::getOrientation() const {
return bv.axes;
}
template <>
-inline const Matrix3f& BVNode<RSS>::getOrientation() const {
+inline const Matrix3s& BVNode<RSS>::getOrientation() const {
return bv.axes;
}
template <>
-inline const Matrix3f& BVNode<OBBRSS>::getOrientation() const {
+inline const Matrix3s& BVNode<OBBRSS>::getOrientation() const {
return bv.obb.axes;
}
diff --git a/include/coal/BV/OBB.h b/include/coal/BV/OBB.h
index b16f3d01..8255f023 100644
--- a/include/coal/BV/OBB.h
+++ b/include/coal/BV/OBB.h
@@ -56,15 +56,15 @@ struct COAL_DLLAPI OBB {
/// assume that axis[0] corresponds to the axis with the longest box edge,
/// axis[1] corresponds to the shorter one and axis[2] corresponds to the
/// shortest one.
- Matrix3f axes;
+ Matrix3s axes;
/// @brief Center of OBB
- Vec3f To;
+ Vec3s To;
/// @brief Half dimensions of OBB
- Vec3f extent;
+ Vec3s extent;
- OBB() : axes(Matrix3f::Zero()), To(Vec3f::Zero()), extent(Vec3f::Zero()) {}
+ OBB() : axes(Matrix3s::Zero()), To(Vec3s::Zero()), extent(Vec3s::Zero()) {}
/// @brief Equality operator
bool operator==(const OBB& other) const {
@@ -75,7 +75,7 @@ struct COAL_DLLAPI OBB {
bool operator!=(const OBB& other) const { return !(*this == other); }
/// @brief Check whether the OBB contains a point.
- bool contain(const Vec3f& p) const;
+ bool contain(const Vec3s& p) const;
/// Check collision between two OBB
/// @return true if collision happens.
@@ -89,11 +89,11 @@ struct COAL_DLLAPI OBB {
CoalScalar& sqrDistLowerBound) const;
/// @brief Distance between two OBBs, not implemented.
- CoalScalar distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+ CoalScalar distance(const OBB& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
/// @brief A simple way to merge the OBB and a point (the result is not
/// compact).
- OBB& operator+=(const Vec3f& p);
+ OBB& operator+=(const Vec3s& p);
/// @brief Merge the OBB and another OBB (the result is not compact).
OBB& operator+=(const OBB& other) {
@@ -109,7 +109,7 @@ struct COAL_DLLAPI OBB {
inline CoalScalar size() const { return extent.squaredNorm(); }
/// @brief Center of the OBB
- inline const Vec3f& center() const { return To; }
+ inline const Vec3s& center() const { return To; }
/// @brief Width of the OBB.
inline CoalScalar width() const { return 2 * extent[0]; }
@@ -125,16 +125,16 @@ struct COAL_DLLAPI OBB {
};
/// @brief Translate the OBB bv
-COAL_DLLAPI OBB translate(const OBB& bv, const Vec3f& t);
+COAL_DLLAPI OBB translate(const OBB& bv, const Vec3s& t);
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
const OBB& b2);
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
const OBB& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound);
@@ -143,8 +143,8 @@ COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
/// @param a half dimensions of first box,
/// @param b half dimensions of second box.
/// The second box is in identity configuration.
-COAL_DLLAPI bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
- const Vec3f& b);
+COAL_DLLAPI bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b);
} // namespace coal
#endif
diff --git a/include/coal/BV/OBBRSS.h b/include/coal/BV/OBBRSS.h
index 003b396e..97ea6bb1 100644
--- a/include/coal/BV/OBBRSS.h
+++ b/include/coal/BV/OBBRSS.h
@@ -68,7 +68,7 @@ struct COAL_DLLAPI OBBRSS {
bool operator!=(const OBBRSS& other) const { return !(*this == other); }
/// @brief Check whether the OBBRSS contains a point
- inline bool contain(const Vec3f& p) const { return obb.contain(p); }
+ inline bool contain(const Vec3s& p) const { return obb.contain(p); }
/// @brief Check collision between two OBBRSS
bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); }
@@ -83,13 +83,13 @@ struct COAL_DLLAPI OBBRSS {
/// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the
/// nearest points
- CoalScalar distance(const OBBRSS& other, Vec3f* P = NULL,
- Vec3f* Q = NULL) const {
+ CoalScalar distance(const OBBRSS& other, Vec3s* P = NULL,
+ Vec3s* Q = NULL) const {
return rss.distance(other.rss, P, Q);
}
/// @brief Merge the OBBRSS and a point
- OBBRSS& operator+=(const Vec3f& p) {
+ OBBRSS& operator+=(const Vec3s& p) {
obb += p;
rss += p;
return *this;
@@ -113,7 +113,7 @@ struct COAL_DLLAPI OBBRSS {
inline CoalScalar size() const { return obb.size(); }
/// @brief Center of the OBBRSS
- inline const Vec3f& center() const { return obb.center(); }
+ inline const Vec3s& center() const { return obb.center(); }
/// @brief Width of the OBRSS
inline CoalScalar width() const { return obb.width(); }
@@ -130,7 +130,7 @@ struct COAL_DLLAPI OBBRSS {
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0)
/// and b2 is in indentity
-inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
+inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
const OBBRSS& b2) {
return overlap(R0, T0, b1.obb, b2.obb);
}
@@ -141,7 +141,7 @@ inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
/// @param b2 second OBBRSS in identity position
/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do
/// not overlap.
-inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
+inline bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBBRSS& b1,
const OBBRSS& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) {
return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
@@ -149,9 +149,9 @@ inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0)
/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points
-inline CoalScalar distance(const Matrix3f& R0, const Vec3f& T0,
- const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL,
- Vec3f* Q = NULL) {
+inline CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+ const OBBRSS& b1, const OBBRSS& b2, Vec3s* P = NULL,
+ Vec3s* Q = NULL) {
return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
diff --git a/include/coal/BV/RSS.h b/include/coal/BV/RSS.h
index 8d9cfba5..8bf2bd07 100644
--- a/include/coal/BV/RSS.h
+++ b/include/coal/BV/RSS.h
@@ -58,10 +58,10 @@ struct COAL_DLLAPI RSS {
/// assume that axis[0] corresponds to the axis with the longest length,
/// axis[1] corresponds to the shorter one and axis[2] corresponds to the
/// shortest one.
- Matrix3f axes;
+ Matrix3s axes;
/// @brief Origin of the rectangle in RSS
- Vec3f Tr;
+ Vec3s Tr;
/// @brief Side lengths of rectangle
CoalScalar length[2];
@@ -70,7 +70,7 @@ struct COAL_DLLAPI RSS {
CoalScalar radius;
/// Â @brief Default constructor with default values
- RSS() : axes(Matrix3f::Zero()), Tr(Vec3f::Zero()), radius(-1) {
+ RSS() : axes(Matrix3s::Zero()), Tr(Vec3s::Zero()), radius(-1) {
length[0] = 0;
length[1] = 0;
}
@@ -86,7 +86,7 @@ struct COAL_DLLAPI RSS {
bool operator!=(const RSS& other) const { return !(*this == other); }
/// @brief Check whether the RSS contains a point
- bool contain(const Vec3f& p) const;
+ bool contain(const Vec3s& p) const;
/// @brief Check collision between two RSS
bool overlap(const RSS& other) const;
@@ -100,11 +100,11 @@ struct COAL_DLLAPI RSS {
/// @brief the distance between two RSS; P and Q, if not NULL, return the
/// nearest points
- CoalScalar distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+ CoalScalar distance(const RSS& other, Vec3s* P = NULL, Vec3s* Q = NULL) const;
/// @brief A simple way to merge the RSS and a point, not compact.
/// @todo This function may have some bug.
- RSS& operator+=(const Vec3f& p);
+ RSS& operator+=(const Vec3s& p);
/// @brief Merge the RSS and another RSS
inline RSS& operator+=(const RSS& other) {
@@ -122,7 +122,7 @@ struct COAL_DLLAPI RSS {
}
/// @brief The RSS center
- inline const Vec3f& center() const { return Tr; }
+ inline const Vec3s& center() const { return Tr; }
/// @brief Width of the RSS
inline CoalScalar width() const { return length[0] + 2 * radius; }
@@ -153,18 +153,18 @@ struct COAL_DLLAPI RSS {
/// not the RSS. But the direction P - Q is the correct direction for cloest
/// points Notice that P and Q are both in the local frame of the first RSS (not
/// global frame and not even the local frame of object 1)
-COAL_DLLAPI CoalScalar distance(const Matrix3f& R0, const Vec3f& T0,
- const RSS& b1, const RSS& b2, Vec3f* P = NULL,
- Vec3f* Q = NULL);
+COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+ const RSS& b1, const RSS& b2, Vec3s* P = NULL,
+ Vec3s* Q = NULL);
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
const RSS& b2);
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
/// b2 is in identity.
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
const RSS& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound);
diff --git a/include/coal/BV/kDOP.h b/include/coal/BV/kDOP.h
index 4803cd4b..cb837752 100644
--- a/include/coal/BV/kDOP.h
+++ b/include/coal/BV/kDOP.h
@@ -100,10 +100,10 @@ class COAL_DLLAPI KDOP {
KDOP();
/// @brief Creating kDOP containing only one point
- KDOP(const Vec3f& v);
+ KDOP(const Vec3s& v);
/// @brief Creating kDOP containing two points
- KDOP(const Vec3f& a, const Vec3f& b);
+ KDOP(const Vec3s& a, const Vec3s& b);
/// @brief Equality operator
bool operator==(const KDOP& other) const {
@@ -126,11 +126,11 @@ class COAL_DLLAPI KDOP {
CoalScalar& sqrDistLowerBound) const;
/// @brief The distance between two KDOP<N>. Not implemented.
- CoalScalar distance(const KDOP<N>& other, Vec3f* P = NULL,
- Vec3f* Q = NULL) const;
+ CoalScalar distance(const KDOP<N>& other, Vec3s* P = NULL,
+ Vec3s* Q = NULL) const;
/// @brief Merge the point and the KDOP
- KDOP<N>& operator+=(const Vec3f& p);
+ KDOP<N>& operator+=(const Vec3s& p);
/// @brief Merge two KDOPs
KDOP<N>& operator+=(const KDOP<N>& other);
@@ -144,7 +144,7 @@ class COAL_DLLAPI KDOP {
}
/// @brief The (AABB) center
- inline Vec3f center() const {
+ inline Vec3s center() const {
return (dist_.template head<3>() + dist_.template segment<3>(N / 2)) / 2;
}
@@ -165,17 +165,17 @@ class COAL_DLLAPI KDOP {
inline CoalScalar& dist(short i) { return dist_[i]; }
//// @brief Check whether one point is inside the KDOP
- bool inside(const Vec3f& p) const;
+ bool inside(const Vec3s& p) const;
};
template <short N>
-bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
+bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
const KDOP<N>& /*b2*/) {
COAL_THROW_PRETTY("not implemented", std::logic_error);
}
template <short N>
-bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
+bool overlap(const Matrix3s& /*R0*/, const Vec3s& /*T0*/, const KDOP<N>& /*b1*/,
const KDOP<N>& /*b2*/, const CollisionRequest& /*request*/,
CoalScalar& /*sqrDistLowerBound*/) {
COAL_THROW_PRETTY("not implemented", std::logic_error);
@@ -183,7 +183,7 @@ bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP<N>& /*b1*/,
/// @brief translate the KDOP BV
template <short N>
-COAL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
+COAL_DLLAPI KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t);
} // namespace coal
diff --git a/include/coal/BV/kIOS.h b/include/coal/BV/kIOS.h
index c663bd2c..83277ab6 100644
--- a/include/coal/BV/kIOS.h
+++ b/include/coal/BV/kIOS.h
@@ -54,7 +54,7 @@ class COAL_DLLAPI kIOS {
struct COAL_DLLAPI kIOS_Sphere {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- Vec3f o;
+ Vec3s o;
CoalScalar r;
bool operator==(const kIOS_Sphere& other) const {
@@ -69,7 +69,7 @@ class COAL_DLLAPI kIOS {
/// @brief generate one sphere enclosing two spheres
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0,
const kIOS_Sphere& s1) {
- Vec3f d = s1.o - s0.o;
+ Vec3s d = s1.o - s0.o;
CoalScalar dist2 = d.squaredNorm();
CoalScalar diff_r = s1.r - s0.r;
@@ -122,7 +122,7 @@ class COAL_DLLAPI kIOS {
OBB obb;
/// @brief Check whether the kIOS contains a point
- bool contain(const Vec3f& p) const;
+ bool contain(const Vec3s& p) const;
/// @brief Check collision between two kIOS
bool overlap(const kIOS& other) const;
@@ -132,11 +132,11 @@ class COAL_DLLAPI kIOS {
CoalScalar& sqrDistLowerBound) const;
/// @brief The distance between two kIOS
- CoalScalar distance(const kIOS& other, Vec3f* P = NULL,
- Vec3f* Q = NULL) const;
+ CoalScalar distance(const kIOS& other, Vec3s* P = NULL,
+ Vec3s* Q = NULL) const;
/// @brief A simple way to merge the kIOS and a point
- kIOS& operator+=(const Vec3f& p);
+ kIOS& operator+=(const Vec3s& p);
/// @brief Merge the kIOS and another kIOS
kIOS& operator+=(const kIOS& other) {
@@ -151,7 +151,7 @@ class COAL_DLLAPI kIOS {
CoalScalar size() const;
/// @brief Center of the kIOS
- const Vec3f& center() const { return spheres[0].o; }
+ const Vec3s& center() const { return spheres[0].o; }
/// @brief Width of the kIOS
CoalScalar width() const;
@@ -167,26 +167,26 @@ class COAL_DLLAPI kIOS {
};
/// @brief Translate the kIOS BV
-COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3f& t);
+COAL_DLLAPI kIOS translate(const kIOS& bv, const Vec3s& t);
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
/// and b2 is in identity.
/// @todo Not efficient
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
const kIOS& b2);
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0)
/// and b2 is in identity.
/// @todo Not efficient
-COAL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
+COAL_DLLAPI bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
const kIOS& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound);
/// @brief Approximate distance between two kIOS bounding volumes
/// @todo P and Q is not returned, need implementation
-COAL_DLLAPI CoalScalar distance(const Matrix3f& R0, const Vec3f& T0,
- const kIOS& b1, const kIOS& b2, Vec3f* P = NULL,
- Vec3f* Q = NULL);
+COAL_DLLAPI CoalScalar distance(const Matrix3s& R0, const Vec3s& T0,
+ const kIOS& b1, const kIOS& b2, Vec3s* P = NULL,
+ Vec3s* Q = NULL);
} // namespace coal
diff --git a/include/coal/BVH/BVH_model.h b/include/coal/BVH/BVH_model.h
index de80f330..722578f4 100644
--- a/include/coal/BVH/BVH_model.h
+++ b/include/coal/BVH/BVH_model.h
@@ -65,13 +65,13 @@ class BVSplitter;
class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
public:
/// @brief Geometry point data
- std::shared_ptr<std::vector<Vec3f>> vertices;
+ std::shared_ptr<std::vector<Vec3s>> vertices;
/// @brief Geometry triangle index data, will be NULL for point clouds
std::shared_ptr<std::vector<Triangle>> tri_indices;
/// @brief Geometry point data in previous frame
- std::shared_ptr<std::vector<Vec3f>> prev_vertices;
+ std::shared_ptr<std::vector<Vec3s>> prev_vertices;
/// @brief Number of triangles
unsigned int num_tris;
@@ -114,23 +114,23 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
int beginModel(unsigned int num_tris = 0, unsigned int num_vertices = 0);
/// @brief Add one point in the new BVH model
- int addVertex(const Vec3f& p);
+ int addVertex(const Vec3s& p);
/// @brief Add points in the new BVH model
- int addVertices(const Matrixx3f& points);
+ int addVertices(const MatrixX3s& points);
/// @brief Add triangles in the new BVH model
int addTriangles(const Matrixx3i& triangles);
/// @brief Add one triangle in the new BVH model
- int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
+ int addTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
/// @brief Add a set of triangles in the new BVH model
- int addSubModel(const std::vector<Vec3f>& ps,
+ int addSubModel(const std::vector<Vec3s>& ps,
const std::vector<Triangle>& ts);
/// @brief Add a set of points in the new BVH model
- int addSubModel(const std::vector<Vec3f>& ps);
+ int addSubModel(const std::vector<Vec3s>& ps);
/// @brief End BVH model construction, will build the bounding volume
/// hierarchy
@@ -141,13 +141,13 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
int beginReplaceModel();
/// @brief Replace one point in the old BVH model
- int replaceVertex(const Vec3f& p);
+ int replaceVertex(const Vec3s& p);
/// @brief Replace one triangle in the old BVH model
- int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
+ int replaceTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
/// @brief Replace a set of points in the old BVH model
- int replaceSubModel(const std::vector<Vec3f>& ps);
+ int replaceSubModel(const std::vector<Vec3s>& ps);
/// @brief End BVH model replacement, will also refit or rebuild the bounding
/// volume hierarchy
@@ -159,13 +159,13 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
int beginUpdateModel();
/// @brief Update one point in the old BVH model
- int updateVertex(const Vec3f& p);
+ int updateVertex(const Vec3s& p);
/// @brief Update one triangle in the old BVH model
- int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3);
+ int updateTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3);
/// @brief Update a set of points in the old BVH model
- int updateSubModel(const std::vector<Vec3f>& ps);
+ int updateSubModel(const std::vector<Vec3s>& ps);
/// @brief End BVH model update, will also refit or rebuild the bounding
/// volume hierarchy
@@ -198,16 +198,16 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
/// save one matrix transformation.
virtual void makeParentRelative() = 0;
- Vec3f computeCOM() const {
+ Vec3s computeCOM() const {
CoalScalar vol = 0;
- Vec3f com(0, 0, 0);
+ Vec3s com(0, 0, 0);
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"vertices."
<< std::endl;
return com;
}
- const std::vector<Vec3f>& vertices_ = *vertices;
+ const std::vector<Vec3s>& vertices_ = *vertices;
if (!(tri_indices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"triangles."
@@ -236,7 +236,7 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
<< std::endl;
return vol;
}
- const std::vector<Vec3f>& vertices_ = *vertices;
+ const std::vector<Vec3s>& vertices_ = *vertices;
if (!(tri_indices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"triangles."
@@ -254,10 +254,10 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
return vol / 6;
}
- Matrix3f computeMomentofInertia() const {
- Matrix3f C = Matrix3f::Zero();
+ Matrix3s computeMomentofInertia() const {
+ Matrix3s C = Matrix3s::Zero();
- Matrix3f C_canonical;
+ Matrix3s C_canonical;
C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0,
1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0;
@@ -267,7 +267,7 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
<< std::endl;
return C;
}
- const std::vector<Vec3f>& vertices_ = *vertices;
+ const std::vector<Vec3s>& vertices_ = *vertices;
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
"not contain vertices."
@@ -277,15 +277,15 @@ class COAL_DLLAPI BVHModelBase : public CollisionGeometry {
const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
const Triangle& tri = tri_indices_[i];
- const Vec3f& v1 = vertices_[tri[0]];
- const Vec3f& v2 = vertices_[tri[1]];
- const Vec3f& v3 = vertices_[tri[2]];
- Matrix3f A;
+ const Vec3s& v1 = vertices_[tri[0]];
+ const Vec3s& v2 = vertices_[tri[1]];
+ const Vec3s& v3 = vertices_[tri[2]];
+ Matrix3s A;
A << v1.transpose(), v2.transpose(), v3.transpose();
C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
- return C.trace() * Matrix3f::Identity() - C;
+ return C.trace() * Matrix3s::Identity() - C;
}
protected:
@@ -368,8 +368,8 @@ class COAL_DLLAPI BVHModel : public BVHModelBase {
/// transform related to its parent BV node. When traversing the BVH, this can
/// save one matrix transformation.
void makeParentRelative() {
- Matrix3f I(Matrix3f::Identity());
- makeParentRelativeRecurse(0, I, Vec3f::Zero());
+ Matrix3s I(Matrix3s::Identity());
+ makeParentRelativeRecurse(0, I, Vec3s::Zero());
}
protected:
@@ -409,8 +409,8 @@ class COAL_DLLAPI BVHModel : public BVHModelBase {
/// @ recursively compute each bv's transform related to its parent. For
/// default BV, only the translation works. For oriented BV (OBB, RSS,
/// OBBRSS), special implementation is provided.
- void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
- const Vec3f& parent_c) {
+ void makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+ const Vec3s& parent_c) {
bv_node_vector_t& bvs_ = *bvs;
if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
@@ -497,17 +497,17 @@ class COAL_DLLAPI BVHModel : public BVHModelBase {
/// @}
template <>
-void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
- const Vec3f& parent_c);
+void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+ const Vec3s& parent_c);
template <>
-void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
- const Vec3f& parent_c);
+void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+ const Vec3s& parent_c);
template <>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
- Matrix3f& parent_axes,
- const Vec3f& parent_c);
+ Matrix3s& parent_axes,
+ const Vec3s& parent_c);
/// @brief Specialization of getNodeType() for BVHModel with different BV types
template <>
diff --git a/include/coal/BVH/BVH_utility.h b/include/coal/BVH/BVH_utility.h
index a3687ab4..ef24f884 100644
--- a/include/coal/BVH/BVH_utility.h
+++ b/include/coal/BVH/BVH_utility.h
@@ -82,33 +82,33 @@ COAL_DLLAPI BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
/// @brief Compute the covariance matrix for a set or subset of points. if ts =
/// null, then indices refer to points directly; otherwise refer to triangles
-COAL_DLLAPI void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+COAL_DLLAPI void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
- Matrix3f& M);
+ Matrix3s& M);
/// @brief Compute the RSS bounding volume parameters: radius, rectangle size
/// and the origin, given the BV axises.
COAL_DLLAPI void getRadiusAndOriginAndRectangleSize(
- Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, unsigned int n,
- const Matrix3f& axes, Vec3f& origin, CoalScalar l[2], CoalScalar& r);
+ Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, unsigned int n,
+ const Matrix3s& axes, Vec3s& origin, CoalScalar l[2], CoalScalar& r);
/// @brief Compute the bounding volume extent and center for a set or subset of
/// points, given the BV axises.
-COAL_DLLAPI void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+COAL_DLLAPI void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
- Matrix3f& axes, Vec3f& center,
- Vec3f& extent);
+ Matrix3s& axes, Vec3s& center,
+ Vec3s& extent);
/// @brief Compute the center and radius for a triangle's circumcircle
-COAL_DLLAPI void circumCircleComputation(const Vec3f& a, const Vec3f& b,
- const Vec3f& c, Vec3f& center,
+COAL_DLLAPI void circumCircleComputation(const Vec3s& a, const Vec3s& b,
+ const Vec3s& c, Vec3s& center,
CoalScalar& radius);
/// @brief Compute the maximum distance from a given center point to a point
/// cloud
-COAL_DLLAPI CoalScalar maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+COAL_DLLAPI CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
- const Vec3f& query);
+ const Vec3s& query);
} // namespace coal
diff --git a/include/coal/broadphase/broadphase_SaP.h b/include/coal/broadphase/broadphase_SaP.h
index 28c06e57..118ee65a 100644
--- a/include/coal/broadphase/broadphase_SaP.h
+++ b/include/coal/broadphase/broadphase_SaP.h
@@ -146,10 +146,10 @@ class COAL_DLLAPI SaPCollisionManager : public BroadPhaseCollisionManager {
EndPoint* next[3];
/// @brief get the value of the end point
- const Vec3f& getVal() const;
+ const Vec3s& getVal() const;
/// @brief set the value of the end point
- Vec3f& getVal();
+ Vec3s& getVal();
CoalScalar getVal(int i) const;
diff --git a/include/coal/broadphase/broadphase_spatialhash-inl.h b/include/coal/broadphase/broadphase_spatialhash-inl.h
index 1509bcd7..9f155a4e 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(
- CoalScalar cell_size, const Vec3f& scene_min, const Vec3f& scene_max,
+ CoalScalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max,
unsigned int default_table_size)
: scene_limit(AABB(scene_min, scene_max)),
hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
@@ -313,7 +313,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(
auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
auto aabb = obj->getAABB();
if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
}
@@ -354,7 +354,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(
break;
} else {
if (min_dist < old_min_distance) {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb = AABB(obj->getAABB(), min_dist_delta);
status = 0;
} else {
@@ -499,7 +499,7 @@ size_t SpatialHashingCollisionManager<HashTable>::size() const {
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::computeBound(
- std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u) {
+ std::vector<CollisionObject*>& objs, Vec3s& l, Vec3s& u) {
AABB bound;
for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
diff --git a/include/coal/broadphase/broadphase_spatialhash.h b/include/coal/broadphase/broadphase_spatialhash.h
index e2828a53..10c4830c 100644
--- a/include/coal/broadphase/broadphase_spatialhash.h
+++ b/include/coal/broadphase/broadphase_spatialhash.h
@@ -56,8 +56,8 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager {
typedef BroadPhaseCollisionManager Base;
using Base::getObjects;
- SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3f& scene_min,
- const Vec3f& scene_max,
+ SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3s& scene_min,
+ const Vec3s& scene_max,
unsigned int default_table_size = 1000);
~SpatialHashingCollisionManager();
@@ -117,8 +117,8 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager {
size_t size() const;
/// @brief compute the bound for the environent
- static void computeBound(std::vector<CollisionObject*>& objs, Vec3f& l,
- Vec3f& u);
+ static void computeBound(std::vector<CollisionObject*>& objs, Vec3s& l,
+ Vec3s& u);
protected:
/// @brief perform collision test between one object and all the objects
diff --git a/include/coal/broadphase/detail/hierarchy_tree-inl.h b/include/coal/broadphase/detail/hierarchy_tree-inl.h
index b771e77e..af29cf6a 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*/, CoalScalar /*margin*/) {
+ const Vec3s& /*vel*/, CoalScalar /*margin*/) {
if (leaf->bv.contain(bv)) return false;
tree.update_(leaf, bv);
return true;
@@ -167,7 +167,7 @@ struct UpdateImpl {
static bool run(const HierarchyTree<BV>& tree,
typename HierarchyTree<BV>::Node* leaf, const BV& bv,
- const Vec3f& /*vel*/) {
+ const Vec3s& /*vel*/) {
if (leaf->bv.contain(bv)) return false;
tree.update_(leaf, bv);
return true;
@@ -176,14 +176,14 @@ struct UpdateImpl {
//==============================================================================
template <typename BV>
-bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel,
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel,
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) {
+bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel) {
return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel);
}
@@ -408,7 +408,7 @@ typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1(
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (num_leaves > bu_threshold) {
- Vec3f split_p = (*lbeg)->bv.center();
+ Vec3s split_p = (*lbeg)->bv.center();
BV vol = (*lbeg)->bv;
NodeVecIterator it;
for (it = lbeg + 1; it < lend; ++it) {
@@ -420,7 +420,7 @@ typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1(
long bestmidp = num_leaves;
int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
for (it = lbeg; it < lend; ++it) {
- Vec3f x = (*it)->bv.center() - split_p;
+ Vec3s x = (*it)->bv.center() - split_p;
for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
}
@@ -970,9 +970,9 @@ struct SelectImpl<S, AABB> {
const AABB& bv = node.bv;
const AABB& bv1 = node1.bv;
const AABB& bv2 = node2.bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
+ Vec3s v = bv.min_ + bv.max_;
+ Vec3s v1 = v - (bv1.min_ + bv1.max_);
+ Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
@@ -983,9 +983,9 @@ struct SelectImpl<S, AABB> {
const AABB& bv = query;
const AABB& bv1 = node1.bv;
const AABB& bv2 = node2.bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
+ Vec3s v = bv.min_ + bv.max_;
+ Vec3s v1 = v - (bv1.min_ + bv1.max_);
+ Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
diff --git a/include/coal/broadphase/detail/hierarchy_tree.h b/include/coal/broadphase/detail/hierarchy_tree.h
index 67a6b7ec..6b5fe526 100644
--- a/include/coal/broadphase/detail/hierarchy_tree.h
+++ b/include/coal/broadphase/detail/hierarchy_tree.h
@@ -100,10 +100,10 @@ class HierarchyTree {
bool update(Node* leaf, const BV& bv);
/// @brief update one leaf's bounding volume, with prediction
- bool update(Node* leaf, const BV& bv, const Vec3f& vel, CoalScalar margin);
+ bool update(Node* leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
/// @brief update one leaf's bounding volume, with prediction
- bool update(Node* leaf, const BV& bv, const Vec3f& vel);
+ bool update(Node* leaf, const BV& bv, const Vec3s& vel);
/// @brief get the max height of the tree
size_t getMaxHeight() const;
diff --git a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h
index 9ac67f89..fa902e67 100644
--- a/include/coal/broadphase/detail/hierarchy_tree_array-inl.h
+++ b/include/coal/broadphase/detail/hierarchy_tree_array-inl.h
@@ -295,7 +295,7 @@ bool HierarchyTree<BV>::update(size_t leaf, const BV& bv) {
//==============================================================================
template <typename BV>
-bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel,
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel,
CoalScalar margin) {
COAL_UNUSED_VARIABLE(bv);
COAL_UNUSED_VARIABLE(vel);
@@ -308,7 +308,7 @@ bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel,
//==============================================================================
template <typename BV>
-bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel) {
+bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel) {
COAL_UNUSED_VARIABLE(vel);
if (nodes[leaf].bv.contain(bv)) return false;
@@ -556,7 +556,7 @@ size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (num_leaves > bu_threshold) {
- Vec3f split_p = nodes[*lbeg].bv.center();
+ Vec3s split_p = nodes[*lbeg].bv.center();
BV vol = nodes[*lbeg].bv;
for (size_t* i = lbeg + 1; i < lend; ++i) {
split_p += nodes[*i].bv.center();
@@ -567,7 +567,7 @@ size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) {
int bestmidp = (int)num_leaves;
int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
for (size_t* i = lbeg; i < lend; ++i) {
- Vec3f x = nodes[*i].bv.center() - split_p;
+ Vec3s x = nodes[*i].bv.center() - split_p;
for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
}
@@ -949,9 +949,9 @@ struct SelectImpl<S, AABB> {
const AABB& bv = nodes[query].bv;
const AABB& bv1 = nodes[node1].bv;
const AABB& bv2 = nodes[node2].bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
+ Vec3s v = bv.min_ + bv.max_;
+ Vec3s v1 = v - (bv1.min_ + bv1.max_);
+ Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
@@ -962,9 +962,9 @@ struct SelectImpl<S, AABB> {
const AABB& bv = query;
const AABB& bv1 = nodes[node1].bv;
const AABB& bv2 = nodes[node2].bv;
- Vec3f v = bv.min_ + bv.max_;
- Vec3f v1 = v - (bv1.min_ + bv1.max_);
- Vec3f v2 = v - (bv2.min_ + bv2.max_);
+ Vec3s v = bv.min_ + bv.max_;
+ Vec3s v1 = v - (bv1.min_ + bv1.max_);
+ Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
diff --git a/include/coal/broadphase/detail/hierarchy_tree_array.h b/include/coal/broadphase/detail/hierarchy_tree_array.h
index a7edf618..e4f12d16 100644
--- a/include/coal/broadphase/detail/hierarchy_tree_array.h
+++ b/include/coal/broadphase/detail/hierarchy_tree_array.h
@@ -114,10 +114,10 @@ class HierarchyTree {
bool update(size_t leaf, const BV& bv);
/// @brief update one leaf's bounding volume, with prediction
- bool update(size_t leaf, const BV& bv, const Vec3f& vel, CoalScalar margin);
+ bool update(size_t leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
/// @brief update one leaf's bounding volume, with prediction
- bool update(size_t leaf, const BV& bv, const Vec3f& vel);
+ bool update(size_t leaf, const BV& bv, const Vec3s& vel);
/// @brief get the max height of the tree
size_t getMaxHeight() const;
diff --git a/include/coal/broadphase/detail/morton-inl.h b/include/coal/broadphase/detail/morton-inl.h
index 4f27aef9..b646f299 100644
--- a/include/coal/broadphase/detail/morton-inl.h
+++ b/include/coal/broadphase/detail/morton-inl.h
@@ -62,7 +62,7 @@ morton_functor<S, uint32_t>::morton_functor(const AABB& bbox)
//==============================================================================
template <typename S>
-uint32_t morton_functor<S, uint32_t>::operator()(const Vec3f& point) const {
+uint32_t morton_functor<S, uint32_t>::operator()(const Vec3s& point) const {
uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
@@ -82,7 +82,7 @@ morton_functor<S, uint64_t>::morton_functor(const AABB& bbox)
//==============================================================================
template <typename S>
-uint64_t morton_functor<S, uint64_t>::operator()(const Vec3f& point) const {
+uint64_t morton_functor<S, uint64_t>::operator()(const Vec3s& point) const {
uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
@@ -115,7 +115,7 @@ morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox)
//==============================================================================
template <typename S, size_t N>
std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
- const Vec3f& point) const {
+ const Vec3s& point) const {
S x = (point[0] - base[0]) * inv[0];
S y = (point[1] - base[1]) * inv[1];
S z = (point[2] - base[2]) * inv[2];
diff --git a/include/coal/broadphase/detail/morton.h b/include/coal/broadphase/detail/morton.h
index f2e8073b..f7a9c947 100644
--- a/include/coal/broadphase/detail/morton.h
+++ b/include/coal/broadphase/detail/morton.h
@@ -71,10 +71,10 @@ template <typename S>
struct morton_functor<S, uint32_t> {
morton_functor(const AABB& bbox);
- uint32_t operator()(const Vec3f& point) const;
+ uint32_t operator()(const Vec3s& point) const;
- const Vec3f base;
- const Vec3f inv;
+ const Vec3s base;
+ const Vec3s inv;
static constexpr size_t bits();
};
@@ -87,10 +87,10 @@ template <typename S>
struct morton_functor<S, uint64_t> {
morton_functor(const AABB& bbox);
- uint64_t operator()(const Vec3f& point) const;
+ uint64_t operator()(const Vec3s& point) const;
- const Vec3f base;
- const Vec3f inv;
+ const Vec3s base;
+ const Vec3s inv;
static constexpr size_t bits();
};
@@ -103,10 +103,10 @@ struct morton_functor<S, std::bitset<N>> {
morton_functor(const AABB& bbox);
- std::bitset<N> operator()(const Vec3f& point) const;
+ std::bitset<N> operator()(const Vec3s& point) const;
- const Vec3f base;
- const Vec3f inv;
+ const Vec3s base;
+ const Vec3s inv;
static constexpr size_t bits();
};
diff --git a/include/coal/collision_data.h b/include/coal/collision_data.h
index 488211f6..7be9827a 100644
--- a/include/coal/collision_data.h
+++ b/include/coal/collision_data.h
@@ -85,7 +85,7 @@ struct COAL_DLLAPI Contact {
/// one contact point but have a zero intersection volume). If the shapes
/// overlap, dist(o1, o2) = -((p2-p1).norm()). Otherwise, dist(o1, o2) =
/// (p2-p1).norm().
- Vec3f normal;
+ Vec3s normal;
/// @brief nearest points associated to this contact.
/// @note Also referred as "witness points" in other collision libraries.
@@ -96,10 +96,10 @@ struct COAL_DLLAPI Contact {
/// vector. If o1 and o2 have multiple contacts, the nearest_points are
/// associated with the contact which has the greatest penetration depth.
/// TODO (louis): rename `nearest_points` to `witness_points`.
- std::array<Vec3f, 2> nearest_points;
+ std::array<Vec3s, 2> nearest_points;
/// @brief contact position, in world space
- Vec3f pos;
+ Vec3s pos;
/// @brief penetration depth
CoalScalar penetration_depth;
@@ -111,7 +111,7 @@ struct COAL_DLLAPI Contact {
Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
penetration_depth = (std::numeric_limits<CoalScalar>::max)();
nearest_points[0] = nearest_points[1] = normal = pos =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
@@ -119,11 +119,11 @@ struct COAL_DLLAPI Contact {
: o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
penetration_depth = (std::numeric_limits<CoalScalar>::max)();
nearest_points[0] = nearest_points[1] = normal = pos =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
- int b2_, const Vec3f& pos_, const Vec3f& normal_, CoalScalar depth_)
+ int b2_, const Vec3s& pos_, const Vec3s& normal_, CoalScalar depth_)
: o1(o1_),
o2(o2_),
b1(b1_),
@@ -135,7 +135,7 @@ struct COAL_DLLAPI Contact {
penetration_depth(depth_) {}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
- int b2_, const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_,
+ int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_,
CoalScalar depth_)
: o1(o1_),
o2(o2_),
@@ -177,7 +177,7 @@ struct COAL_DLLAPI QueryRequest {
bool enable_cached_gjk_guess;
/// @brief the gjk initial guess set by user
- mutable Vec3f cached_gjk_guess;
+ mutable Vec3s cached_gjk_guess;
/// @brief the support function initial guess set by user
mutable support_func_guess_t cached_support_func_guess;
@@ -274,7 +274,7 @@ struct COAL_DLLAPI QueryRequest {
/// @brief base class for all query results
struct COAL_DLLAPI QueryResult {
/// @brief stores the last GJK ray when relevant.
- Vec3f cached_gjk_guess;
+ Vec3s cached_gjk_guess;
/// @brief stores the last support function vertex index, when relevant.
support_func_guess_t cached_support_func_guess;
@@ -283,7 +283,7 @@ struct COAL_DLLAPI QueryResult {
CPUTimes timings;
QueryResult()
- : cached_gjk_guess(Vec3f::Zero()),
+ : cached_gjk_guess(Vec3s::Zero()),
cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
};
@@ -402,7 +402,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult {
/// @brief normal associated to nearest_points.
/// Same as `CollisionResult::nearest_points` but for the normal.
- Vec3f normal;
+ Vec3s normal;
/// @brief nearest points.
/// A `CollisionResult` can have multiple contacts.
@@ -411,13 +411,13 @@ struct COAL_DLLAPI CollisionResult : QueryResult {
/// For bounding volumes and BVHs, these nearest points are available
/// only when distance_lower_bound is inferior to
/// CollisionRequest::break_distance.
- std::array<Vec3f, 2> nearest_points;
+ std::array<Vec3s, 2> nearest_points;
public:
CollisionResult()
: distance_lower_bound((std::numeric_limits<CoalScalar>::max)()) {
nearest_points[0] = nearest_points[1] = normal =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
}
/// @brief Update the lower bound only if the distance is inferior.
@@ -485,7 +485,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult {
contacts.clear();
timings.clear();
nearest_points[0] = nearest_points[1] = normal =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
}
/// @brief reposition Contact objects when fcl inverts them
@@ -511,7 +511,7 @@ struct COAL_DLLAPI CollisionResult : QueryResult {
/// to compute a contact volume instead of a contact patch.
struct COAL_DLLAPI ContactPatch {
public:
- using Polygon = std::vector<Vec2f>;
+ using Polygon = std::vector<Vec2s>;
/// @brief Frame of the set, expressed in the world coordinates.
/// The z-axis of the frame's rotation is the contact patch normal.
@@ -566,7 +566,7 @@ struct COAL_DLLAPI ContactPatch {
}
/// @brief Normal of the contact patch, expressed in the WORLD frame.
- Vec3f getNormal() const {
+ Vec3s getNormal() const {
if (this->direction == PatchDirection::INVERTED) {
return -this->tf.rotation().col(2);
}
@@ -581,14 +581,14 @@ struct COAL_DLLAPI ContactPatch {
/// of the set. It then takes only the x and y components of the vector,
/// effectively doing a projection onto the plane to which the set belongs.
/// TODO(louis): if necessary, we can store the offset to the plane (x, y).
- void addPoint(const Vec3f& point_3d) {
- const Vec3f point = this->tf.inverseTransform(point_3d);
+ void addPoint(const Vec3s& point_3d) {
+ const Vec3s point = this->tf.inverseTransform(point_3d);
this->m_points.emplace_back(point.template head<2>());
}
/// @brief Get the i-th point of the set, expressed in the 3D world frame.
- Vec3f getPoint(const size_t i) const {
- Vec3f point(0, 0, 0);
+ Vec3s getPoint(const size_t i) const {
+ Vec3s point(0, 0, 0);
point.head<2>() = this->point(i);
point = tf.transform(point);
return point;
@@ -597,8 +597,8 @@ struct COAL_DLLAPI ContactPatch {
/// @brief Get the i-th point of the contact patch, projected back onto the
/// first shape of the collision pair. This point is expressed in the 3D
/// world frame.
- Vec3f getPointShape1(const size_t i) const {
- Vec3f point = this->getPoint(i);
+ Vec3s getPointShape1(const size_t i) const {
+ Vec3s point = this->getPoint(i);
point -= (this->penetration_depth / 2) * this->getNormal();
return point;
}
@@ -606,8 +606,8 @@ struct COAL_DLLAPI ContactPatch {
/// @brief Get the i-th point of the contact patch, projected back onto the
/// first shape of the collision pair. This 3D point is expressed in the world
/// frame.
- Vec3f getPointShape2(const size_t i) const {
- Vec3f point = this->getPoint(i);
+ Vec3s getPointShape2(const size_t i) const {
+ Vec3s point = this->getPoint(i);
point += (this->penetration_depth / 2) * this->getNormal();
return point;
}
@@ -619,7 +619,7 @@ struct COAL_DLLAPI ContactPatch {
const Polygon& points() const { return this->m_points; }
/// @brief Getter for the i-th 2D point in the set.
- Vec2f& point(const size_t i) {
+ Vec2s& point(const size_t i) {
COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
if (i < this->m_points.size()) {
return this->m_points[i];
@@ -628,7 +628,7 @@ struct COAL_DLLAPI ContactPatch {
}
/// @brief Const getter for the i-th 2D point in the set.
- const Vec2f& point(const size_t i) const {
+ const Vec2s& point(const size_t i) const {
COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
if (i < this->m_points.size()) {
return this->m_points[i];
@@ -680,9 +680,9 @@ struct COAL_DLLAPI ContactPatch {
// Check all points of the contact patch.
for (size_t i = 0; i < this->size(); ++i) {
bool found = false;
- const Vec3f pi = this->getPoint(i);
+ const Vec3s pi = this->getPoint(i);
for (size_t j = 0; j < other.size(); ++j) {
- const Vec3f other_pj = other.getPoint(j);
+ const Vec3s other_pj = other.getPoint(j);
if (pi.isApprox(other_pj, tol)) {
found = true;
}
@@ -1058,11 +1058,11 @@ struct COAL_DLLAPI DistanceResult : QueryResult {
CoalScalar min_distance;
/// @brief normal.
- Vec3f normal;
+ Vec3s normal;
/// @brief nearest points.
/// See CollisionResult::nearest_points.
- std::array<Vec3f, 2> nearest_points;
+ std::array<Vec3s, 2> nearest_points;
/// @brief collision object 1
const CollisionGeometry* o1;
@@ -1088,8 +1088,8 @@ struct COAL_DLLAPI DistanceResult : QueryResult {
DistanceResult(
CoalScalar min_distance_ = (std::numeric_limits<CoalScalar>::max)())
: min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
- const Vec3f nan(
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+ const Vec3s nan(
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
nearest_points[0] = nearest_points[1] = normal = nan;
}
@@ -1107,8 +1107,8 @@ struct COAL_DLLAPI DistanceResult : QueryResult {
/// @brief add distance information into the result
void update(CoalScalar distance, const CollisionGeometry* o1_,
- const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1,
- const Vec3f& p2, const Vec3f& normal_) {
+ const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1,
+ const Vec3s& p2, const Vec3s& normal_) {
if (min_distance > distance) {
min_distance = distance;
o1 = o1_;
@@ -1137,8 +1137,8 @@ struct COAL_DLLAPI DistanceResult : QueryResult {
/// @brief clear the result
void clear() {
- const Vec3f nan(
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+ const Vec3s nan(
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
min_distance = (std::numeric_limits<CoalScalar>::max)();
o1 = NULL;
o2 = NULL;
@@ -1184,8 +1184,8 @@ inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
CollisionResult& res,
const CoalScalar& distance,
- const Vec3f& p0, const Vec3f& p1,
- const Vec3f& normal) {
+ const Vec3s& p0, const Vec3s& p1,
+ const Vec3s& normal) {
if (distance < res.distance_lower_bound) {
res.distance_lower_bound = distance;
res.nearest_points[0] = p0;
diff --git a/include/coal/collision_object.h b/include/coal/collision_object.h
index 1088ed62..503ca601 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<CoalScalar>::max)())),
+ : aabb_center(Vec3s::Constant((std::numeric_limits<CoalScalar>::max)())),
aabb_radius(-1),
cost_density(1),
threshold_occupied(1),
@@ -148,7 +148,7 @@ class COAL_DLLAPI CollisionGeometry {
bool isUncertain() const;
/// @brief AABB center in local coordinate
- Vec3f aabb_center;
+ Vec3s aabb_center;
/// @brief AABB radius
CoalScalar aabb_radius;
@@ -170,23 +170,23 @@ class COAL_DLLAPI CollisionGeometry {
CoalScalar threshold_free;
/// @brief compute center of mass
- virtual Vec3f computeCOM() const { return Vec3f::Zero(); }
+ virtual Vec3s computeCOM() const { return Vec3s::Zero(); }
/// @brief compute the inertia matrix, related to the origin
- virtual Matrix3f computeMomentofInertia() const {
- return Matrix3f::Constant(NAN);
+ virtual Matrix3s computeMomentofInertia() const {
+ return Matrix3s::Constant(NAN);
}
/// @brief compute the volume
virtual CoalScalar computeVolume() const { return 0; }
/// @brief compute the inertia matrix, related to the com
- virtual Matrix3f computeMomentofInertiaRelatedToCOM() const {
- Matrix3f C = computeMomentofInertia();
- Vec3f com = computeCOM();
+ virtual Matrix3s computeMomentofInertiaRelatedToCOM() const {
+ Matrix3s C = computeMomentofInertia();
+ Vec3s com = computeCOM();
CoalScalar V = computeVolume();
- return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
+ return (Matrix3s() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2],
C(1, 0) + V * com[1] * com[0],
C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
@@ -226,7 +226,7 @@ class COAL_DLLAPI CollisionObject {
}
CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
- const Matrix3f& R, const Vec3f& T,
+ const Matrix3s& R, const Vec3s& T,
bool compute_local_aabb = true)
: cgeom(cgeom_), t(R, T), user_data(nullptr) {
init(compute_local_aabb);
@@ -261,7 +261,7 @@ class COAL_DLLAPI CollisionObject {
} else {
aabb.min_ = aabb.max_ = t.getTranslation();
- Vec3f min_world, max_world;
+ Vec3s min_world, max_world;
for (int k = 0; k < 3; ++k) {
min_world.array() = t.getRotation().row(k).array() *
cgeom->aabb_local.min_.transpose().array();
@@ -281,22 +281,22 @@ class COAL_DLLAPI CollisionObject {
void setUserData(void* data) { user_data = data; }
/// @brief get translation of the object
- inline const Vec3f& getTranslation() const { return t.getTranslation(); }
+ inline const Vec3s& getTranslation() const { return t.getTranslation(); }
/// @brief get matrix rotation of the object
- inline const Matrix3f& getRotation() const { return t.getRotation(); }
+ inline const Matrix3s& getRotation() const { return t.getRotation(); }
/// @brief get object's transform
inline const Transform3f& getTransform() const { return t; }
/// @brief set object's rotation matrix
- void setRotation(const Matrix3f& R) { t.setRotation(R); }
+ void setRotation(const Matrix3s& R) { t.setRotation(R); }
/// @brief set object's translation
- void setTranslation(const Vec3f& T) { t.setTranslation(T); }
+ void setTranslation(const Vec3s& T) { t.setTranslation(T); }
/// @brief set object's transform
- void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); }
+ void setTransform(const Matrix3s& R, const Vec3s& T) { t.setTransform(R, T); }
/// @brief set object's transform
void setTransform(const Transform3f& tf) { t = tf; }
diff --git a/include/coal/contact_patch/contact_patch_solver.h b/include/coal/contact_patch/contact_patch_solver.h
index 09ae43c1..63c7fd34 100644
--- a/include/coal/contact_patch/contact_patch_solver.h
+++ b/include/coal/contact_patch/contact_patch_solver.h
@@ -180,8 +180,8 @@ struct COAL_DLLAPI ContactPatchSolver {
/// @note we make the following hypothesis:
/// 1) c != d (should be when creating initial polytopes)
/// 2) (c, d) is not parallel to ray -> if so, we return d.
- static Vec2f computeLineSegmentIntersection(const Vec2f& a, const Vec2f& b,
- const Vec2f& c, const Vec2f& d);
+ static Vec2s computeLineSegmentIntersection(const Vec2s& a, const Vec2s& b,
+ const Vec2s& c, const Vec2s& d);
/// @brief Construct support set function for shape.
static SupportSetFunction makeSupportSetFunction(
diff --git a/include/coal/contact_patch/contact_patch_solver.hxx b/include/coal/contact_patch/contact_patch_solver.hxx
index f730b79d..403612f3 100644
--- a/include/coal/contact_patch/contact_patch_solver.hxx
+++ b/include/coal/contact_patch/contact_patch_solver.hxx
@@ -44,7 +44,7 @@ namespace coal {
// ============================================================================
inline void ContactPatchSolver::set(const ContactPatchRequest& request) {
- // Note: it's important for the number of pre-allocated Vec3f in
+ // Note: it's important for the number of pre-allocated Vec3s in
// `m_clipping_sets` to be larger than `request.max_size_patch`
// because we don't know in advance how many supports will be discarded to
// form the convex-hulls of the shapes supports which will serve as the
@@ -138,12 +138,12 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
// We compute the determinant; if it is non-zero, the intersection
// has already been computed: it's `Contact::pos`.
const Polygon& pts1 = this->support_set_shape1.points();
- const Vec2f& a = pts1[0];
- const Vec2f& b = pts1[1];
+ const Vec2s& a = pts1[0];
+ const Vec2s& b = pts1[1];
const Polygon& pts2 = this->support_set_shape2.points();
- const Vec2f& c = pts2[0];
- const Vec2f& d = pts2[1];
+ const Vec2s& c = pts2[0];
+ const Vec2s& d = pts2[1];
const CoalScalar det =
(b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0));
@@ -153,20 +153,20 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
return;
}
- const Vec2f cd = (d - c);
+ const Vec2s cd = (d - c);
const CoalScalar l = cd.squaredNorm();
Polygon& patch = contact_patch.points();
// Project a onto [c, d]
CoalScalar t1 = (a - c).dot(cd);
t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l));
- const Vec2f p1 = c + t1 * cd;
+ const Vec2s p1 = c + t1 * cd;
patch.emplace_back(p1);
// Project b onto [c, d]
CoalScalar t2 = (b - c).dot(cd);
t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l));
- const Vec2f p2 = c + t2 * cd;
+ const Vec2s p2 = c + t2 * cd;
if ((p1 - p2).squaredNorm() >= eps) {
patch.emplace_back(p2);
}
@@ -222,19 +222,19 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
Polygon& current = *(current_ptr);
current.clear();
- const Vec2f& a = clipper[i];
- const Vec2f& b = clipper[(i + 1) % clipper_size];
- const Vec2f ab = b - a;
+ const Vec2s& a = clipper[i];
+ const Vec2s& b = clipper[(i + 1) % clipper_size];
+ const Vec2s ab = b - a;
if (previous.size() == 2) {
//
// Segment-Polygon case
//
- const Vec2f& p1 = previous[0];
- const Vec2f& p2 = previous[1];
+ const Vec2s& p1 = previous[0];
+ const Vec2s& p2 = previous[1];
- const Vec2f ap1 = p1 - a;
- const Vec2f ap2 = p2 - a;
+ const Vec2s ap1 = p1 - a;
+ const Vec2s ap2 = p2 - a;
const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
@@ -256,7 +256,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
// [p1, p2].
if (det1 >= 0) {
if (det1 > eps) {
- const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
+ const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p1);
current.emplace_back(p);
continue;
@@ -268,7 +268,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
}
} else {
if (det2 > eps) {
- const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
+ const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p2);
current.emplace_back(p);
continue;
@@ -289,11 +289,11 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
const size_t previous_size = previous.size();
for (size_t j = 0; j < previous_size; ++j) {
- const Vec2f& p1 = previous[j];
- const Vec2f& p2 = previous[(j + 1) % previous_size];
+ const Vec2s& p1 = previous[j];
+ const Vec2s& p2 = previous[(j + 1) % previous_size];
- const Vec2f ap1 = p1 - a;
- const Vec2f ap2 = p2 - a;
+ const Vec2s ap1 = p1 - a;
+ const Vec2s ap2 = p2 - a;
const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
@@ -320,7 +320,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
current.emplace_back(p1);
this->added_to_patch[j] = true;
}
- const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
+ const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p);
} else {
// a, b and p1 are colinear; we add only p1.
@@ -331,7 +331,7 @@ void ContactPatchSolver::computePatch(const ShapeType1& s1,
}
} else {
if (det2 > eps) {
- const Vec2f p = computeLineSegmentIntersection(a, b, p1, p2);
+ const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p);
} else {
if (!this->added_to_patch[(j + 1) % previous.size()]) {
@@ -406,10 +406,10 @@ inline void ContactPatchSolver::reset(const ShapeType1& shape1,
}
// ==========================================================================
-inline Vec2f ContactPatchSolver::computeLineSegmentIntersection(
- const Vec2f& a, const Vec2f& b, const Vec2f& c, const Vec2f& d) {
- const Vec2f ab = b - a;
- const Vec2f n(-ab(1), ab(0));
+inline Vec2s ContactPatchSolver::computeLineSegmentIntersection(
+ const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) {
+ const Vec2s ab = b - a;
+ const Vec2s n(-ab(1), ab(0));
const CoalScalar denominator = n.dot(c - d);
if (std::abs(denominator) < std::numeric_limits<double>::epsilon()) {
return d;
diff --git a/include/coal/data_types.h b/include/coal/data_types.h
index b9b64bb7..ea8c97d3 100644
--- a/include/coal/data_types.h
+++ b/include/coal/data_types.h
@@ -59,6 +59,8 @@
#endif // COAL_HAS_OCTOMAP
namespace coal {
+// We keep the FCL_REAL typedef and the Vec[..]f typedefs for backward
+// compatibility.
typedef double FCL_REAL;
typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
typedef Eigen::Matrix<FCL_REAL, 2, 1> Vec2f;
@@ -75,15 +77,15 @@ 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<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<CoalScalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
typedef Eigen::Vector2i support_func_guess_t;
/// @brief Initial guess to use for the GJK algorithm
-/// DefaultGuess: Vec3f(1, 0, 0)
+/// DefaultGuess: Vec3s(1, 0, 0)
/// CachedGuess: previous vector found by GJK or guess cached by the user
/// BoundingVolumeGuess: guess using the centers of the shapes' AABB
/// WARNING: to use BoundingVolumeGuess, computeLocalAABB must have been called
diff --git a/include/coal/hfield.h b/include/coal/hfield.h
index 4204efc2..9949296e 100644
--- a/include/coal/hfield.h
+++ b/include/coal/hfield.h
@@ -151,17 +151,17 @@ struct COAL_DLLAPI HFNode : public HFNodeBase {
/// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
/// the underlying BV supports distance, return the nearest points.
- CoalScalar distance(const HFNode& other, Vec3f* P1 = NULL,
- Vec3f* P2 = NULL) const {
+ CoalScalar distance(const HFNode& other, Vec3s* P1 = NULL,
+ Vec3s* P2 = NULL) const {
return bv.distance(other.bv, P1, P2);
}
/// @brief Access to the center of the BV
- Vec3f getCenter() const { return bv.center(); }
+ Vec3s getCenter() const { return bv.center(); }
/// @brief Access to the orientation of the BV
- coal::Matrix3f::IdentityReturnType getOrientation() const {
- return Matrix3f::Identity();
+ coal::Matrix3s::IdentityReturnType getOrientation() const {
+ return Matrix3s::Identity();
}
virtual ~HFNode() {}
@@ -171,7 +171,7 @@ namespace details {
template <typename BV>
struct UpdateBoundingVolume {
- static void run(const Vec3f& pointA, const Vec3f& pointB, BV& bv) {
+ static void run(const Vec3s& pointA, const Vec3s& pointB, BV& bv) {
AABB bv_aabb(pointA, pointB);
// AABB bv_aabb;
// bv_aabb.update(pointA,pointB);
@@ -181,7 +181,7 @@ struct UpdateBoundingVolume {
template <>
struct UpdateBoundingVolume<AABB> {
- static void run(const Vec3f& pointA, const Vec3f& pointB, AABB& bv) {
+ static void run(const Vec3s& pointA, const Vec3s& pointB, AABB& bv) {
AABB bv_aabb(pointA, pointB);
convertBV(bv_aabb, bv);
// bv.update(pointA,pointB);
@@ -226,7 +226,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
/// \param[in] min_height Minimal height of the height field
///
HeightField(const CoalScalar x_dim, const CoalScalar y_dim,
- const MatrixXf& heights,
+ const MatrixXs& heights,
const CoalScalar min_height = (CoalScalar)0)
: CollisionGeometry() {
init(x_dim, y_dim, heights, min_height);
@@ -249,12 +249,12 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
num_bvs(other.num_bvs) {}
/// @brief Returns a const reference of the grid along the X direction.
- const VecXf& getXGrid() const { return x_grid; }
+ const VecXs& getXGrid() const { return x_grid; }
/// @brief Returns a const reference of the grid along the Y direction.
- const VecXf& getYGrid() const { return y_grid; }
+ const VecXs& getYGrid() const { return y_grid; }
/// @brief Returns a const reference of the heights
- const MatrixXf& getHeights() const { return heights; }
+ const MatrixXs& getHeights() const { return heights; }
/// @brief Returns the dimension of the Height Field along the X direction.
CoalScalar getXDim() const { return x_dim; }
@@ -276,8 +276,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
/// @brief Compute the AABB for the HeightField, used for broad-phase
/// collision
void computeLocalAABB() {
- const Vec3f A(x_grid[0], y_grid[0], min_height);
- const Vec3f B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
+ const Vec3s A(x_grid[0], y_grid[0], min_height);
+ const Vec3s B(x_grid[x_grid.size() - 1], y_grid[y_grid.size() - 1],
max_height);
const AABB aabb_(A, B);
@@ -287,7 +287,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
}
/// @brief Update Height Field height
- void updateHeights(const MatrixXf& new_heights) {
+ void updateHeights(const MatrixXs& new_heights) {
if (new_heights.rows() != heights.rows() ||
new_heights.cols() != heights.cols())
COAL_THROW_PRETTY(
@@ -306,7 +306,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
protected:
void init(const CoalScalar x_dim, const CoalScalar y_dim,
- const MatrixXf& heights, const CoalScalar min_height) {
+ const MatrixXs& heights, const CoalScalar min_height) {
this->x_dim = x_dim;
this->y_dim = y_dim;
this->heights = heights.cwiseMax(min_height);
@@ -317,8 +317,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
assert(NX >= 2 && "The number of columns is too small.");
assert(NY >= 2 && "The number of rows is too small.");
- x_grid = VecXf::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
- y_grid = VecXf::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);
+ x_grid = VecXs::LinSpaced(NX, -0.5 * x_dim, 0.5 * x_dim);
+ y_grid = VecXs::LinSpaced(NY, 0.5 * y_dim, -0.5 * y_dim);
// Allocate BVS
const size_t num_tot_bvs =
@@ -333,18 +333,18 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
/// @brief Get the object type: it is a HFIELD
OBJECT_TYPE getObjectType() const { return OT_HFIELD; }
- Vec3f computeCOM() const { return Vec3f::Zero(); }
+ Vec3s computeCOM() const { return Vec3s::Zero(); }
CoalScalar computeVolume() const { return 0; }
- Matrix3f computeMomentofInertia() const { return Matrix3f::Zero(); }
+ Matrix3s computeMomentofInertia() const { return Matrix3s::Zero(); }
protected:
/// @brief Dimensions in meters along X and Y directions
CoalScalar x_dim, y_dim;
/// @brief Elevation values in meters of the Height Field
- MatrixXf heights;
+ MatrixXs heights;
/// @brief Minimal height of the Height Field: all values bellow min_height
/// will be discarded.
@@ -352,7 +352,7 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
/// @brief Grids along the X and Y directions. Useful for plotting or other
/// related things.
- VecXf x_grid, y_grid;
+ VecXs x_grid, y_grid;
/// @brief Bounding volume hierarchy
BVS bvs;
@@ -386,8 +386,8 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
bv_node.max_height = max_height;
- const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
- const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size],
+ const Vec3s pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height);
+ const Vec3s pointB(x_grid[bv_node.x_id + bv_node.x_size],
y_grid[bv_node.y_id + bv_node.y_size], max_height);
details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
@@ -445,10 +445,10 @@ class COAL_DLLAPI HeightField : public CollisionGeometry {
bv_node.max_height = max_height;
// max_height = std::max(max_height,min_height);
- const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height);
+ const Vec3s pointA(x_grid[x_id], y_grid[y_id], min_height);
assert(x_id + x_size < x_grid.size());
assert(y_id + y_size < y_grid.size());
- const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
+ const Vec3s pointB(x_grid[x_id + x_size], y_grid[y_id + y_size],
max_height);
details::UpdateBoundingVolume<BV>::run(pointA, pointB, bv_node.bv);
diff --git a/include/coal/internal/BV_fitter.h b/include/coal/internal/BV_fitter.h
index 93f12678..a115d914 100644
--- a/include/coal/internal/BV_fitter.h
+++ b/include/coal/internal/BV_fitter.h
@@ -48,7 +48,7 @@ namespace coal {
/// @brief Compute a bounding volume that fits a set of n points.
template <typename BV>
-void fit(Vec3f* ps, unsigned int n, BV& bv) {
+void fit(Vec3s* ps, unsigned int n, BV& bv) {
for (unsigned int i = 0; i < n; ++i) // TODO(jcarpent): vectorize
{
bv += ps[i];
@@ -56,19 +56,19 @@ void fit(Vec3f* ps, unsigned int n, BV& bv) {
}
template <>
-void fit<OBB>(Vec3f* ps, unsigned int n, OBB& bv);
+void fit<OBB>(Vec3s* ps, unsigned int n, OBB& bv);
template <>
-void fit<RSS>(Vec3f* ps, unsigned int n, RSS& bv);
+void fit<RSS>(Vec3s* ps, unsigned int n, RSS& bv);
template <>
-void fit<kIOS>(Vec3f* ps, unsigned int n, kIOS& bv);
+void fit<kIOS>(Vec3s* ps, unsigned int n, kIOS& bv);
template <>
-void fit<OBBRSS>(Vec3f* ps, unsigned int n, OBBRSS& bv);
+void fit<OBBRSS>(Vec3s* ps, unsigned int n, OBBRSS& bv);
template <>
-void fit<AABB>(Vec3f* ps, unsigned int n, AABB& bv);
+void fit<AABB>(Vec3s* ps, unsigned int n, AABB& bv);
/// @brief The class for the default algorithm fitting a bounding volume to a
/// set of points
@@ -79,7 +79,7 @@ class COAL_DLLAPI BVFitterTpl {
virtual ~BVFitterTpl() {}
/// @brief Prepare the geometry primitive data for fitting
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) {
+ void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) {
vertices = vertices_;
prev_vertices = NULL;
tri_indices = tri_indices_;
@@ -88,7 +88,7 @@ class COAL_DLLAPI BVFitterTpl {
/// @brief Prepare the geometry primitive data for fitting, for deformable
/// mesh
- void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_,
+ void set(Vec3s* vertices_, Vec3s* prev_vertices_, Triangle* tri_indices_,
BVHModelType type_) {
vertices = vertices_;
prev_vertices = prev_vertices_;
@@ -109,8 +109,8 @@ class COAL_DLLAPI BVFitterTpl {
}
protected:
- Vec3f* vertices;
- Vec3f* prev_vertices;
+ Vec3s* vertices;
+ Vec3s* prev_vertices;
Triangle* tri_indices;
BVHModelType type;
};
diff --git a/include/coal/internal/BV_splitter.h b/include/coal/internal/BV_splitter.h
index 6106a203..9bfdf459 100644
--- a/include/coal/internal/BV_splitter.h
+++ b/include/coal/internal/BV_splitter.h
@@ -64,7 +64,7 @@ class BVSplitter {
virtual ~BVSplitter() {}
/// @brief Set the geometry data needed by the split rule
- void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) {
+ void set(Vec3s* vertices_, Triangle* tri_indices_, BVHModelType type_) {
vertices = vertices_;
tri_indices = tri_indices_;
type = type_;
@@ -90,7 +90,7 @@ class BVSplitter {
}
/// @brief Apply the split rule on a given point
- bool apply(const Vec3f& q) const { return q[split_axis] > split_value; }
+ bool apply(const Vec3s& q) const { return q[split_axis] > split_value; }
/// @brief Clear the geometry data set before
void clear() {
@@ -105,7 +105,7 @@ class BVSplitter {
/// is needed. For oriented node, we can use a vector to make a better split
/// decision.
int split_axis;
- Vec3f split_vector;
+ Vec3s split_vector;
/// @brief The split threshold, different primitives are splitted according
/// whether their projection on the split_axis is larger or smaller than the
@@ -113,7 +113,7 @@ class BVSplitter {
CoalScalar split_value;
/// @brief The mesh vertices or points handled by the splitter
- Vec3f* vertices;
+ Vec3s* vertices;
/// @brief The triangles handled by the splitter
Triangle* tri_indices;
@@ -126,7 +126,7 @@ class BVSplitter {
/// @brief Split algorithm 1: Split the node from center
void computeRule_bvcenter(const BV& bv, unsigned int*, unsigned int) {
- Vec3f center = bv.center();
+ Vec3s center = bv.center();
int axis = 2;
if (bv.width() >= bv.height() && bv.width() >= bv.depth())
@@ -207,16 +207,16 @@ class BVSplitter {
};
template <>
-bool COAL_DLLAPI BVSplitter<OBB>::apply(const Vec3f& q) const;
+bool COAL_DLLAPI BVSplitter<OBB>::apply(const Vec3s& q) const;
template <>
-bool COAL_DLLAPI BVSplitter<RSS>::apply(const Vec3f& q) const;
+bool COAL_DLLAPI BVSplitter<RSS>::apply(const Vec3s& q) const;
template <>
-bool COAL_DLLAPI BVSplitter<kIOS>::apply(const Vec3f& q) const;
+bool COAL_DLLAPI BVSplitter<kIOS>::apply(const Vec3s& q) const;
template <>
-bool COAL_DLLAPI BVSplitter<OBBRSS>::apply(const Vec3f& q) const;
+bool COAL_DLLAPI BVSplitter<OBBRSS>::apply(const Vec3s& q) const;
template <>
void COAL_DLLAPI BVSplitter<OBB>::computeRule_bvcenter(
diff --git a/include/coal/internal/intersect.h b/include/coal/internal/intersect.h
index acbf532f..4b5ef8d7 100644
--- a/include/coal/internal/intersect.h
+++ b/include/coal/internal/intersect.h
@@ -47,8 +47,8 @@ namespace coal {
/// @brief CCD intersect kernel among primitives
class COAL_DLLAPI Intersect {
public:
- static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2,
- const Vec3f& v3, Vec3f* n, CoalScalar* t);
+ static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
+ const Vec3s& v3, Vec3s* n, CoalScalar* t);
}; // class Intersect
/// @brief Project functions
@@ -69,28 +69,28 @@ class COAL_DLLAPI Project {
};
/// @brief Project point p onto line a-b
- static ProjectResult projectLine(const Vec3f& a, const Vec3f& b,
- const Vec3f& p);
+ static ProjectResult projectLine(const Vec3s& a, const Vec3s& b,
+ const Vec3s& p);
/// @brief Project point p onto triangle a-b-c
- static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b,
- const Vec3f& c, const Vec3f& p);
+ static ProjectResult projectTriangle(const Vec3s& a, const Vec3s& b,
+ const Vec3s& c, const Vec3s& p);
/// @brief Project point p onto tetrahedra a-b-c-d
- static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b,
- const Vec3f& c, const Vec3f& d,
- const Vec3f& p);
+ static ProjectResult projectTetrahedra(const Vec3s& a, const Vec3s& b,
+ const Vec3s& c, const Vec3s& d,
+ const Vec3s& p);
/// @brief Project origin (0) onto line a-b
- static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b);
+ static ProjectResult projectLineOrigin(const Vec3s& a, const Vec3s& b);
/// @brief Project origin (0) onto triangle a-b-c
- static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b,
- const Vec3f& c);
+ static ProjectResult projectTriangleOrigin(const Vec3s& a, const Vec3s& b,
+ const Vec3s& c);
/// @brief Project origin (0) onto tetrahedran a-b-c-d
- static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b,
- const Vec3f& c, const Vec3f& d);
+ static ProjectResult projectTetrahedraOrigin(const Vec3s& a, const Vec3s& b,
+ const Vec3s& c, const Vec3s& d);
};
/// @brief Triangle distance functions
@@ -101,8 +101,8 @@ class COAL_DLLAPI TriangleDistance {
/// The second segment is Q + t * B
/// X, Y are the closest points on the two segments
/// VEC is the vector between X and Y
- static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
- const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y);
+ static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
+ const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y);
/// Compute squared distance between triangles
/// @param S and T are two triangles
@@ -113,13 +113,13 @@ class COAL_DLLAPI TriangleDistance {
/// if the triangles overlap, P and Q are basically a random pair of points
/// from the triangles, not coincident points on the intersection of the
/// triangles, as might be expected.
- static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P,
- Vec3f& Q);
+ static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P,
+ Vec3s& Q);
- static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
- const Vec3f& S3, const Vec3f& T1,
- const Vec3f& T2, const Vec3f& T3, Vec3f& P,
- Vec3f& Q);
+ static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+ const Vec3s& S3, const Vec3s& T1,
+ const Vec3s& T2, const Vec3s& T3, Vec3s& P,
+ Vec3s& Q);
/// Compute squared distance between triangles
/// @param S and T are two triangles
@@ -131,9 +131,9 @@ class COAL_DLLAPI TriangleDistance {
/// if the triangles overlap, P and Q are basically a random pair of points
/// from the triangles, not coincident points on the intersection of the
/// triangles, as might be expected.
- static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
- const Matrix3f& R, const Vec3f& Tl, Vec3f& P,
- Vec3f& Q);
+ static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+ const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+ Vec3s& Q);
/// Compute squared distance between triangles
/// @param S and T are two triangles
@@ -145,8 +145,8 @@ class COAL_DLLAPI TriangleDistance {
/// if the triangles overlap, P and Q are basically a random pair of points
/// from the triangles, not coincident points on the intersection of the
/// triangles, as might be expected.
- static CoalScalar sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
- const Transform3f& tf, Vec3f& P, Vec3f& Q);
+ static CoalScalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+ const Transform3f& tf, Vec3s& P, Vec3s& Q);
/// Compute squared distance between triangles
/// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
@@ -158,11 +158,11 @@ class COAL_DLLAPI TriangleDistance {
/// if the triangles overlap, P and Q are basically a random pair of points
/// from the triangles, not coincident points on the intersection of the
/// triangles, as might be expected.
- static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
- const Vec3f& S3, const Vec3f& T1,
- const Vec3f& T2, const Vec3f& T3,
- const Matrix3f& R, const Vec3f& Tl, Vec3f& P,
- Vec3f& Q);
+ static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+ const Vec3s& S3, const Vec3s& T1,
+ const Vec3s& T2, const Vec3s& T3,
+ const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
+ Vec3s& Q);
/// Compute squared distance between triangles
/// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
@@ -174,10 +174,10 @@ class COAL_DLLAPI TriangleDistance {
/// if the triangles overlap, P and Q are basically a random pair of points
/// from the triangles, not coincident points on the intersection of the
/// triangles, as might be expected.
- static CoalScalar sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
- const Vec3f& S3, const Vec3f& T1,
- const Vec3f& T2, const Vec3f& T3,
- const Transform3f& tf, Vec3f& P, Vec3f& Q);
+ static CoalScalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+ const Vec3s& S3, const Vec3s& T1,
+ const Vec3s& T2, const Vec3s& T3,
+ const Transform3f& tf, Vec3s& P, Vec3s& Q);
};
} // namespace coal
diff --git a/include/coal/internal/shape_shape_func.h b/include/coal/internal/shape_shape_func.h
index 07a4a97e..1d518e7f 100644
--- a/include/coal/internal/shape_shape_func.h
+++ b/include/coal/internal/shape_shape_func.h
@@ -58,7 +58,7 @@ struct ShapeShapeDistancer {
// Witness points on shape1 and shape2, normal pointing from shape1 to
// shape2.
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance =
ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
o1, tf1, o2, tf2, nsolver, request.enable_signed_distance, p1, p2,
@@ -73,8 +73,8 @@ struct ShapeShapeDistancer {
static CoalScalar run(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
- const bool compute_signed_distance, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
+ const bool compute_signed_distance, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) {
const ShapeType1* obj1 = static_cast<const ShapeType1*>(o1);
const ShapeType2* obj2 = static_cast<const ShapeType2*>(o2);
return nsolver->shapeDistance(*obj1, tf1, *obj2, tf2,
@@ -116,8 +116,8 @@ CoalScalar ShapeShapeDistance(const CollisionGeometry* o1,
const Transform3f& tf1,
const CollisionGeometry* o2,
const Transform3f& tf2, const GJKSolver* nsolver,
- const bool compute_signed_distance, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
+ const bool compute_signed_distance, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) {
return ::coal::ShapeShapeDistancer<ShapeType1, ShapeType2>::run(
o1, tf1, o2, tf2, nsolver, compute_signed_distance, p1, p2, normal);
}
@@ -143,7 +143,7 @@ struct ShapeShapeCollider {
const bool compute_penetration =
request.enable_contact || (request.security_margin < 0);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
CoalScalar distance = internal::ShapeShapeDistance<ShapeType1, ShapeType2>(
o1, tf1, o2, tf2, nsolver, compute_penetration, p1, p2, normal);
@@ -219,14 +219,14 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
COAL_DLLAPI CoalScalar internal::ShapeShapeDistance<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); \
+ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+ Vec3s& p2, Vec3s& normal); \
template <> \
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); \
+ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+ Vec3s& p2, Vec3s& normal); \
template <> \
inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T1, T2>( \
const CollisionGeometry* o1, const Transform3f& tf1, \
@@ -263,8 +263,8 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
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); \
+ const GJKSolver* nsolver, const bool compute_signed_distance, Vec3s& p1, \
+ Vec3s& p2, Vec3s& normal); \
template <> \
inline COAL_DLLAPI CoalScalar ShapeShapeDistance<T, T>( \
const CollisionGeometry* o1, const Transform3f& tf1, \
diff --git a/include/coal/internal/traversal.h b/include/coal/internal/traversal.h
index 36b8004f..32fcac45 100644
--- a/include/coal/internal/traversal.h
+++ b/include/coal/internal/traversal.h
@@ -46,21 +46,21 @@ enum { RelativeTransformationIsIdentity = 1 };
namespace details {
template <bool enabled>
struct COAL_DLLAPI RelativeTransformation {
- RelativeTransformation() : R(Matrix3f::Identity()) {}
+ RelativeTransformation() : R(Matrix3s::Identity()) {}
- const Matrix3f& _R() const { return R; }
- const Vec3f& _T() const { return T; }
+ const Matrix3s& _R() const { return R; }
+ const Vec3s& _T() const { return T; }
- Matrix3f R;
- Vec3f T;
+ Matrix3s R;
+ Vec3s T;
};
template <>
struct COAL_DLLAPI RelativeTransformation<false> {
- static const Matrix3f& _R() {
+ static const Matrix3s& _R() {
COAL_THROW_PRETTY("should never reach this point", std::logic_error);
}
- static const Vec3f& _T() {
+ static const Vec3s& _T() {
COAL_THROW_PRETTY("should never reach this point", std::logic_error);
}
};
diff --git a/include/coal/internal/traversal_node_bvh_hfield.h b/include/coal/internal/traversal_node_bvh_hfield.h
index ad2d203d..fc550eb2 100644
--- a/include/coal/internal/traversal_node_bvh_hfield.h
+++ b/include/coal/internal/traversal_node_bvh_hfield.h
@@ -182,9 +182,9 @@ class MeshHeightFieldCollisionTraversalNode
int primitive_id1 = node1.primitiveId();
const Triangle& tri_id1 = tri_indices1[primitive_id1];
- const Vec3f& P1 = vertices1[tri_id1[0]];
- const Vec3f& P2 = vertices1[tri_id1[1]];
- const Vec3f& P3 = vertices1[tri_id1[2]];
+ const Vec3s& P1 = vertices1[tri_id1[0]];
+ const Vec3s& P2 = vertices1[tri_id1[1]];
+ const Vec3s& P3 = vertices1[tri_id1[2]];
TriangleP tri1(P1, P2, P3);
@@ -193,16 +193,16 @@ class MeshHeightFieldCollisionTraversalNode
details::buildConvexTriangles(node2, *this->model2, convex2, convex2);
GJKSolver solver;
- Vec3f p1,
+ Vec3s p1,
p2; // closest points if no collision contact points if collision.
- Vec3f normal;
+ Vec3s normal;
CoalScalar distance;
solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
normal);
CoalScalar distToCollision = distance - this->request.security_margin;
sqrDistLowerBound = distance * distance;
if (distToCollision <= 0) { // collision
- Vec3f p(p1); // contact point
+ Vec3s p(p1); // contact point
CoalScalar penetrationDepth(0);
if (this->result->numContacts() < this->request.num_max_contacts) {
// How much (Q1, Q2, Q3) should be moved so that all vertices are
@@ -229,7 +229,7 @@ class MeshHeightFieldCollisionTraversalNode
mutable int num_leaf_tests;
mutable CoalScalar query_time_seconds;
- Vec3f* vertices1 Triangle* tri_indices1;
+ Vec3s* vertices1 Triangle* tri_indices1;
details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};
@@ -253,7 +253,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl {
static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
return b1.distance(b2);
}
- static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
+ static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1,
const BVNode<BV>& b2) {
return distance(R, T, b1.bv, b2.bv);
}
@@ -271,7 +271,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
}
return sqrt(sqrDistLowerBound);
}
- static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ static CoalScalar run(const Matrix3s& R, const Vec3s& T,
const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
CoalScalar sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
@@ -296,7 +296,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
}
return sqrt(sqrDistLowerBound);
}
- static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ static CoalScalar run(const Matrix3s& R, const Vec3s& T,
const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
CoalScalar sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
@@ -439,16 +439,16 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
const Triangle& tri_id1 = tri_indices1[primitive_id1];
const Triangle& tri_id2 = tri_indices2[primitive_id2];
- const Vec3f& t11 = vertices1[tri_id1[0]];
- const Vec3f& t12 = vertices1[tri_id1[1]];
- const Vec3f& t13 = vertices1[tri_id1[2]];
+ const Vec3s& t11 = vertices1[tri_id1[0]];
+ const Vec3s& t12 = vertices1[tri_id1[1]];
+ const Vec3s& t13 = vertices1[tri_id1[2]];
- const Vec3f& t21 = vertices2[tri_id2[0]];
- const Vec3f& t22 = vertices2[tri_id2[1]];
- const Vec3f& t23 = vertices2[tri_id2[2]];
+ const Vec3s& t21 = vertices2[tri_id2[0]];
+ const Vec3s& t22 = vertices2[tri_id2[1]];
+ const Vec3s& t23 = vertices2[tri_id2[2]];
// nearest point pair
- Vec3f P1, P2, normal;
+ Vec3s P1, P2, normal;
CoalScalar d2;
if (RTIsIdentity)
@@ -471,8 +471,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
return false;
}
- Vec3f* vertices1;
- Vec3f* vertices2;
+ Vec3s* vertices1;
+ Vec3s* vertices2;
Triangle* tri_indices1;
Triangle* tri_indices2;
@@ -489,8 +489,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
const Triangle& init_tri1 = tri_indices1[init_tri_id1];
const Triangle& init_tri2 = tri_indices2[init_tri_id2];
- Vec3f init_tri1_points[3];
- Vec3f init_tri2_points[3];
+ Vec3s init_tri1_points[3];
+ Vec3s init_tri2_points[3];
init_tri1_points[0] = vertices1[init_tri1[0]];
init_tri1_points[1] = vertices1[init_tri1[1]];
@@ -500,7 +500,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
init_tri2_points[1] = vertices2[init_tri2[1]];
init_tri2_points[2] = vertices2[init_tri2[2]];
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance(
init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
@@ -534,12 +534,12 @@ typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
namespace details {
template <typename BV>
-inline const Matrix3f& getBVAxes(const BV& bv) {
+inline const Matrix3s& getBVAxes(const BV& bv) {
return bv.axes;
}
template <>
-inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
+inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) {
return bv.obb.axes;
}
diff --git a/include/coal/internal/traversal_node_bvh_shape.h b/include/coal/internal/traversal_node_bvh_shape.h
index ec68e528..cff2a801 100644
--- a/include/coal/internal/traversal_node_bvh_shape.h
+++ b/include/coal/internal/traversal_node_bvh_shape.h
@@ -153,7 +153,7 @@ class MeshShapeCollisionTraversalNode
// collision.
const bool compute_penetration =
this->request.enable_contact || (this->request.security_margin < 0);
- Vec3f c1, c2, normal;
+ Vec3s c1, c2, normal;
CoalScalar distance;
if (RTIsIdentity) {
@@ -186,7 +186,7 @@ class MeshShapeCollisionTraversalNode
assert(this->result->isCollision() || sqrDistLowerBound > 0);
} // leafCollides
- Vec3f* vertices;
+ Vec3s* vertices;
Triangle* tri_indices;
const GJKSolver* nsolver;
@@ -308,7 +308,7 @@ class MeshShapeDistanceTraversalNode
const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
this->vertices[tri_id[2]]);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
&tri, this->tf1, this->model2, this->tf2, this->nsolver,
this->request.enable_signed_distance, p1, p2, normal);
@@ -325,7 +325,7 @@ class MeshShapeDistanceTraversalNode
return false;
}
- Vec3f* vertices;
+ Vec3s* vertices;
Triangle* tri_indices;
CoalScalar rel_err;
@@ -340,7 +340,7 @@ namespace details {
template <typename BV, typename S>
void meshShapeDistanceOrientedNodeleafComputeDistance(
unsigned int b1, unsigned int /* b2 */, const BVHModel<BV>* model1,
- const S& model2, Vec3f* vertices, Triangle* tri_indices,
+ const S& model2, Vec3s* vertices, Triangle* tri_indices,
const Transform3f& tf1, const Transform3f& tf2, const GJKSolver* nsolver,
bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request,
DistanceResult& result) {
@@ -353,7 +353,7 @@ void meshShapeDistanceOrientedNodeleafComputeDistance(
const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
vertices[tri_id[2]]);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
&tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
normal);
@@ -364,7 +364,7 @@ void meshShapeDistanceOrientedNodeleafComputeDistance(
template <typename BV, typename S>
static inline void distancePreprocessOrientedNode(
- const BVHModel<BV>* model1, Vec3f* vertices, Triangle* tri_indices,
+ const BVHModel<BV>* model1, Vec3s* vertices, Triangle* tri_indices,
int init_tri_id, const S& model2, const Transform3f& tf1,
const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result) {
@@ -372,7 +372,7 @@ static inline void distancePreprocessOrientedNode(
const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
vertices[tri_id[2]]);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance = internal::ShapeShapeDistance<TriangleP, S>(
&tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
normal);
diff --git a/include/coal/internal/traversal_node_bvhs.h b/include/coal/internal/traversal_node_bvhs.h
index 99a442f3..a7b2446f 100644
--- a/include/coal/internal/traversal_node_bvhs.h
+++ b/include/coal/internal/traversal_node_bvhs.h
@@ -193,12 +193,12 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
const Triangle& tri_id1 = tri_indices1[primitive_id1];
const Triangle& tri_id2 = tri_indices2[primitive_id2];
- const Vec3f& P1 = vertices1[tri_id1[0]];
- const Vec3f& P2 = vertices1[tri_id1[1]];
- const Vec3f& P3 = vertices1[tri_id1[2]];
- const Vec3f& Q1 = vertices2[tri_id2[0]];
- const Vec3f& Q2 = vertices2[tri_id2[1]];
- const Vec3f& Q3 = vertices2[tri_id2[2]];
+ const Vec3s& P1 = vertices1[tri_id1[0]];
+ const Vec3s& P2 = vertices1[tri_id1[1]];
+ const Vec3s& P3 = vertices1[tri_id1[2]];
+ const Vec3s& Q1 = vertices2[tri_id2[0]];
+ const Vec3s& Q2 = vertices2[tri_id2[1]];
+ const Vec3s& Q3 = vertices2[tri_id2[2]];
TriangleP tri1(P1, P2, P3);
TriangleP tri2(Q1, Q2, Q3);
@@ -208,7 +208,7 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
const bool compute_penetration =
this->request.enable_contact || (this->request.security_margin < 0);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
DistanceResult distanceResult;
CoalScalar distance = internal::ShapeShapeDistance<TriangleP, TriangleP>(
&tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1,
@@ -231,8 +231,8 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
sqrDistLowerBound = distToCollision * distToCollision;
}
- Vec3f* vertices1;
- Vec3f* vertices2;
+ Vec3s* vertices1;
+ Vec3s* vertices2;
Triangle* tri_indices1;
Triangle* tri_indices2;
@@ -255,7 +255,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl {
static CoalScalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
return b1.distance(b2);
}
- static CoalScalar run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
+ static CoalScalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1,
const BVNode<BV>& b2) {
return distance(R, T, b1.bv, b2.bv);
}
@@ -273,7 +273,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
}
return sqrt(sqrDistLowerBound);
}
- static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ static CoalScalar run(const Matrix3s& R, const Vec3s& T,
const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
CoalScalar sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
@@ -298,7 +298,7 @@ struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
}
return sqrt(sqrDistLowerBound);
}
- static CoalScalar run(const Matrix3f& R, const Vec3f& T,
+ static CoalScalar run(const Matrix3s& R, const Vec3s& T,
const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
CoalScalar sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
@@ -441,16 +441,16 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
const Triangle& tri_id1 = tri_indices1[primitive_id1];
const Triangle& tri_id2 = tri_indices2[primitive_id2];
- const Vec3f& t11 = vertices1[tri_id1[0]];
- const Vec3f& t12 = vertices1[tri_id1[1]];
- const Vec3f& t13 = vertices1[tri_id1[2]];
+ const Vec3s& t11 = vertices1[tri_id1[0]];
+ const Vec3s& t12 = vertices1[tri_id1[1]];
+ const Vec3s& t13 = vertices1[tri_id1[2]];
- const Vec3f& t21 = vertices2[tri_id2[0]];
- const Vec3f& t22 = vertices2[tri_id2[1]];
- const Vec3f& t23 = vertices2[tri_id2[2]];
+ const Vec3s& t21 = vertices2[tri_id2[0]];
+ const Vec3s& t22 = vertices2[tri_id2[1]];
+ const Vec3s& t23 = vertices2[tri_id2[2]];
// nearest point pair
- Vec3f P1, P2, normal;
+ Vec3s P1, P2, normal;
CoalScalar d2;
if (RTIsIdentity)
@@ -473,8 +473,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
return false;
}
- Vec3f* vertices1;
- Vec3f* vertices2;
+ Vec3s* vertices1;
+ Vec3s* vertices2;
Triangle* tri_indices1;
Triangle* tri_indices2;
@@ -491,8 +491,8 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
const Triangle& init_tri1 = tri_indices1[init_tri_id1];
const Triangle& init_tri2 = tri_indices2[init_tri_id2];
- Vec3f init_tri1_points[3];
- Vec3f init_tri2_points[3];
+ Vec3s init_tri1_points[3];
+ Vec3s init_tri2_points[3];
init_tri1_points[0] = vertices1[init_tri1[0]];
init_tri1_points[1] = vertices1[init_tri1[1]];
@@ -502,7 +502,7 @@ class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
init_tri2_points[1] = vertices2[init_tri2[1]];
init_tri2_points[2] = vertices2[init_tri2[2]];
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
CoalScalar distance = sqrt(TriangleDistance::sqrTriDistance(
init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
@@ -536,12 +536,12 @@ typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
namespace details {
template <typename BV>
-inline const Matrix3f& getBVAxes(const BV& bv) {
+inline const Matrix3s& getBVAxes(const BV& bv) {
return bv.axes;
}
template <>
-inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
+inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) {
return bv.obb.axes;
}
diff --git a/include/coal/internal/traversal_node_hfield_shape.h b/include/coal/internal/traversal_node_hfield_shape.h
index 1b5f6b1e..438f5c8c 100644
--- a/include/coal/internal/traversal_node_hfield_shape.h
+++ b/include/coal/internal/traversal_node_hfield_shape.h
@@ -59,30 +59,30 @@ namespace details {
template <typename BV>
Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
const HeightField<BV>& model) {
- const MatrixXf& heights = model.getHeights();
- const VecXf& x_grid = model.getXGrid();
- const VecXf& y_grid = model.getYGrid();
+ const MatrixXs& heights = model.getHeights();
+ const VecXs& x_grid = model.getXGrid();
+ const VecXs& y_grid = model.getYGrid();
const CoalScalar min_height = model.getMinHeight();
const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
- const Eigen::Block<const MatrixXf, 2, 2> cell =
+ const Eigen::Block<const MatrixXs, 2, 2> cell =
heights.block<2, 2>(node.y_id, node.x_id);
assert(cell.maxCoeff() > min_height &&
"max_height is lower than min_height"); // Check whether the geometry
// is degenerated
- std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
- Vec3f(x0, y0, min_height),
- Vec3f(x0, y1, min_height),
- Vec3f(x1, y1, min_height),
- Vec3f(x1, y0, min_height),
- Vec3f(x0, y0, cell(0, 0)),
- Vec3f(x0, y1, cell(1, 0)),
- Vec3f(x1, y1, cell(1, 1)),
- Vec3f(x1, y0, cell(0, 1)),
+ std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+ Vec3s(x0, y0, min_height),
+ Vec3s(x0, y1, min_height),
+ Vec3s(x1, y1, min_height),
+ Vec3s(x1, y0, min_height),
+ Vec3s(x0, y0, cell(0, 0)),
+ Vec3s(x0, y1, cell(1, 0)),
+ Vec3s(x1, y1, cell(1, 1)),
+ Vec3s(x1, y0, cell(0, 1)),
}));
std::shared_ptr<std::vector<Quadrilateral>> polygons(
@@ -122,16 +122,16 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
Convex<Triangle>& convex1, int& convex1_active_faces,
Convex<Triangle>& convex2,
int& convex2_active_faces) {
- const MatrixXf& heights = model.getHeights();
- const VecXf& x_grid = model.getXGrid();
- const VecXf& y_grid = model.getYGrid();
+ const MatrixXs& heights = model.getHeights();
+ const VecXs& x_grid = model.getXGrid();
+ const VecXs& y_grid = model.getYGrid();
const CoalScalar min_height = model.getMinHeight();
const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
const CoalScalar max_height = node.max_height;
- const Eigen::Block<const MatrixXf, 2, 2> cell =
+ const Eigen::Block<const MatrixXs, 2, 2> cell =
heights.block<2, 2>(node.y_id, node.x_id);
const int contact_active_faces = node.contact_active_faces;
@@ -174,13 +174,13 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
COAL_UNUSED_VARIABLE(max_height);
{
- std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
- Vec3f(x0, y0, min_height), // A
- Vec3f(x0, y1, min_height), // B
- Vec3f(x1, y0, min_height), // C
- Vec3f(x0, y0, cell(0, 0)), // D
- Vec3f(x0, y1, cell(1, 0)), // E
- Vec3f(x1, y0, cell(0, 1)), // F
+ std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+ Vec3s(x0, y0, min_height), // A
+ Vec3s(x0, y1, min_height), // B
+ Vec3s(x1, y0, min_height), // C
+ Vec3s(x0, y0, cell(0, 0)), // D
+ Vec3s(x0, y1, cell(1, 0)), // E
+ Vec3s(x1, y0, cell(0, 1)), // F
}));
std::shared_ptr<std::vector<Triangle>> triangles(
@@ -202,13 +202,13 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
}
{
- std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
- Vec3f(x0, y1, min_height), // A
- Vec3f(x1, y1, min_height), // B
- Vec3f(x1, y0, min_height), // C
- Vec3f(x0, y1, cell(1, 0)), // D
- Vec3f(x1, y1, cell(1, 1)), // E
- Vec3f(x1, y0, cell(0, 1)), // F
+ std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+ Vec3s(x0, y1, min_height), // A
+ Vec3s(x1, y1, min_height), // B
+ Vec3s(x1, y0, min_height), // C
+ Vec3s(x0, y1, cell(1, 0)), // D
+ Vec3s(x1, y1, cell(1, 1)), // E
+ Vec3s(x1, y0, cell(0, 1)), // F
}));
std::shared_ptr<std::vector<Triangle>> triangles(
@@ -230,23 +230,23 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
}
}
-inline Vec3f projectTriangle(const Vec3f& pointA, const Vec3f& pointB,
- const Vec3f& pointC, const Vec3f& point) {
+inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB,
+ const Vec3s& pointC, const Vec3s& point) {
const Project::ProjectResult result =
Project::projectTriangle(pointA, pointB, pointC, point);
- Vec3f res = result.parameterization[0] * pointA +
+ Vec3s res = result.parameterization[0] * pointA +
result.parameterization[1] * pointB +
result.parameterization[2] * pointC;
return res;
}
-inline Vec3f projectTetrahedra(const Vec3f& pointA, const Vec3f& pointB,
- const Vec3f& pointC, const Vec3f& pointD,
- const Vec3f& point) {
+inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB,
+ const Vec3s& pointC, const Vec3s& pointD,
+ const Vec3s& point) {
const Project::ProjectResult result =
Project::projectTetrahedra(pointA, pointB, pointC, pointD, point);
- Vec3f res = result.parameterization[0] * pointA +
+ Vec3s res = result.parameterization[0] * pointA +
result.parameterization[1] * pointB +
result.parameterization[2] * pointC +
result.parameterization[3] * pointD;
@@ -254,46 +254,46 @@ inline Vec3f projectTetrahedra(const Vec3f& pointA, const Vec3f& pointB,
return res;
}
-inline Vec3f computeTriangleNormal(const Triangle& triangle,
- const std::vector<Vec3f>& points) {
- const Vec3f pointA = points[triangle[0]];
- const Vec3f pointB = points[triangle[1]];
- const Vec3f pointC = points[triangle[2]];
+inline Vec3s computeTriangleNormal(const Triangle& triangle,
+ const std::vector<Vec3s>& points) {
+ const Vec3s pointA = points[triangle[0]];
+ const Vec3s pointB = points[triangle[1]];
+ const Vec3s pointC = points[triangle[2]];
- const Vec3f normal = (pointB - pointA).cross(pointC - pointA).normalized();
+ const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized();
assert(!normal.array().isNaN().any() && "normal is ill-defined");
return normal;
}
-inline Vec3f projectPointOnTriangle(const Vec3f& contact_point,
+inline Vec3s projectPointOnTriangle(const Vec3s& contact_point,
const Triangle& triangle,
- const std::vector<Vec3f>& points) {
- const Vec3f pointA = points[triangle[0]];
- const Vec3f pointB = points[triangle[1]];
- const Vec3f pointC = points[triangle[2]];
+ const std::vector<Vec3s>& points) {
+ const Vec3s pointA = points[triangle[0]];
+ const Vec3s pointB = points[triangle[1]];
+ const Vec3s pointC = points[triangle[2]];
- const Vec3f contact_point_projected =
+ const Vec3s contact_point_projected =
projectTriangle(pointA, pointB, pointC, contact_point);
return contact_point_projected;
}
inline CoalScalar distanceContactPointToTriangle(
- const Vec3f& contact_point, const Triangle& triangle,
- const std::vector<Vec3f>& points) {
- const Vec3f contact_point_projected =
+ const Vec3s& contact_point, const Triangle& triangle,
+ const std::vector<Vec3s>& points) {
+ const Vec3s contact_point_projected =
projectPointOnTriangle(contact_point, triangle, points);
return (contact_point_projected - contact_point).norm();
}
inline CoalScalar distanceContactPointToFace(const size_t face_id,
- const Vec3f& contact_point,
+ const Vec3s& contact_point,
const Convex<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);
+ const std::vector<Vec3s>& points = *(convex.points);
if (face_id <= 1) {
const Triangle& triangle = (*(convex.polygons))[face_id];
closest_face_id = face_id;
@@ -321,10 +321,10 @@ template <typename Polygone, typename Shape>
bool binCorrection(const Convex<Polygone>& convex,
const int convex_active_faces, const Shape& shape,
const Transform3f& shape_pose, CoalScalar& distance,
- Vec3f& contact_1, Vec3f& contact_2, Vec3f& normal,
- Vec3f& face_normal, const bool is_collision) {
+ Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal,
+ Vec3s& face_normal, const bool is_collision) {
const CoalScalar prec = 1e-12;
- const std::vector<Vec3f>& points = *(convex.points);
+ const std::vector<Vec3s>& points = *(convex.points);
bool hfield_witness_is_on_bin_side = true;
@@ -366,16 +366,16 @@ bool binCorrection(const Convex<Polygone>& convex,
if (!face_triangle.isValid())
COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error);
- const Vec3f face_pointA = points[face_triangle[0]];
+ const Vec3s face_pointA = points[face_triangle[0]];
face_normal = computeTriangleNormal(face_triangle, points);
int hint = 0;
// Since we compute the support manually, we need to take into account the
// sphere swept radius of the shape.
// TODO: take into account the swept-sphere radius of the bin.
- const Vec3f _support = getSupport<details::SupportOptions::WithSweptSphere>(
+ const Vec3s _support = getSupport<details::SupportOptions::WithSweptSphere>(
&shape, -shape_pose.rotation().transpose() * face_normal, hint);
- const Vec3f support =
+ const Vec3s support =
shape_pose.rotation() * _support + shape_pose.translation();
// Project support into the inclined bin having triangle
@@ -384,7 +384,7 @@ bool binCorrection(const Convex<Polygone>& convex,
const CoalScalar distance_support_projection_plane =
projection_plane.signedDistance(support);
- const Vec3f projected_support =
+ const Vec3s projected_support =
support - distance_support_projection_plane * face_normal;
// We need now to project the projected in the triangle shape
@@ -405,8 +405,8 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
const Convex<Polygone>& convex2,
const int convex2_active_faces, const Transform3f& tf1,
const Shape& shape, const Transform3f& tf2,
- CoalScalar& distance, Vec3f& c1, Vec3f& c2, Vec3f& normal,
- Vec3f& normal_top, bool& hfield_witness_is_on_bin_side) {
+ CoalScalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal,
+ Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) {
enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
const Transform3f Id;
@@ -416,8 +416,8 @@ bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
// The only thing we need to make sure is that in case of collision, the
// penetration information is computed (as we do bins comparison).
const bool compute_penetration = true;
- Vec3f contact1_1, contact1_2, contact2_1, contact2_2;
- Vec3f normal1, normal1_top, normal2, normal2_top;
+ Vec3s contact1_1, contact1_2, contact2_1, contact2_2;
+ Vec3s normal1, normal1_top, normal2, normal2_top;
CoalScalar distance1, distance2;
if (RTIsIdentity) {
@@ -608,8 +608,8 @@ class HeightFieldShapeCollisionTraversalNode
}
CoalScalar distance;
- // Vec3f contact_point, normal;
- Vec3f c1, c2, normal, normal_face;
+ // Vec3s contact_point, normal;
+ Vec3s c1, c2, normal, normal_face;
bool hfield_witness_is_on_bin_side;
bool collision = details::shapeDistance<Triangle, S, Options>(
@@ -632,8 +632,8 @@ class HeightFieldShapeCollisionTraversalNode
} else
sqrDistLowerBound = distToCollision * distToCollision;
- // const Vec3f c1 = contact_point - distance * 0.5 * normal;
- // const Vec3f c2 = contact_point + distance * 0.5 * normal;
+ // const Vec3s c1 = contact_point - distance * 0.5 * normal;
+ // const Vec3s c2 = contact_point + distance * 0.5 * normal;
internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
distToCollision, c1, c2, normal);
@@ -715,7 +715,7 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
const ConvexQuadrilateral convex =
details::buildConvexQuadrilateral(node, *this->model1);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance =
internal::ShapeShapeDistance<ConvexQuadrilateral, S>(
&convex, this->tf1, this->model2, this->tf2, this->nsolver,
diff --git a/include/coal/internal/traversal_node_octree.h b/include/coal/internal/traversal_node_octree.h
index 29cefe59..f9b49722 100644
--- a/include/coal/internal/traversal_node_octree.h
+++ b/include/coal/internal/traversal_node_octree.h
@@ -259,7 +259,7 @@ class COAL_DLLAPI OcTreeSolver {
box.computeLocalAABB();
}
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance = internal::ShapeShapeDistance<Box, S>(
&box, box_tf, &s, tf2, this->solver,
this->drequest->enable_signed_distance, p1, p2, normal);
@@ -390,7 +390,7 @@ class COAL_DLLAPI OcTreeSolver {
(*(tree2->vertices))[tri_id[1]],
(*(tree2->vertices))[tri_id[2]]);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance =
internal::ShapeShapeDistance<Box, TriangleP>(
&box, box_tf, &tri, tf2, this->solver,
@@ -515,7 +515,7 @@ class COAL_DLLAPI OcTreeSolver {
// collision.
const bool compute_penetration = this->crequest->enable_contact ||
(this->crequest->security_margin < 0);
- Vec3f c1, c2, normal;
+ Vec3s c1, c2, normal;
const CoalScalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
&box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
normal);
@@ -618,7 +618,7 @@ class COAL_DLLAPI OcTreeSolver {
convex2.computeLocalAABB();
}
- Vec3f c1, c2, normal, normal_top;
+ Vec3s c1, c2, normal, normal_top;
CoalScalar distance;
bool hfield_witness_is_on_bin_side;
@@ -643,8 +643,8 @@ class COAL_DLLAPI OcTreeSolver {
} else
sqrDistLowerBound = distToCollision * distToCollision;
- // const Vec3f c1 = contact_point - distance * 0.5 * normal;
- // const Vec3f c2 = contact_point + distance * 0.5 * normal;
+ // const Vec3s c1 = contact_point - distance * 0.5 * normal;
+ // const Vec3s c2 = contact_point + distance * 0.5 * normal;
internal::updateDistanceLowerBoundFromLeaf(
*crequest, *cresult, distToCollision, c1, c2, -normal);
@@ -738,7 +738,7 @@ class COAL_DLLAPI OcTreeSolver {
convex2.computeLocalAABB();
}
- Vec3f c1, c2, normal, normal_top;
+ Vec3s c1, c2, normal, normal_top;
CoalScalar distance;
bool hfield_witness_is_on_bin_side;
@@ -763,8 +763,8 @@ class COAL_DLLAPI OcTreeSolver {
} else
sqrDistLowerBound = distToCollision * distToCollision;
- // const Vec3f c1 = contact_point - distance * 0.5 * normal;
- // const Vec3f c2 = contact_point + distance * 0.5 * normal;
+ // const Vec3s c1 = contact_point - distance * 0.5 * normal;
+ // const Vec3s c2 = contact_point + distance * 0.5 * normal;
internal::updateDistanceLowerBoundFromLeaf(
*crequest, *cresult, distToCollision, c1, c2, normal);
@@ -820,7 +820,7 @@ class COAL_DLLAPI OcTreeSolver {
box2.computeLocalAABB();
}
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
const CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
&box1, box1_tf, &box2, box2_tf, this->solver,
this->drequest->enable_signed_distance, p1, p2, normal);
@@ -948,7 +948,7 @@ class COAL_DLLAPI OcTreeSolver {
// collision.
const bool compute_penetration = (this->crequest->enable_contact ||
(this->crequest->security_margin < 0));
- Vec3f c1, c2, normal;
+ Vec3s c1, c2, normal;
CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
&box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
c2, normal);
diff --git a/include/coal/internal/traversal_node_setup.h b/include/coal/internal/traversal_node_setup.h
index 6a84580a..6f60609e 100644
--- a/include/coal/internal/traversal_node_setup.h
+++ b/include/coal/internal/traversal_node_setup.h
@@ -340,11 +340,11 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
if (!tf1.isIdentity() &&
model1.vertices.get()) // TODO(jcarpent): vectorized version
{
- std::vector<Vec3f> vertices_transformed(model1.num_vertices);
- const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
+ std::vector<Vec3s> vertices_transformed(model1.num_vertices);
+ const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- const Vec3f& p = model1_vertices_[i];
- Vec3f new_v = tf1.transform(p);
+ const Vec3s& p = model1_vertices_[i];
+ Vec3s new_v = tf1.transform(p);
vertices_transformed[i] = new_v;
}
@@ -479,11 +479,11 @@ bool initialize(
std::invalid_argument)
if (!tf1.isIdentity() && model1.vertices.get()) {
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
+ std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+ const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- const Vec3f& p = model1_vertices_[i];
- Vec3f new_v = tf1.transform(p);
+ const Vec3s& p = model1_vertices_[i];
+ Vec3s new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
@@ -495,11 +495,11 @@ bool initialize(
}
if (!tf2.isIdentity() && model2.vertices.get()) {
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
+ std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
+ const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
for (unsigned int i = 0; i < model2.num_vertices; ++i) {
- const Vec3f& p = model2_vertices_[i];
- Vec3f new_v = tf2.transform(p);
+ const Vec3s& p = model2_vertices_[i];
+ Vec3s new_v = tf2.transform(p);
vertices_transformed2[i] = new_v;
}
@@ -600,11 +600,11 @@ bool initialize(
std::invalid_argument)
if (!tf1.isIdentity() && model1.vertices.get()) {
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
+ std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
+ const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- const Vec3f& p = model1_vertices_[i];
- Vec3f new_v = tf1.transform(p);
+ const Vec3s& p = model1_vertices_[i];
+ Vec3s new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
@@ -616,11 +616,11 @@ bool initialize(
}
if (!tf2.isIdentity() && model2.vertices.get()) {
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- const std::vector<Vec3f>& model2_vertices_ = *(model2.vertices);
+ std::vector<Vec3s> vertices_transformed2(model2.num_vertices);
+ const std::vector<Vec3s>& model2_vertices_ = *(model2.vertices);
for (unsigned int i = 0; i < model2.num_vertices; ++i) {
- const Vec3f& p = model2_vertices_[i];
- Vec3f new_v = tf2.transform(p);
+ const Vec3s& p = model2_vertices_[i];
+ Vec3s new_v = tf2.transform(p);
vertices_transformed2[i] = new_v;
}
@@ -706,11 +706,11 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
std::invalid_argument)
if (!tf1.isIdentity() && model1.vertices.get()) {
- const std::vector<Vec3f>& model1_vertices_ = *(model1.vertices);
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
+ const std::vector<Vec3s>& model1_vertices_ = *(model1.vertices);
+ std::vector<Vec3s> vertices_transformed1(model1.num_vertices);
for (unsigned int i = 0; i < model1.num_vertices; ++i) {
- const Vec3f& p = model1_vertices_[i];
- Vec3f new_v = tf1.transform(p);
+ const Vec3s& p = model1_vertices_[i];
+ Vec3s new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
diff --git a/include/coal/math/transform.h b/include/coal/math/transform.h
index 3bfcd0c3..ff96bac1 100644
--- a/include/coal/math/transform.h
+++ b/include/coal/math/transform.h
@@ -54,10 +54,10 @@ static inline std::ostream& operator<<(std::ostream& o, const Quatf& q) {
/// @brief Simple transform class used locally by InterpMotion
class COAL_DLLAPI Transform3f {
/// @brief Matrix cache
- Matrix3f R;
+ Matrix3s R;
/// @brief Translation vector
- Vec3f T;
+ Vec3s T;
public:
/// @brief Default transform is no movement
@@ -79,13 +79,13 @@ class COAL_DLLAPI Transform3f {
: R(q_.toRotationMatrix()), T(T_) {}
/// @brief Construct transform from rotation
- Transform3f(const Matrix3f& R_) : R(R_), T(Vec3f::Zero()) {}
+ Transform3f(const Matrix3s& R_) : R(R_), T(Vec3s::Zero()) {}
/// @brief Construct transform from rotation
- Transform3f(const Quatf& q_) : R(q_), T(Vec3f::Zero()) {}
+ Transform3f(const Quatf& q_) : R(q_), T(Vec3s::Zero()) {}
/// @brief Construct transform from translation
- Transform3f(const Vec3f& T_) : R(Matrix3f::Identity()), T(T_) {}
+ Transform3f(const Vec3s& T_) : R(Matrix3s::Identity()), T(T_) {}
/// @brief Construct transform from other transform
Transform3f(const Transform3f& tf) : R(tf.R), T(tf.T) {}
@@ -98,22 +98,22 @@ class COAL_DLLAPI Transform3f {
}
/// @brief get translation
- inline const Vec3f& getTranslation() const { return T; }
+ inline const Vec3s& getTranslation() const { return T; }
/// @brief get translation
- inline const Vec3f& translation() const { return T; }
+ inline const Vec3s& translation() const { return T; }
/// @brief get translation
- inline Vec3f& translation() { return T; }
+ inline Vec3s& translation() { return T; }
/// @brief get rotation
- inline const Matrix3f& getRotation() const { return R; }
+ inline const Matrix3s& getRotation() const { return R; }
/// @brief get rotation
- inline const Matrix3f& rotation() const { return R; }
+ inline const Matrix3s& rotation() const { return R; }
/// @brief get rotation
- inline Matrix3f& rotation() { return R; }
+ inline Matrix3s& rotation() { return R; }
/// @brief get quaternion
inline Quatf getQuatRotation() const { return Quatf(R); }
@@ -127,7 +127,7 @@ class COAL_DLLAPI Transform3f {
}
/// @brief set transform from rotation and translation
- inline void setTransform(const Quatf& q_, const Vec3f& T_) {
+ inline void setTransform(const Quatf& q_, const Vec3s& T_) {
R = q_.toRotationMatrix();
T = T_;
}
@@ -149,13 +149,13 @@ class COAL_DLLAPI Transform3f {
/// @brief transform a given vector by the transform
template <typename Derived>
- inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const {
+ inline Vec3s transform(const Eigen::MatrixBase<Derived>& v) const {
return R * v + T;
}
/// @brief transform a given vector by the inverse of the transform
template <typename Derived>
- inline Vec3f inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
+ inline Vec3s inverseTransform(const Eigen::MatrixBase<Derived>& v) const {
return R.transpose() * (v - T);
}
@@ -257,8 +257,8 @@ inline Transform3f& Transform3f::setRandom() {
/// @brief Construct othonormal basis from vector.
/// The z-axis is the normalized input vector.
-inline Matrix3f constructOrthonormalBasisFromVector(const Vec3f& vec) {
- Matrix3f basis = Matrix3f::Zero();
+inline Matrix3s constructOrthonormalBasisFromVector(const Vec3s& vec) {
+ Matrix3s basis = Matrix3s::Zero();
basis.col(2) = vec.normalized();
basis.col(1) = -vec.unitOrthogonal();
basis.col(0) = basis.col(1).cross(vec);
diff --git a/include/coal/mesh_loader/assimp.h b/include/coal/mesh_loader/assimp.h
index 2be9cfc9..0133f456 100644
--- a/include/coal/mesh_loader/assimp.h
+++ b/include/coal/mesh_loader/assimp.h
@@ -53,7 +53,7 @@ namespace coal {
namespace internal {
struct COAL_DLLAPI TriangleAndVertices {
- std::vector<coal::Vec3f> vertices_;
+ std::vector<coal::Vec3s> vertices_;
std::vector<coal::Triangle> triangles_;
};
@@ -75,7 +75,7 @@ struct COAL_DLLAPI Loader {
* @param[in] vertices_offset Current number of vertices in the model
* @param tv Triangles and Vertices of the mesh submodels
*/
-COAL_DLLAPI void buildMesh(const coal::Vec3f& scale, const aiScene* scene,
+COAL_DLLAPI void buildMesh(const coal::Vec3s& scale, const aiScene* scene,
unsigned vertices_offset, TriangleAndVertices& tv);
/**
@@ -87,7 +87,7 @@ COAL_DLLAPI void buildMesh(const coal::Vec3f& scale, const aiScene* scene,
*/
template <class BoundingVolume>
inline void meshFromAssimpScene(
- const coal::Vec3f& scale, const aiScene* scene,
+ const coal::Vec3s& scale, const aiScene* scene,
const shared_ptr<BVHModel<BoundingVolume> >& mesh) {
TriangleAndVertices tv;
@@ -114,7 +114,7 @@ inline void meshFromAssimpScene(
*/
template <class BoundingVolume>
inline void loadPolyhedronFromResource(
- const std::string& resource_path, const coal::Vec3f& scale,
+ const std::string& resource_path, const coal::Vec3s& scale,
const shared_ptr<BVHModel<BoundingVolume> >& polyhedron) {
internal::Loader scene;
scene.load(resource_path);
diff --git a/include/coal/mesh_loader/loader.h b/include/coal/mesh_loader/loader.h
index c24f48d0..18b2255b 100644
--- a/include/coal/mesh_loader/loader.h
+++ b/include/coal/mesh_loader/loader.h
@@ -54,7 +54,7 @@ class COAL_DLLAPI MeshLoader {
virtual ~MeshLoader() {}
virtual BVHModelPtr_t load(const std::string& filename,
- const Vec3f& scale = Vec3f::Ones());
+ const Vec3s& scale = Vec3s::Ones());
/// Create an OcTree from a file in binary octomap format.
/// \todo add OctreePtr_t
@@ -76,13 +76,13 @@ class COAL_DLLAPI CachedMeshLoader : public MeshLoader {
CachedMeshLoader(const NODE_TYPE& bvType = BV_OBBRSS) : MeshLoader(bvType) {}
- virtual BVHModelPtr_t load(const std::string& filename, const Vec3f& scale);
+ virtual BVHModelPtr_t load(const std::string& filename, const Vec3s& scale);
struct COAL_DLLAPI Key {
std::string filename;
- Vec3f scale;
+ Vec3s scale;
- Key(const std::string& f, const Vec3f& s) : filename(f), scale(s) {}
+ Key(const std::string& f, const Vec3s& s) : filename(f), scale(s) {}
bool operator<(const CachedMeshLoader::Key& b) const;
};
diff --git a/include/coal/narrowphase/gjk.h b/include/coal/narrowphase/gjk.h
index ae92c05e..ea8ed8f0 100644
--- a/include/coal/narrowphase/gjk.h
+++ b/include/coal/narrowphase/gjk.h
@@ -53,10 +53,10 @@ namespace details {
struct COAL_DLLAPI GJK {
struct COAL_DLLAPI SimplexV {
/// @brief support vector for shape 0 and 1.
- Vec3f w0, w1;
+ Vec3s w0, w1;
/// @brief support vector (i.e., the furthest point on the shape along the
/// support direction)
- Vec3f w;
+ Vec3s w;
};
typedef unsigned char vertex_id_t;
@@ -108,7 +108,7 @@ struct COAL_DLLAPI GJK {
GJKConvergenceCriterionType convergence_criterion_type;
MinkowskiDiff const* shape;
- Vec3f ray;
+ Vec3s ray;
support_func_guess_t support_hint;
/// @brief The distance between the two shapes, computed by GJK.
/// If the distance is below GJK's threshold, the shapes are in collision in
@@ -154,12 +154,12 @@ struct COAL_DLLAPI GJK {
/// @brief GJK algorithm, given the initial value guess
Status evaluate(
- const MinkowskiDiff& shape, const Vec3f& guess,
+ const MinkowskiDiff& shape, const Vec3s& guess,
const support_func_guess_t& supportHint = support_func_guess_t::Zero());
/// @brief apply the support function along a direction, the result is return
/// in sv
- inline void getSupport(const Vec3f& d, SimplexV& sv,
+ inline void getSupport(const Vec3s& d, SimplexV& sv,
support_func_guess_t& hint) const {
shape->support(d, sv.w0, sv.w1, hint);
sv.w = sv.w0 - sv.w1;
@@ -181,11 +181,11 @@ struct COAL_DLLAPI GJK {
/// @param[out] w1 is the witness point on shape1.
/// @param[out] normal is the normal of the separating plane found by
/// GJK. It points from shape0 to shape1.
- void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
- Vec3f& w1, Vec3f& normal) const;
+ void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+ Vec3s& w1, Vec3s& normal) const;
/// @brief get the guess from current simplex
- Vec3f getGuessFromSimplex() const;
+ Vec3s getGuessFromSimplex() const;
/// @brief Distance threshold for early break.
/// GJK stops when it proved the distance is more than this threshold.
@@ -197,7 +197,7 @@ struct COAL_DLLAPI GJK {
/// @brief Convergence check used to stop GJK when shapes are not in
/// collision.
- bool checkConvergence(const Vec3f& w, const CoalScalar& rl, CoalScalar& alpha,
+ bool checkConvergence(const Vec3s& w, const CoalScalar& rl, CoalScalar& alpha,
const CoalScalar& omega) const;
/// @brief Get the max number of iterations of GJK.
@@ -225,7 +225,7 @@ struct COAL_DLLAPI GJK {
inline void removeVertex(Simplex& simplex);
/// @brief append one vertex to the simplex
- inline void appendVertex(Simplex& simplex, const Vec3f& v,
+ inline void appendVertex(Simplex& simplex, const Vec3s& v,
support_func_guess_t& hint);
/// @brief Project origin (0) onto line a-b
@@ -258,7 +258,7 @@ struct COAL_DLLAPI GJK {
struct COAL_DLLAPI EPA {
typedef GJK::SimplexV SimplexVertex;
struct COAL_DLLAPI SimplexFace {
- Vec3f n;
+ Vec3s n;
CoalScalar d;
bool ignore; // If the origin does not project inside the face, we
// ignore this face.
@@ -272,7 +272,7 @@ struct COAL_DLLAPI EPA {
// (with 0 <= i <= 2).
size_t pass;
- SimplexFace() : n(Vec3f::Zero()), ignore(false) {};
+ SimplexFace() : n(Vec3s::Zero()), ignore(false) {};
};
/// @brief The simplex list of EPA is a linked list of faces.
@@ -342,7 +342,7 @@ struct COAL_DLLAPI EPA {
public:
Status status;
GJK::Simplex result;
- Vec3f normal;
+ Vec3s normal;
support_func_guess_t support_hint;
CoalScalar depth;
SimplexFace* closest_face;
@@ -409,7 +409,7 @@ struct COAL_DLLAPI EPA {
/// \return a Status which can be demangled using (status & Valid) or
/// (status & Failed). The other values provide a more detailled
/// status
- Status evaluate(GJK& gjk, const Vec3f& guess);
+ Status evaluate(GJK& gjk, const Vec3s& guess);
/// Get the witness points on each object, and the corresponding normal.
/// @param[in] shape is the Minkowski difference of the two shapes.
@@ -418,8 +418,8 @@ struct COAL_DLLAPI EPA {
/// @param[in] normal is the normal found by EPA. It points from shape0 to
/// shape1. The normal is used to correct the witness points on the shapes if
/// the shapes have a non-zero swept-sphere radius.
- void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
- Vec3f& w1, Vec3f& normal) const;
+ void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+ Vec3s& w1, Vec3s& normal) const;
private:
/// @brief Allocates memory for the EPA algorithm.
diff --git a/include/coal/narrowphase/minkowski_difference.h b/include/coal/narrowphase/minkowski_difference.h
index c0af9424..3449301a 100644
--- a/include/coal/narrowphase/minkowski_difference.h
+++ b/include/coal/narrowphase/minkowski_difference.h
@@ -62,11 +62,11 @@ struct COAL_DLLAPI MinkowskiDiff {
/// @brief rotation from shape1 to shape0
/// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
- Matrix3f oR1;
+ Matrix3s oR1;
/// @brief translation from shape1 to shape0
/// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
- Vec3f ot1;
+ Vec3s ot1;
/// @brief The radii of the sphere swepted around each shape of the Minkowski
/// difference. The 2 values correspond to the swept-sphere radius of shape 0
@@ -79,8 +79,8 @@ struct COAL_DLLAPI MinkowskiDiff {
bool normalize_support_direction;
typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
- const Vec3f& dir, Vec3f& support0,
- Vec3f& support1,
+ const Vec3s& dir, Vec3s& support0,
+ Vec3s& support1,
support_func_guess_t& hint,
ShapeSupportData data[2]);
GetSupportFunction getSupportFunc;
@@ -139,7 +139,7 @@ struct COAL_DLLAPI MinkowskiDiff {
/// @tparam `SupportOptions` see `set(const ShapeBase*, const
/// ShapeBase*)` for more details.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
- inline Vec3f support0(const Vec3f& dir, int& hint) const {
+ inline Vec3s support0(const Vec3s& dir, int& hint) const {
return getSupport<_SupportOptions>(shapes[0], dir, hint);
}
@@ -156,7 +156,7 @@ struct COAL_DLLAPI MinkowskiDiff {
/// @tparam `SupportOptions` see `set(const ShapeBase*, const
/// ShapeBase*)` for more details.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
- inline Vec3f support1(const Vec3f& dir, int& hint) const {
+ inline Vec3s support1(const Vec3s& dir, int& hint) const {
// clang-format off
return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1;
// clang-format on
@@ -171,7 +171,7 @@ struct COAL_DLLAPI MinkowskiDiff {
/// frame of shape0.
/// @param[in/out] hint used to initialize the search when shape is a
/// ConvexBase object.
- inline void support(const Vec3f& dir, Vec3f& supp0, Vec3f& supp1,
+ inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1,
support_func_guess_t& hint) const {
assert(getSupportFunc != NULL);
getSupportFunc(*this, dir, supp0, supp1, hint,
diff --git a/include/coal/narrowphase/narrowphase.h b/include/coal/narrowphase/narrowphase.h
index 261917cc..03046b45 100644
--- a/include/coal/narrowphase/narrowphase.h
+++ b/include/coal/narrowphase/narrowphase.h
@@ -76,7 +76,7 @@ struct COAL_DLLAPI GJKSolver {
bool enable_cached_guess;
/// @brief smart guess
- mutable Vec3f cached_guess;
+ mutable Vec3s cached_guess;
/// @brief smart guess for the support function
mutable support_func_guess_t support_func_cached_guess;
@@ -126,7 +126,7 @@ struct COAL_DLLAPI GJKSolver {
gjk_tolerance(GJK_DEFAULT_TOLERANCE),
gjk_initial_guess(GJKInitialGuess::DefaultGuess),
enable_cached_guess(false), // Use gjk_initial_guess instead
- cached_guess(Vec3f(1, 0, 0)),
+ cached_guess(Vec3s(1, 0, 0)),
support_func_cached_guess(support_func_guess_t::Zero()),
distance_upper_bound((std::numeric_limits<CoalScalar>::max)()),
gjk_variant(GJKVariant::DefaultGJK),
@@ -148,7 +148,7 @@ struct COAL_DLLAPI GJKSolver {
explicit GJKSolver(const DistanceRequest& request)
: gjk(request.gjk_max_iterations, request.gjk_tolerance),
epa(0, request.epa_tolerance) {
- this->cached_guess = Vec3f(1, 0, 0);
+ this->cached_guess = Vec3s(1, 0, 0);
this->support_func_cached_guess = support_func_guess_t::Zero();
set(request);
@@ -200,7 +200,7 @@ struct COAL_DLLAPI GJKSolver {
explicit GJKSolver(const CollisionRequest& request)
: gjk(request.gjk_max_iterations, request.gjk_tolerance),
epa(0, request.epa_tolerance) {
- this->cached_guess = Vec3f(1, 0, 0);
+ this->cached_guess = Vec3s(1, 0, 0);
this->support_func_cached_guess = support_func_guess_t::Zero();
set(request);
@@ -307,8 +307,8 @@ struct COAL_DLLAPI GJKSolver {
template <typename S1, typename S2>
CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2,
const Transform3f& tf2,
- const bool compute_penetration, Vec3f& p1, Vec3f& p2,
- Vec3f& normal) const {
+ const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+ Vec3s& normal) const {
constexpr bool relative_transformation_already_computed = false;
CoalScalar distance;
this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2,
@@ -322,8 +322,8 @@ struct COAL_DLLAPI GJKSolver {
template <typename S1>
CoalScalar shapeDistance(const S1& s1, const Transform3f& tf1,
const TriangleP& s2, const Transform3f& tf2,
- const bool compute_penetration, Vec3f& p1, Vec3f& p2,
- Vec3f& normal) const {
+ const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+ Vec3s& normal) const {
const Transform3f tf_1M2(tf1.inverseTimes(tf2));
TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b),
tf_1M2.transform(s2.c));
@@ -339,8 +339,8 @@ struct COAL_DLLAPI GJKSolver {
template <typename 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 {
+ const bool compute_penetration, Vec3s& p1, Vec3s& p2,
+ Vec3s& normal) const {
CoalScalar distance = this->shapeDistance<S2>(
s2, tf2, s1, tf1, compute_penetration, p2, p1, normal);
normal = -normal;
@@ -351,9 +351,9 @@ struct COAL_DLLAPI GJKSolver {
/// @brief initialize GJK.
/// This method assumes `minkowski_difference` has been set.
template <typename S1, typename S2>
- void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3f& guess,
+ void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess,
support_func_guess_t& support_hint,
- const Vec3f& default_guess = Vec3f(1, 0, 0)) const {
+ const Vec3s& default_guess = Vec3s(1, 0, 0)) const {
// There is no reason not to warm-start the support function, so we always
// do it.
support_hint = this->support_func_cached_guess;
@@ -422,7 +422,7 @@ struct COAL_DLLAPI GJKSolver {
void runGJKAndEPA(
const S1& s1, const Transform3f& tf1, const S2& s2,
const Transform3f& tf2, const bool compute_penetration,
- CoalScalar& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal,
+ CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal,
const bool relative_transformation_already_computed = false) const {
// Reset internal state of GJK algorithm
if (relative_transformation_already_computed)
@@ -437,7 +437,7 @@ struct COAL_DLLAPI GJKSolver {
this->epa.status = details::EPA::Status::DidNotRun;
// Get initial guess for GJK: default, cached or bounding volume guess
- Vec3f guess;
+ Vec3s guess;
support_func_guess_t support_hint;
getGJKInitialGuess(*(this->minkowski_difference.shapes[0]),
*(this->minkowski_difference.shapes[1]), guess,
@@ -449,11 +449,11 @@ struct COAL_DLLAPI GJKSolver {
case details::GJK::DidNotRun:
COAL_ASSERT(false, "GJK did not run. It should have!",
std::logic_error);
- this->cached_guess = Vec3f(1, 0, 0);
+ this->cached_guess = Vec3s(1, 0, 0);
this->support_func_cached_guess.setZero();
distance = -(std::numeric_limits<CoalScalar>::max)();
p1 = p2 = normal =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
break;
case details::GJK::Failed:
//
@@ -588,8 +588,8 @@ struct COAL_DLLAPI GJKSolver {
void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3f& tf1,
CoalScalar& distance,
- Vec3f& p1, Vec3f& p2,
- Vec3f& normal) const {
+ Vec3s& p1, Vec3s& p2,
+ Vec3s& normal) const {
COAL_UNUSED_VARIABLE(tf1);
// Cache gjk result for potential future call to this GJKSolver.
this->cached_guess = this->gjk.ray;
@@ -597,7 +597,7 @@ struct COAL_DLLAPI GJKSolver {
distance = this->gjk.distance;
p1 = p2 = normal =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::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):
@@ -608,8 +608,8 @@ struct COAL_DLLAPI GJKSolver {
}
void GJKExtractWitnessPointsAndNormal(const Transform3f& tf1,
- CoalScalar& distance, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) const {
+ CoalScalar& distance, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) const {
// Apart from early stopping, there are two cases where GJK says there is no
// collision:
// 1. GJK proved the distance is above its tolerance (default 1e-6).
@@ -628,7 +628,7 @@ struct COAL_DLLAPI GJKSolver {
// TODO: On degenerated case, the closest points may be non-unique.
// (i.e. an object face normal is colinear to `gjk.ray`)
gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal);
- Vec3f p = tf1.transform(0.5 * (p1 + p2));
+ Vec3s p = tf1.transform(0.5 * (p1 + p2));
normal = tf1.getRotation() * normal;
p1.noalias() = p - 0.5 * distance * normal;
p2.noalias() = p + 0.5 * distance * normal;
@@ -636,8 +636,8 @@ struct COAL_DLLAPI GJKSolver {
void GJKCollisionExtractWitnessPointsAndNormal(const Transform3f& tf1,
CoalScalar& distance,
- Vec3f& p1, Vec3f& p2,
- Vec3f& normal) const {
+ Vec3s& p1, Vec3s& p2,
+ Vec3s& normal) const {
COAL_UNUSED_VARIABLE(tf1);
COAL_ASSERT(this->gjk.distance <=
this->gjk.getTolerance() + this->m_dummy_precision,
@@ -651,12 +651,12 @@ struct COAL_DLLAPI GJKSolver {
distance = this->gjk.distance;
p1 = p2 = normal =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
}
void EPAExtractWitnessPointsAndNormal(const Transform3f& tf1,
- CoalScalar& distance, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) const {
+ CoalScalar& distance, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) const {
// Cache EPA result for potential future call to this GJKSolver.
// This caching allows to warm-start the next GJK call.
this->cached_guess = -(this->epa.depth * this->epa.normal);
@@ -703,22 +703,22 @@ struct COAL_DLLAPI GJKSolver {
// We compute the middle points of the current $p_1$ and $p_2$ and we use
// the normal and the distance given by EPA to compute the new $p_1$ and
// $p_2$.
- Vec3f p = tf1.transform(0.5 * (p1 + p2));
+ Vec3s p = tf1.transform(0.5 * (p1 + p2));
normal = tf1.getRotation() * normal;
p1.noalias() = p - 0.5 * distance * normal;
p2.noalias() = p + 0.5 * distance * normal;
}
void EPAFailedExtractWitnessPointsAndNormal(const Transform3f& tf1,
- CoalScalar& distance, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) const {
- this->cached_guess = Vec3f(1, 0, 0);
+ CoalScalar& distance, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) const {
+ this->cached_guess = Vec3s(1, 0, 0);
this->support_func_cached_guess.setZero();
COAL_UNUSED_VARIABLE(tf1);
distance = -(std::numeric_limits<CoalScalar>::max)();
p1 = p2 = normal =
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
}
};
diff --git a/include/coal/narrowphase/support_functions.h b/include/coal/narrowphase/support_functions.h
index 8dac64d4..df79a135 100644
--- a/include/coal/narrowphase/support_functions.h
+++ b/include/coal/narrowphase/support_functions.h
@@ -74,7 +74,7 @@ enum SupportOptions {
/// sphere radii. Please see `MinkowskiDiff::set(const ShapeBase*, const
/// ShapeBase*)` for more details.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint);
+Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint);
/// @brief Stores temporary data for the computation of support points.
struct COAL_DLLAPI ShapeSupportData {
@@ -83,7 +83,7 @@ struct COAL_DLLAPI ShapeSupportData {
// @brief Tracks the last support direction used on this shape; used to
// warm-start the ConvexBase support function.
- Vec3f last_dir = Vec3f::Zero();
+ Vec3s last_dir = Vec3s::Zero();
// @brief Temporary set used to compute the convex-hull of a support set.
// Only used for ConvexBase and Box.
@@ -92,46 +92,46 @@ struct COAL_DLLAPI ShapeSupportData {
/// @brief Triangle support function.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
+void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
+ Vec3s& support, int& /*unused*/,
ShapeSupportData& /*unused*/);
/// @brief Box support function.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/);
/// @brief Sphere support function.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const Sphere* sphere, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/);
/// @brief Ellipsoid support function.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
+void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir,
+ Vec3s& support, int& /*unused*/,
ShapeSupportData& /*unused*/);
/// @brief Capsule support function.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const Capsule* capsule, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/);
/// @brief Cone support function.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/);
/// @brief Cylinder support function.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/);
/// @brief ConvexBase support function.
/// @note See @ref LargeConvex and SmallConvex to see how to optimize
/// ConvexBase's support computation.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support,
int& hint, ShapeSupportData& /*unused*/);
/// @brief Cast a `ConvexBase` to a `LargeConvex` to use the log version of
@@ -145,13 +145,13 @@ struct SmallConvex : ShapeBase {};
/// @brief Support function for large ConvexBase (>32 vertices).
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const SmallConvex* convex, const Vec3f& dir,
- Vec3f& support, int& hint, ShapeSupportData& data);
+void getShapeSupport(const SmallConvex* convex, const Vec3s& dir,
+ Vec3s& support, int& hint, ShapeSupportData& data);
/// @brief Support function for small ConvexBase (<32 vertices).
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
- Vec3f& support, int& hint, ShapeSupportData& support_data);
+void getShapeSupport(const LargeConvex* convex, const Vec3s& dir,
+ Vec3s& support, int& hint, ShapeSupportData& support_data);
// ============================================================================
// ========================== SUPPORT SET FUNCTIONS ===========================
@@ -203,12 +203,12 @@ void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
/// @note This function automatically deals with the `direction` of the
/// SupportSet.
template <int _SupportOptions = SupportOptions::NoSweptSphere>
-void getSupportSet(const ShapeBase* shape, const Vec3f& dir,
+void getSupportSet(const ShapeBase* shape, const Vec3s& dir,
SupportSet& support_set, int& hint,
size_t num_sampled_supports = 6, CoalScalar tol = 1e-3) {
support_set.tf.rotation() = constructOrthonormalBasisFromVector(dir);
- const Vec3f& support_dir = support_set.getNormal();
- const Vec3f support = getSupport<_SupportOptions>(shape, support_dir, hint);
+ const Vec3s& support_dir = support_set.getNormal();
+ const Vec3s support = getSupport<_SupportOptions>(shape, support_dir, hint);
getSupportSet<_SupportOptions>(shape, support_set, hint, num_sampled_supports,
tol);
}
diff --git a/include/coal/octree.h b/include/coal/octree.h
index 7dee68df..52c50795 100644
--- a/include/coal/octree.h
+++ b/include/coal/octree.h
@@ -103,8 +103,8 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
/// @brief compute the AABB for the octree in its local coordinate system
void computeLocalAABB() {
- typedef Eigen::Matrix<float, 3, 1> Vec3float;
- Vec3float max_extent, min_extent;
+ typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
+ Vec3sloat max_extent, min_extent;
octomap::OcTree::iterator it =
tree->begin((unsigned char)tree->getTreeDepth());
@@ -115,10 +115,10 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
{
const octomap::point3d& coord =
it.getCoordinate(); // getCoordinate returns a copy
- max_extent = min_extent = Eigen::Map<const Vec3float>(&coord.x());
+ max_extent = min_extent = Eigen::Map<const Vec3sloat>(&coord.x());
for (++it; it != end; ++it) {
const octomap::point3d& coord = it.getCoordinate();
- const Vec3float pos = Eigen::Map<const Vec3float>(&coord.x());
+ const Vec3sloat pos = Eigen::Map<const Vec3sloat>(&coord.x());
max_extent = max_extent.array().max(pos.array());
min_extent = min_extent.array().min(pos.array());
}
@@ -140,7 +140,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
CoalScalar delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
// std::cout << "octree size " << delta << std::endl;
- return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
+ return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta));
}
/// @brief Returns the depth of the octree
@@ -175,8 +175,8 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
/// @brief transform the octree into a bunch of boxes; uncertainty information
/// is kept in the boxes. However, we only keep the occupied boxes (i.e., the
/// boxes whose occupied probability is higher enough).
- std::vector<Vec6f> toBoxes() const {
- std::vector<Vec6f> boxes;
+ std::vector<Vec6s> toBoxes() const {
+ std::vector<Vec6s> boxes;
boxes.reserve(tree->size() / 2);
for (octomap::OcTree::iterator
it = tree->begin((unsigned char)tree->getTreeDepth()),
@@ -191,7 +191,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
CoalScalar c = (*it).getOccupancy();
CoalScalar t = tree->getOccupancyThres();
- Vec6f box;
+ Vec6s box;
box << x, y, z, size, c, t;
boxes.push_back(box);
}
@@ -201,7 +201,7 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
/// \brief Returns a byte description of *this
std::vector<uint8_t> tobytes() const {
- typedef Eigen::Matrix<float, 3, 1> Vec3float;
+ typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
const size_t total_size = (tree->size() * sizeof(CoalScalar) * 3) / 2;
std::vector<uint8_t> bytes;
bytes.reserve(total_size);
@@ -210,8 +210,8 @@ class COAL_DLLAPI OcTree : public CollisionGeometry {
it = tree->begin((unsigned char)tree->getTreeDepth()),
end = tree->end();
it != end; ++it) {
- const Vec3f box_pos =
- Eigen::Map<Vec3float>(&it.getCoordinate().x()).cast<CoalScalar>();
+ const Vec3s box_pos =
+ Eigen::Map<Vec3sloat>(&it.getCoordinate().x()).cast<CoalScalar>();
if (isNodeOccupied(&*it))
std::copy(box_pos.data(), box_pos.data() + sizeof(CoalScalar) * 3,
std::back_inserter(bytes));
diff --git a/include/coal/serialization/collision_data.h b/include/coal/serialization/collision_data.h
index 2cc10f44..0c1466dc 100644
--- a/include/coal/serialization/collision_data.h
+++ b/include/coal/serialization/collision_data.h
@@ -27,7 +27,7 @@ void load(Archive& ar, coal::Contact& contact, const unsigned int /*version*/) {
ar >> make_nvp("b1", contact.b1);
ar >> make_nvp("b2", contact.b2);
ar >> make_nvp("normal", contact.normal);
- std::array<coal::Vec3f, 2> nearest_points;
+ std::array<coal::Vec3s, 2> nearest_points;
ar >> make_nvp("nearest_points", nearest_points);
contact.nearest_points[0] = nearest_points[0];
contact.nearest_points[1] = nearest_points[1];
@@ -113,7 +113,7 @@ void load(Archive& ar, coal::CollisionResult& collision_result,
for (size_t k = 0; k < contacts.size(); ++k)
collision_result.addContact(contacts[k]);
ar >> make_nvp("distance_lower_bound", collision_result.distance_lower_bound);
- std::array<coal::Vec3f, 2> nearest_points;
+ std::array<coal::Vec3s, 2> nearest_points;
ar >> make_nvp("nearest_points", nearest_points);
collision_result.nearest_points[0] = nearest_points[0];
collision_result.nearest_points[1] = nearest_points[1];
@@ -155,7 +155,7 @@ void load(Archive& ar, coal::DistanceResult& distance_result,
ar >> make_nvp("base", boost::serialization::base_object<coal::QueryResult>(
distance_result));
ar >> make_nvp("min_distance", distance_result.min_distance);
- std::array<coal::Vec3f, 2> nearest_points;
+ std::array<coal::Vec3s, 2> nearest_points;
ar >> make_nvp("nearest_points", nearest_points);
distance_result.nearest_points[0] = nearest_points[0];
distance_result.nearest_points[1] = nearest_points[1];
diff --git a/include/coal/serialization/convex.h b/include/coal/serialization/convex.h
index 52999cee..5c89a918 100644
--- a/include/coal/serialization/convex.h
+++ b/include/coal/serialization/convex.h
@@ -50,7 +50,7 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base,
convex_base.points.reset();
if (convex_base.num_points > 0)
convex_base.points.reset(
- new std::vector<Vec3f>(convex_base.num_points));
+ new std::vector<Vec3s>(convex_base.num_points));
}
if (num_normals_and_offsets_previous !=
@@ -59,7 +59,7 @@ void serialize(Archive& ar, coal::ConvexBase& convex_base,
convex_base.offsets.reset();
if (convex_base.num_normals_and_offsets > 0) {
convex_base.normals.reset(
- new std::vector<Vec3f>(convex_base.num_normals_and_offsets));
+ new std::vector<Vec3s>(convex_base.num_normals_and_offsets));
convex_base.offsets.reset(
new std::vector<CoalScalar>(convex_base.num_normals_and_offsets));
}
diff --git a/include/coal/serialization/kIOS.h b/include/coal/serialization/kIOS.h
index 14c6457f..18d349db 100644
--- a/include/coal/serialization/kIOS.h
+++ b/include/coal/serialization/kIOS.h
@@ -23,7 +23,7 @@ void save(Archive& ar, const coal::kIOS& bv, const unsigned int /*version*/) {
// Number of spheres in kIOS is never larger than kIOS::kios_max_num_spheres
ar& make_nvp("num_spheres", bv.num_spheres);
- std::array<coal::Vec3f, coal::kIOS::max_num_spheres> centers{};
+ std::array<coal::Vec3s, coal::kIOS::max_num_spheres> centers{};
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;
@@ -39,7 +39,7 @@ template <class Archive>
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::Vec3s, coal::kIOS::max_num_spheres> centers;
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()));
diff --git a/include/coal/shape/convex.h b/include/coal/shape/convex.h
index 916fdfda..a17108a6 100644
--- a/include/coal/shape/convex.h
+++ b/include/coal/shape/convex.h
@@ -60,7 +60,7 @@ class Convex : public ConvexBase {
/// \param polygons_ \copydoc Convex::polygons
/// \param num_polygons_ the number of polygons.
/// \note num_polygons_ is not the allocated size of polygons_.
- Convex(std::shared_ptr<std::vector<Vec3f>> points_, unsigned int num_points_,
+ Convex(std::shared_ptr<std::vector<Vec3s>> points_, unsigned int num_points_,
std::shared_ptr<std::vector<PolygonT>> polygons_,
unsigned int num_polygons_);
@@ -71,9 +71,9 @@ class Convex : public ConvexBase {
~Convex();
/// based on http://number-none.com/blow/inertia/bb_inertia.doc
- Matrix3f computeMomentofInertia() const;
+ Matrix3s computeMomentofInertia() const;
- Vec3f computeCOM() const;
+ Vec3s computeCOM() const;
CoalScalar computeVolume() const;
@@ -88,7 +88,7 @@ class Convex : public ConvexBase {
/// \param num_polygons the number of polygons.
/// \note num_polygons is not the allocated size of polygons.
///
- void set(std::shared_ptr<std::vector<Vec3f>> points, unsigned int num_points,
+ void set(std::shared_ptr<std::vector<Vec3s>> points, unsigned int num_points,
std::shared_ptr<std::vector<PolygonT>> polygons,
unsigned int num_polygons);
diff --git a/include/coal/shape/details/convex.hxx b/include/coal/shape/details/convex.hxx
index 566af023..0c0ccf58 100644
--- a/include/coal/shape/details/convex.hxx
+++ b/include/coal/shape/details/convex.hxx
@@ -44,7 +44,7 @@
namespace coal {
template <typename PolygonT>
-Convex<PolygonT>::Convex(std::shared_ptr<std::vector<Vec3f>> points_,
+Convex<PolygonT>::Convex(std::shared_ptr<std::vector<Vec3s>> points_,
unsigned int num_points_,
std::shared_ptr<std::vector<PolygonT>> polygons_,
unsigned int num_polygons_)
@@ -67,7 +67,7 @@ template <typename PolygonT>
Convex<PolygonT>::~Convex() {}
template <typename PolygonT>
-void Convex<PolygonT>::set(std::shared_ptr<std::vector<Vec3f>> points_,
+void Convex<PolygonT>::set(std::shared_ptr<std::vector<Vec3s>> points_,
unsigned int num_points_,
std::shared_ptr<std::vector<PolygonT>> polygons_,
unsigned int num_polygons_) {
@@ -86,13 +86,13 @@ Convex<PolygonT>* Convex<PolygonT>::clone() const {
}
template <typename PolygonT>
-Matrix3f Convex<PolygonT>::computeMomentofInertia() const {
+Matrix3s Convex<PolygonT>::computeMomentofInertia() const {
typedef typename PolygonT::size_type size_type;
typedef typename PolygonT::index_type index_type;
- Matrix3f C = Matrix3f::Zero();
+ Matrix3s C = Matrix3s::Zero();
- Matrix3f C_canonical;
+ Matrix3s C_canonical;
C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0, 1 / 120.0,
1 / 120.0, 1 / 120.0, 1 / 60.0;
@@ -102,7 +102,7 @@ Matrix3f Convex<PolygonT>::computeMomentofInertia() const {
<< std::endl;
return C;
}
- const std::vector<Vec3f>& points_ = *points;
+ const std::vector<Vec3s>& points_ = *points;
if (!(polygons.get())) {
std::cerr
<< "Error in `Convex::computeMomentofInertia`! Convex has no polygons."
@@ -114,43 +114,43 @@ Matrix3f Convex<PolygonT>::computeMomentofInertia() const {
const PolygonT& polygon = polygons_[i];
// compute the center of the polygon
- Vec3f plane_center(0, 0, 0);
+ Vec3s plane_center(0, 0, 0);
for (size_type j = 0; j < polygon.size(); ++j)
plane_center += points_[polygon[(index_type)j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the
// plane center and the reference point (zero) of the convex shape
- const Vec3f& v3 = plane_center;
+ const Vec3s& v3 = plane_center;
for (size_type j = 0; j < polygon.size(); ++j) {
index_type e_first = polygon[static_cast<index_type>(j)];
index_type e_second =
polygon[static_cast<index_type>((j + 1) % polygon.size())];
- const Vec3f& v1 = points_[e_first];
- const Vec3f& v2 = points_[e_second];
- Matrix3f A;
+ const Vec3s& v1 = points_[e_first];
+ const Vec3s& v2 = points_[e_second];
+ Matrix3s A;
A << v1.transpose(), v2.transpose(),
v3.transpose(); // this is A' in the original document
C += A.transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
}
- return C.trace() * Matrix3f::Identity() - C;
+ return C.trace() * Matrix3s::Identity() - C;
}
template <typename PolygonT>
-Vec3f Convex<PolygonT>::computeCOM() const {
+Vec3s Convex<PolygonT>::computeCOM() const {
typedef typename PolygonT::size_type size_type;
typedef typename PolygonT::index_type index_type;
- Vec3f com(0, 0, 0);
+ Vec3s com(0, 0, 0);
CoalScalar vol = 0;
if (!(points.get())) {
std::cerr << "Error in `Convex::computeCOM`! Convex has no vertices."
<< std::endl;
return com;
}
- const std::vector<Vec3f>& points_ = *points;
+ const std::vector<Vec3s>& points_ = *points;
if (!(polygons.get())) {
std::cerr << "Error in `Convex::computeCOM`! Convex has no polygons."
<< std::endl;
@@ -160,20 +160,20 @@ Vec3f Convex<PolygonT>::computeCOM() const {
for (unsigned int i = 0; i < num_polygons; ++i) {
const PolygonT& polygon = polygons_[i];
// compute the center of the polygon
- Vec3f plane_center(0, 0, 0);
+ Vec3s plane_center(0, 0, 0);
for (size_type j = 0; j < polygon.size(); ++j)
plane_center += points_[polygon[(index_type)j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the
// plane center and the reference point (zero) of the convex shape
- const Vec3f& v3 = plane_center;
+ const Vec3s& v3 = plane_center;
for (size_type j = 0; j < polygon.size(); ++j) {
index_type e_first = polygon[static_cast<index_type>(j)];
index_type e_second =
polygon[static_cast<index_type>((j + 1) % polygon.size())];
- const Vec3f& v1 = points_[e_first];
- const Vec3f& v2 = points_[e_second];
+ const Vec3s& v1 = points_[e_first];
+ const Vec3s& v2 = points_[e_second];
CoalScalar d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
com += (points_[e_first] + points_[e_second] + plane_center) * d_six_vol;
@@ -194,7 +194,7 @@ CoalScalar Convex<PolygonT>::computeVolume() const {
<< std::endl;
return vol;
}
- const std::vector<Vec3f>& points_ = *points;
+ const std::vector<Vec3s>& points_ = *points;
if (!(polygons.get())) {
std::cerr << "Error in `Convex::computeVolume`! Convex has no polygons."
<< std::endl;
@@ -205,20 +205,20 @@ CoalScalar Convex<PolygonT>::computeVolume() const {
const PolygonT& polygon = polygons_[i];
// compute the center of the polygon
- Vec3f plane_center(0, 0, 0);
+ Vec3s plane_center(0, 0, 0);
for (size_type j = 0; j < polygon.size(); ++j)
plane_center += points_[polygon[(index_type)j]];
plane_center /= polygon.size();
// compute the volume of tetrahedron making by neighboring two points, the
// plane center and the reference point (zero point) of the convex shape
- const Vec3f& v3 = plane_center;
+ const Vec3s& v3 = plane_center;
for (size_type j = 0; j < polygon.size(); ++j) {
index_type e_first = polygon[static_cast<index_type>(j)];
index_type e_second =
polygon[static_cast<index_type>((j + 1) % polygon.size())];
- const Vec3f& v1 = points_[e_first];
- const Vec3f& v2 = points_[e_second];
+ const Vec3s& v1 = points_[e_first];
+ const Vec3s& v2 = points_[e_second];
CoalScalar d_six_vol = (v1.cross(v2)).dot(v3);
vol += d_six_vol;
}
diff --git a/include/coal/shape/geometric_shape_to_BVH_model.h b/include/coal/shape/geometric_shape_to_BVH_model.h
index 48516cd7..f9a3aca8 100644
--- a/include/coal/shape/geometric_shape_to_BVH_model.h
+++ b/include/coal/shape/geometric_shape_to_BVH_model.h
@@ -51,16 +51,16 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape,
CoalScalar a = shape.halfSide[0];
CoalScalar b = shape.halfSide[1];
CoalScalar c = shape.halfSide[2];
- std::vector<Vec3f> points(8);
+ std::vector<Vec3s> points(8);
std::vector<Triangle> tri_indices(12);
- points[0] = Vec3f(a, -b, c);
- points[1] = Vec3f(a, b, c);
- points[2] = Vec3f(-a, b, c);
- points[3] = Vec3f(-a, -b, c);
- points[4] = Vec3f(a, -b, -c);
- points[5] = Vec3f(a, b, -c);
- points[6] = Vec3f(-a, b, -c);
- points[7] = Vec3f(-a, -b, -c);
+ points[0] = Vec3s(a, -b, c);
+ points[1] = Vec3s(a, b, c);
+ points[2] = Vec3s(-a, b, c);
+ points[3] = Vec3s(-a, -b, c);
+ points[4] = Vec3s(a, -b, -c);
+ points[5] = Vec3s(a, b, -c);
+ points[6] = Vec3s(-a, b, -c);
+ points[7] = Vec3s(-a, -b, -c);
tri_indices[0].set(0, 4, 1);
tri_indices[1].set(1, 4, 5);
@@ -91,7 +91,7 @@ template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
const Transform3f& pose, unsigned int seg,
unsigned int ring) {
- std::vector<Vec3f> points;
+ std::vector<Vec3s> points;
std::vector<Triangle> tri_indices;
CoalScalar r = shape.radius;
@@ -107,13 +107,13 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape,
for (unsigned int i = 0; i < ring; ++i) {
CoalScalar theta_ = theta + thetad * (i + 1);
for (unsigned int j = 0; j < seg; ++j) {
- points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid),
+ points.push_back(Vec3s(r * sin(theta_) * cos(phi + j * phid),
r * sin(theta_) * sin(phi + j * phid),
r * cos(theta_)));
}
}
- points.push_back(Vec3f(0, 0, r));
- points.push_back(Vec3f(0, 0, -r));
+ points.push_back(Vec3s(0, 0, r));
+ points.push_back(Vec3s(0, 0, -r));
for (unsigned int i = 0; i < ring - 1; ++i) {
for (unsigned int j = 0; j < seg; ++j) {
@@ -172,7 +172,7 @@ template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
const Transform3f& pose, unsigned int tot,
unsigned int h_num) {
- std::vector<Vec3f> points;
+ std::vector<Vec3s> points;
std::vector<Triangle> tri_indices;
CoalScalar r = shape.radius;
@@ -186,21 +186,21 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape,
for (unsigned int i = 0; i < tot; ++i)
points.push_back(
- Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
+ Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), h));
for (unsigned int i = 0; i < h_num - 1; ++i) {
for (unsigned int j = 0; j < tot; ++j) {
- points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j),
+ points.push_back(Vec3s(r * cos(phi + phid * j), r * sin(phi + phid * j),
h - (i + 1) * hd));
}
}
for (unsigned int i = 0; i < tot; ++i)
points.push_back(
- Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
+ Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
- points.push_back(Vec3f(0, 0, h));
- points.push_back(Vec3f(0, 0, -h));
+ points.push_back(Vec3s(0, 0, h));
+ points.push_back(Vec3s(0, 0, -h));
for (unsigned int i = 0; i < tot; ++i) {
Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
@@ -264,7 +264,7 @@ template <typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
const Transform3f& pose, unsigned int tot,
unsigned int h_num) {
- std::vector<Vec3f> points;
+ std::vector<Vec3s> points;
std::vector<Triangle> tri_indices;
CoalScalar r = shape.radius;
@@ -282,16 +282,16 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape,
CoalScalar rh = r * (0.5 - h_i / h / 2);
for (unsigned int j = 0; j < tot; ++j) {
points.push_back(
- Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
+ Vec3s(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
}
}
for (unsigned int i = 0; i < tot; ++i)
points.push_back(
- Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
+ Vec3s(r * cos(phi + phid * i), r * sin(phi + phid * i), -h));
- points.push_back(Vec3f(0, 0, h));
- points.push_back(Vec3f(0, 0, -h));
+ points.push_back(Vec3s(0, 0, h));
+ points.push_back(Vec3s(0, 0, -h));
for (unsigned int i = 0; i < tot; ++i) {
Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1));
diff --git a/include/coal/shape/geometric_shapes.h b/include/coal/shape/geometric_shapes.h
index d21d5c21..e88a737e 100644
--- a/include/coal/shape/geometric_shapes.h
+++ b/include/coal/shape/geometric_shapes.h
@@ -111,7 +111,7 @@ class COAL_DLLAPI TriangleP : public ShapeBase {
public:
TriangleP() {};
- TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_)
+ TriangleP(const Vec3s& a_, const Vec3s& b_, const Vec3s& c_)
: ShapeBase(), a(a_), b(b_), c(c_) {}
TriangleP(const TriangleP& other)
@@ -128,13 +128,13 @@ class COAL_DLLAPI TriangleP : public ShapeBase {
// std::pair<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();
+ // Transform3f()); Vec3s AB(b - a), BC(c - b), CA(a - c); AB.normalize();
// BC.normalize();
// CA.normalize();
//
- // Vec3f new_a(a + value * Vec3f(-AB + CA).normalized());
- // Vec3f new_b(b + value * Vec3f(-BC + AB).normalized());
- // Vec3f new_c(c + value * Vec3f(-CA + BC).normalized());
+ // Vec3s new_a(a + value * Vec3s(-AB + CA).normalized());
+ // Vec3s new_b(b + value * Vec3s(-BC + AB).normalized());
+ // Vec3s new_c(c + value * Vec3s(-CA + BC).normalized());
//
// return std::make_pair(new TriangleP(new_a, new_b, new_c),
// Transform3f());
@@ -146,7 +146,7 @@ class COAL_DLLAPI TriangleP : public ShapeBase {
// implement
// }
- Vec3f a, b, c;
+ Vec3s a, b, c;
private:
virtual bool isEqual(const CollisionGeometry& _other) const {
@@ -168,7 +168,7 @@ class COAL_DLLAPI Box : public ShapeBase {
Box(CoalScalar x, CoalScalar y, CoalScalar z)
: ShapeBase(), halfSide(x / 2, y / 2, z / 2) {}
- Box(const Vec3f& side_) : ShapeBase(), halfSide(side_ / 2) {}
+ Box(const Vec3s& side_) : ShapeBase(), halfSide(side_ / 2) {}
Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {}
@@ -186,7 +186,7 @@ class COAL_DLLAPI Box : public ShapeBase {
Box() {}
/// @brief box side half-length
- Vec3f halfSide;
+ Vec3s halfSide;
/// @brief Compute AABB
void computeLocalAABB();
@@ -196,10 +196,10 @@ class COAL_DLLAPI Box : public ShapeBase {
CoalScalar computeVolume() const { return 8 * halfSide.prod(); }
- Matrix3f computeMomentofInertia() const {
+ Matrix3s computeMomentofInertia() const {
CoalScalar V = computeVolume();
- Vec3f s(halfSide.cwiseAbs2() * V);
- return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
+ Vec3s s(halfSide.cwiseAbs2() * V);
+ return (Vec3s(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
}
CoalScalar minInflationValue() const { return -halfSide.minCoeff(); }
@@ -218,7 +218,7 @@ class COAL_DLLAPI Box : public ShapeBase {
<< "is two small. It should be at least: "
<< minInflationValue(),
std::invalid_argument);
- return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))),
+ return std::make_pair(Box(2 * (halfSide + Vec3s::Constant(value))),
Transform3f());
}
@@ -258,9 +258,9 @@ class COAL_DLLAPI Sphere : public ShapeBase {
/// @brief Get node type: a sphere
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
- Matrix3f computeMomentofInertia() const {
+ Matrix3s computeMomentofInertia() const {
CoalScalar I = 0.4 * radius * radius * computeVolume();
- return I * Matrix3f::Identity();
+ return I * Matrix3s::Identity();
}
CoalScalar computeVolume() const {
@@ -310,7 +310,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase {
Ellipsoid(CoalScalar rx, CoalScalar ry, CoalScalar rz)
: ShapeBase(), radii(rx, ry, rz) {}
- explicit Ellipsoid(const Vec3f& radii) : radii(radii) {}
+ explicit Ellipsoid(const Vec3s& radii) : radii(radii) {}
Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {}
@@ -319,7 +319,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase {
/// @brief Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2
/// + z^2/rz^2 = 1)
- Vec3f radii;
+ Vec3s radii;
/// @brief Compute AABB
void computeLocalAABB();
@@ -327,12 +327,12 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase {
/// @brief Get node type: an ellipsoid
NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; }
- Matrix3f computeMomentofInertia() const {
+ Matrix3s computeMomentofInertia() const {
CoalScalar V = computeVolume();
CoalScalar a2 = V * radii[0] * radii[0];
CoalScalar b2 = V * radii[1] * radii[1];
CoalScalar c2 = V * radii[2] * radii[2];
- return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0,
+ return (Matrix3s() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0,
0.2 * (a2 + b2))
.finished();
}
@@ -358,7 +358,7 @@ class COAL_DLLAPI Ellipsoid : public ShapeBase {
<< ") is two small. It should be at least: "
<< minInflationValue(),
std::invalid_argument);
- return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)),
+ return std::make_pair(Ellipsoid(radii + Vec3s::Constant(value)),
Transform3f());
}
@@ -412,7 +412,7 @@ class COAL_DLLAPI Capsule : public ShapeBase {
((halfLength * 2) + radius * 4 / 3.0);
}
- Matrix3f computeMomentofInertia() const {
+ Matrix3s computeMomentofInertia() const {
CoalScalar v_cyl = radius * radius * (halfLength * 2) *
boost::math::constants::pi<CoalScalar>();
CoalScalar v_sph = radius * radius * radius *
@@ -424,7 +424,7 @@ class COAL_DLLAPI Capsule : public ShapeBase {
v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
CoalScalar iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
- return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
+ return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
}
CoalScalar minInflationValue() const { return -radius; }
@@ -496,16 +496,16 @@ class COAL_DLLAPI Cone : public ShapeBase {
(halfLength * 2) / 3;
}
- Matrix3f computeMomentofInertia() const {
+ Matrix3s computeMomentofInertia() const {
CoalScalar V = computeVolume();
CoalScalar ix =
V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
CoalScalar iz = 0.3 * V * radius * radius;
- return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
+ return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
}
- Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); }
+ Vec3s computeCOM() const { return Vec3s(0, 0, -0.5 * halfLength); }
CoalScalar minInflationValue() const {
return -(std::min)(radius, halfLength);
@@ -538,7 +538,7 @@ class COAL_DLLAPI Cone : public ShapeBase {
const CoalScalar new_radius = new_lz / tan_alpha;
return std::make_pair(Cone(new_radius, new_lz),
- Transform3f(Vec3f(0., 0., new_cz)));
+ Transform3f(Vec3s(0., 0., new_cz)));
}
private:
@@ -597,11 +597,11 @@ class COAL_DLLAPI Cylinder : public ShapeBase {
(halfLength * 2);
}
- Matrix3f computeMomentofInertia() const {
+ Matrix3s computeMomentofInertia() const {
CoalScalar V = computeVolume();
CoalScalar ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
CoalScalar iz = V * radius * radius / 2;
- return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
+ return (Matrix3s() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
}
CoalScalar minInflationValue() const {
@@ -656,13 +656,13 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
/// Qhull.
/// \note Coal must have been compiled with option \c COAL_HAS_QHULL set
/// to \c ON.
- static ConvexBase* convexHull(std::shared_ptr<std::vector<Vec3f>>& points,
+ static ConvexBase* convexHull(std::shared_ptr<std::vector<Vec3s>>& points,
unsigned int num_points, bool keepTriangles,
const char* qhullCommand = NULL);
// TODO(louis): put this method in private sometime in the future.
COAL_DEPRECATED static ConvexBase* convexHull(
- const Vec3f* points, unsigned int num_points, bool keepTriangles,
+ const Vec3s* points, unsigned int num_points, bool keepTriangles,
const char* qhullCommand = NULL);
virtual ~ConvexBase();
@@ -716,11 +716,11 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
static constexpr size_t num_vertices_large_convex_threshold = 32;
/// @brief An array of the points of the polygon.
- std::shared_ptr<std::vector<Vec3f>> points;
+ std::shared_ptr<std::vector<Vec3s>> points;
unsigned int num_points;
/// @brief An array of the normals of the polygon.
- std::shared_ptr<std::vector<Vec3f>> normals;
+ std::shared_ptr<std::vector<Vec3s>> normals;
/// @brief An array of the offsets to the normals of the polygon.
/// Note: there are as many offsets as normals.
std::shared_ptr<std::vector<double>> offsets;
@@ -733,7 +733,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
/// @brief center of the convex polytope, this is used for collision: center
/// is guaranteed in the internal of the polytope (as it is convex)
- Vec3f center;
+ Vec3s center;
/// @brief The support warm start polytope contains certain points of `this`
/// which are support points in specific directions of space.
@@ -742,7 +742,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
struct SupportWarmStartPolytope {
/// @brief Array of support points to warm start the support function
/// computation.
- std::vector<Vec3f> points;
+ std::vector<Vec3s> points;
/// @brief Indices of the support points warm starts.
/// These are the indices of the real convex, not the indices of points in
@@ -763,7 +763,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
: ShapeBase(),
num_points(0),
num_normals_and_offsets(0),
- center(Vec3f::Zero()) {}
+ center(Vec3s::Zero()) {}
/// @brief Initialize the points of the convex shape
/// This also initializes the ConvexBase::center.
@@ -771,7 +771,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
/// \param ownStorage weither the ConvexBase owns the data.
/// \param points_ list of 3D points ///
/// \param num_points_ number of 3D points
- void initialize(std::shared_ptr<std::vector<Vec3f>> points_,
+ void initialize(std::shared_ptr<std::vector<Vec3s>> points_,
unsigned int num_points_);
/// @brief Set the points of the convex shape.
@@ -779,7 +779,7 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
/// \param ownStorage weither the ConvexBase owns the data.
/// \param points_ list of 3D points ///
/// \param num_points_ number of 3D points
- void set(std::shared_ptr<std::vector<Vec3f>> points_,
+ void set(std::shared_ptr<std::vector<Vec3s>> points_,
unsigned int num_points_);
/// @brief Copy constructor
@@ -814,8 +814,8 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
(points.get() && !(other.points.get())))
return false;
if (points.get() && other.points.get()) {
- const std::vector<Vec3f>& points_ = *points;
- const std::vector<Vec3f>& other_points_ = *(other.points);
+ const std::vector<Vec3s>& points_ = *points;
+ const std::vector<Vec3s>& other_points_ = *(other.points);
for (unsigned int i = 0; i < num_points; ++i) {
if (points_[i] != (other_points_)[i]) return false;
}
@@ -836,8 +836,8 @@ class COAL_DLLAPI ConvexBase : public ShapeBase {
(normals.get() && !(other.normals.get())))
return false;
if (normals.get() && other.normals.get()) {
- const std::vector<Vec3f>& normals_ = *normals;
- const std::vector<Vec3f>& other_normals_ = *(other.normals);
+ const std::vector<Vec3s>& normals_ = *normals;
+ const std::vector<Vec3s>& other_normals_ = *(other.normals);
for (unsigned int i = 0; i < num_normals_and_offsets; ++i) {
if (normals_[i] != other_normals_[i]) return false;
}
@@ -892,7 +892,7 @@ class Convex;
class COAL_DLLAPI Halfspace : public ShapeBase {
public:
/// @brief Construct a half space with normal direction and offset
- Halfspace(const Vec3f& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
+ Halfspace(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
unitNormalTest();
}
@@ -917,11 +917,11 @@ class COAL_DLLAPI Halfspace : public ShapeBase {
/// @brief Clone *this into a new Halfspace
virtual Halfspace* clone() const { return new Halfspace(*this); };
- CoalScalar signedDistance(const Vec3f& p) const {
+ CoalScalar signedDistance(const Vec3s& p) const {
return n.dot(p) - (d + this->getSweptSphereRadius());
}
- CoalScalar distance(const Vec3f& p) const {
+ CoalScalar distance(const Vec3s& p) const {
return std::abs(this->signedDistance(p));
}
@@ -953,7 +953,7 @@ class COAL_DLLAPI Halfspace : public ShapeBase {
}
/// @brief Plane normal
- Vec3f n;
+ Vec3s n;
/// @brief Plane offset
CoalScalar d;
@@ -983,7 +983,7 @@ class COAL_DLLAPI Halfspace : public ShapeBase {
class COAL_DLLAPI Plane : public ShapeBase {
public:
/// @brief Construct a plane with normal direction and offset
- Plane(const Vec3f& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
+ Plane(const Vec3s& n_, CoalScalar d_) : ShapeBase(), n(n_), d(d_) {
unitNormalTest();
}
@@ -1007,7 +1007,7 @@ class COAL_DLLAPI Plane : public ShapeBase {
/// @brief Clone *this into a new Plane
virtual Plane* clone() const { return new Plane(*this); };
- CoalScalar signedDistance(const Vec3f& p) const {
+ CoalScalar signedDistance(const Vec3s& p) const {
const CoalScalar dist = n.dot(p) - d;
CoalScalar signed_dist =
std::abs(n.dot(p) - d) - this->getSweptSphereRadius();
@@ -1020,7 +1020,7 @@ class COAL_DLLAPI Plane : public ShapeBase {
return signed_dist;
}
- CoalScalar distance(const Vec3f& p) const {
+ CoalScalar distance(const Vec3s& p) const {
return std::abs(std::abs(n.dot(p) - d) - this->getSweptSphereRadius());
}
@@ -1031,7 +1031,7 @@ class COAL_DLLAPI Plane : public ShapeBase {
NODE_TYPE getNodeType() const { return GEOM_PLANE; }
/// @brief Plane normal
- Vec3f n;
+ Vec3s n;
/// @brief Plane offset
CoalScalar d;
diff --git a/include/coal/shape/geometric_shapes_utility.h b/include/coal/shape/geometric_shapes_utility.h
index 83187b4f..30500887 100644
--- a/include/coal/shape/geometric_shapes_utility.h
+++ b/include/coal/shape/geometric_shapes_utility.h
@@ -49,21 +49,21 @@ namespace coal {
namespace details {
/// @brief get the vertices of some convex shape which can bound the given shape
/// in a specific configuration
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const Box& box,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Box& box,
const Transform3f& tf);
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const Sphere& sphere,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Sphere& sphere,
const Transform3f& tf);
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Ellipsoid& ellipsoid,
const Transform3f& tf);
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Capsule& capsule,
const Transform3f& tf);
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const Cone& cone,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Cone& cone,
const Transform3f& tf);
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Cylinder& cylinder,
const Transform3f& tf);
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const ConvexBase& convex,
const Transform3f& tf);
-COAL_DLLAPI std::vector<Vec3f> getBoundVertices(const TriangleP& triangle,
+COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
const Transform3f& tf);
} // namespace details
/// @endcond
@@ -75,7 +75,7 @@ inline void computeBV(const S& s, const Transform3f& tf, BV& bv) {
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- std::vector<Vec3f> convex_bound_vertices = details::getBoundVertices(s, tf);
+ std::vector<Vec3s> convex_bound_vertices = details::getBoundVertices(s, tf);
fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(),
bv);
}
diff --git a/python/broadphase/broadphase.cc b/python/broadphase/broadphase.cc
index 583e611c..28e427a2 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, CoalScalar, const Vec3f &, const Vec3f &,
+ .def(dv::init<Derived, CoalScalar, const Vec3s &, const Vec3s &,
bp::optional<unsigned int>>());
}
}
diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc
index 2adacda2..63d160bb 100644
--- a/python/collision-geometries.cc
+++ b/python/collision-geometries.cc
@@ -75,7 +75,7 @@ namespace bp = boost::python;
using boost::noncopyable;
-typedef std::vector<Vec3f> Vec3fs;
+typedef std::vector<Vec3s> Vec3ss;
typedef std::vector<Triangle> Triangles;
struct BVHModelBaseWrapper {
@@ -83,7 +83,7 @@ struct BVHModelBaseWrapper {
typedef Eigen::Map<RowMatrixX3> MapRowMatrixX3;
typedef Eigen::Ref<RowMatrixX3> RefRowMatrixX3;
- static Vec3f& vertex(BVHModelBase& bvh, unsigned int i) {
+ static Vec3s& vertex(BVHModelBase& bvh, unsigned int i) {
if (i >= bvh.num_vertices) throw std::out_of_range("index is out of range");
return (*(bvh.vertices))[i];
}
@@ -130,7 +130,7 @@ void exposeHeightField(const std::string& bvname) {
type_name.c_str(), doxygen::class_doc<Geometry>(), no_init)
.def(dv::init<HeightField<BV>>())
.def(dv::init<HeightField<BV>, const HeightField<BV>&>())
- .def(dv::init<HeightField<BV>, CoalScalar, CoalScalar, const MatrixXf&,
+ .def(dv::init<HeightField<BV>, CoalScalar, CoalScalar, const MatrixXs&,
bp::optional<CoalScalar>>())
.DEF_CLASS_FUNC(Geometry, getXDim)
@@ -168,7 +168,7 @@ struct ConvexBaseWrapper {
typedef Eigen::Map<VecOfDoubles> MapVecOfDoubles;
typedef Eigen::Ref<VecOfDoubles> RefVecOfDoubles;
- static Vec3f& point(const ConvexBase& convex, unsigned int i) {
+ static Vec3s& point(const ConvexBase& convex, unsigned int i) {
if (i >= convex.num_points)
throw std::out_of_range("index is out of range");
return (*(convex.points))[i];
@@ -178,7 +178,7 @@ struct ConvexBaseWrapper {
return MapRowMatrixX3((*(convex.points))[0].data(), convex.num_points, 3);
}
- static Vec3f& normal(const ConvexBase& convex, unsigned int i) {
+ static Vec3s& normal(const ConvexBase& convex, unsigned int i) {
if (i >= convex.num_normals_and_offsets)
throw std::out_of_range("index is out of range");
return (*(convex.normals))[i];
@@ -210,7 +210,7 @@ struct ConvexBaseWrapper {
return n;
}
- static ConvexBase* convexHull(const Vec3fs& points, bool keepTri,
+ static ConvexBase* convexHull(const Vec3ss& points, bool keepTri,
const char* qhullCommand) {
return ConvexBase::convexHull(points.data(), (unsigned int)points.size(),
keepTri, qhullCommand);
@@ -227,11 +227,11 @@ struct ConvexWrapper {
return (*convex.polygons)[i];
}
- static shared_ptr<Convex_t> constructor(const Vec3fs& _points,
+ static shared_ptr<Convex_t> constructor(const Vec3ss& _points,
const Triangles& _tris) {
- std::shared_ptr<std::vector<Vec3f>> points(
- new std::vector<Vec3f>(_points.size()));
- std::vector<Vec3f>& points_ = *points;
+ std::shared_ptr<std::vector<Vec3s>> points(
+ new std::vector<Vec3s>(_points.size()));
+ std::vector<Vec3s>& points_ = *points;
for (std::size_t i = 0; i < _points.size(); ++i) points_[i] = _points[i];
std::shared_ptr<std::vector<Triangle>> tris(
@@ -279,7 +279,7 @@ void exposeShapes() {
.def(dv::init<Box>())
.def(dv::init<Box, const Box&>())
.def(dv::init<Box, CoalScalar, CoalScalar, CoalScalar>())
- .def(dv::init<Box, const Vec3f&>())
+ .def(dv::init<Box, const Vec3s&>())
.DEF_RW_CLASS_ATTRIB(Box, halfSide)
.def("clone", &Box::clone, doxygen::member_func_doc(&Box::clone),
return_value_policy<manage_new_object>())
@@ -372,7 +372,7 @@ void exposeShapes() {
class_<Halfspace, bases<ShapeBase>, shared_ptr<Halfspace>>(
"Halfspace", doxygen::class_doc<Halfspace>(), no_init)
- .def(dv::init<Halfspace, const Vec3f&, CoalScalar>())
+ .def(dv::init<Halfspace, const Vec3s&, CoalScalar>())
.def(dv::init<Halfspace, const Halfspace&>())
.def(
dv::init<Halfspace, CoalScalar, CoalScalar, CoalScalar, CoalScalar>())
@@ -387,7 +387,7 @@ void exposeShapes() {
class_<Plane, bases<ShapeBase>, shared_ptr<Plane>>(
"Plane", doxygen::class_doc<Plane>(), no_init)
- .def(dv::init<Plane, const Vec3f&, CoalScalar>())
+ .def(dv::init<Plane, const Vec3s&, CoalScalar>())
.def(dv::init<Plane, const Plane&>())
.def(dv::init<Plane, CoalScalar, CoalScalar, CoalScalar, CoalScalar>())
.def(dv::init<Plane>())
@@ -413,7 +413,7 @@ void exposeShapes() {
"Ellipsoid", doxygen::class_doc<Ellipsoid>(), no_init)
.def(dv::init<Ellipsoid>())
.def(dv::init<Ellipsoid, CoalScalar, CoalScalar, CoalScalar>())
- .def(dv::init<Ellipsoid, Vec3f>())
+ .def(dv::init<Ellipsoid, Vec3s>())
.def(dv::init<Ellipsoid, const Ellipsoid&>())
.DEF_RW_CLASS_ATTRIB(Ellipsoid, radii)
.def("clone", &Ellipsoid::clone,
@@ -425,7 +425,7 @@ void exposeShapes() {
class_<TriangleP, bases<ShapeBase>, shared_ptr<TriangleP>>(
"TriangleP", doxygen::class_doc<TriangleP>(), no_init)
.def(dv::init<TriangleP>())
- .def(dv::init<TriangleP, const Vec3f&, const Vec3f&, const Vec3f&>())
+ .def(dv::init<TriangleP, const Vec3s&, const Vec3s&, const Vec3s&>())
.def(dv::init<TriangleP, const TriangleP&>())
.DEF_RW_CLASS_ATTRIB(TriangleP, a)
.DEF_RW_CLASS_ATTRIB(TriangleP, b)
@@ -438,7 +438,7 @@ void exposeShapes() {
}
boost::python::tuple AABB_distance_proxy(const AABB& self, const AABB& other) {
- Vec3f P, Q;
+ Vec3s P, Q;
CoalScalar distance = self.distance(other, &P, &Q);
return boost::python::make_tuple(distance, P, Q);
}
@@ -502,17 +502,17 @@ void exposeCollisionGeometries() {
no_init)
.def(init<>(bp::arg("self"), "Default constructor"))
.def(init<AABB>(bp::args("self", "other"), "Copy constructor"))
- .def(init<Vec3f>(bp::args("self", "v"),
+ .def(init<Vec3s>(bp::args("self", "v"),
"Creating an AABB at position v with zero size."))
- .def(init<Vec3f, Vec3f>(bp::args("self", "a", "b"),
+ .def(init<Vec3s, Vec3s>(bp::args("self", "a", "b"),
"Creating an AABB with two endpoints a and b."))
- .def(init<AABB, Vec3f>(
+ .def(init<AABB, Vec3s>(
bp::args("self", "core", "delta"),
"Creating an AABB centered as core and is of half-dimension delta."))
- .def(init<Vec3f, Vec3f, Vec3f>(bp::args("self", "a", "b", "c"),
+ .def(init<Vec3s, Vec3s, Vec3s>(bp::args("self", "a", "b", "c"),
"Creating an AABB contains three points."))
- .def("contain", (bool(AABB::*)(const Vec3f&) const) & AABB::contain,
+ .def("contain", (bool(AABB::*)(const Vec3s&) const) & AABB::contain,
bp::args("self", "p"), "Check whether the AABB contains a point p.")
.def("contain", (bool(AABB::*)(const AABB&) const) & AABB::contain,
bp::args("self", "other"),
@@ -536,18 +536,18 @@ void exposeCollisionGeometries() {
.add_property(
"min_",
bp::make_function(
- +[](AABB& self) -> Vec3f& { return self.min_; },
+ +[](AABB& self) -> Vec3s& { return self.min_; },
bp::return_internal_reference<>()),
bp::make_function(
- +[](AABB& self, const Vec3f& min_) -> void { self.min_ = min_; }),
+ +[](AABB& self, const Vec3s& min_) -> void { self.min_ = min_; }),
"The min point in the AABB.")
.add_property(
"max_",
bp::make_function(
- +[](AABB& self) -> Vec3f& { return self.max_; },
+ +[](AABB& self) -> Vec3s& { return self.max_; },
bp::return_internal_reference<>()),
bp::make_function(
- +[](AABB& self, const Vec3f& max_) -> void { self.max_ = max_; }),
+ +[](AABB& self, const Vec3s& max_) -> void { self.max_ = max_; }),
"The max point in the AABB.")
.def(bp::self == bp::self)
@@ -555,7 +555,7 @@ void exposeCollisionGeometries() {
.def(bp::self + bp::self)
.def(bp::self += bp::self)
- .def(bp::self += Vec3f())
+ .def(bp::self += Vec3s())
.def("size", &AABB::volume, bp::arg("self"), "Size of the AABB.")
.def("center", &AABB::center, bp::arg("self"), "Center of the AABB.")
@@ -578,19 +578,19 @@ void exposeCollisionGeometries() {
// doxygen::member_func_args(static_cast<AABB&
// (AABB::*)(const CoalScalar)>(&AABB::expand)),
bp::return_internal_reference<>())
- .def("expand", static_cast<AABB& (AABB::*)(const Vec3f&)>(&AABB::expand),
+ .def("expand", static_cast<AABB& (AABB::*)(const Vec3s&)>(&AABB::expand),
// doxygen::member_func_doc(static_cast<AABB& (AABB::*)(const
- // Vec3f &)>(&AABB::expand)),
+ // Vec3s &)>(&AABB::expand)),
// doxygen::member_func_args(static_cast<AABB&
- // (AABB::*)(const Vec3f &)>(&AABB::expand)),
+ // (AABB::*)(const Vec3s &)>(&AABB::expand)),
bp::return_internal_reference<>())
.def_pickle(PickleObject<AABB>())
.def(SerializableVisitor<AABB>());
- def("translate", (AABB(*)(const AABB&, const Vec3f&)) & translate,
+ def("translate", (AABB(*)(const AABB&, const Vec3s&)) & translate,
bp::args("aabb", "t"), "Translate the center of AABB by t");
- def("rotate", (AABB(*)(const AABB&, const Matrix3f&)) & rotate,
+ def("rotate", (AABB(*)(const AABB&, const Matrix3s&)) & rotate,
bp::args("aabb", "R"), "Rotate the AABB object by R");
if (!eigenpy::register_symbolic_link_to_registered_type<
@@ -671,9 +671,9 @@ void exposeCollisionGeometries() {
.def(dv::member_func("addTriangle", &BVHModelBase::addTriangle))
.def(dv::member_func("addTriangles", &BVHModelBase::addTriangles))
.def(dv::member_func<int (BVHModelBase::*)(
- const Vec3fs&, const Triangles&)>("addSubModel",
+ const Vec3ss&, const Triangles&)>("addSubModel",
&BVHModelBase::addSubModel))
- .def(dv::member_func<int (BVHModelBase::*)(const Vec3fs&)>(
+ .def(dv::member_func<int (BVHModelBase::*)(const Vec3ss&)>(
"addSubModel", &BVHModelBase::addSubModel))
.def(dv::member_func("endModel", &BVHModelBase::endModel))
// Expose function to replace a BVH
@@ -701,7 +701,7 @@ void exposeCollisionObject() {
.def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
const Transform3f&, bp::optional<bool>>())
.def(dv::init<CollisionObject, const CollisionGeometryPtr_t&,
- const Matrix3f&, const Vec3f&, bp::optional<bool>>())
+ const Matrix3s&, const Vec3s&, bp::optional<bool>>())
.DEF_CLASS_FUNC(CollisionObject, getObjectType)
.DEF_CLASS_FUNC(CollisionObject, getNodeType)
diff --git a/python/collision.cc b/python/collision.cc
index dac1c2db..12e596a1 100644
--- a/python/collision.cc
+++ b/python/collision.cc
@@ -65,10 +65,10 @@ const CollisionGeometry* geto(const Contact& c) {
}
struct ContactWrapper {
- static Vec3f getNearestPoint1(const Contact& contact) {
+ static Vec3s getNearestPoint1(const Contact& contact) {
return contact.nearest_points[0];
}
- static Vec3f getNearestPoint2(const Contact& contact) {
+ static Vec3s getNearestPoint2(const Contact& contact) {
return contact.nearest_points[1];
}
};
@@ -182,8 +182,8 @@ void exposeCollisionAPI() {
.def(dv::init<Contact, const CollisionGeometry*,
const CollisionGeometry*, int, int>())
.def(dv::init<Contact, const CollisionGeometry*,
- const CollisionGeometry*, int, int, const Vec3f&,
- const Vec3f&, CoalScalar>())
+ const CollisionGeometry*, int, int, const Vec3s&,
+ const Vec3s&, CoalScalar>())
.add_property(
"o1",
make_function(&geto<1>,
diff --git a/python/distance.cc b/python/distance.cc
index 0e0032d9..d83df014 100644
--- a/python/distance.cc
+++ b/python/distance.cc
@@ -59,10 +59,10 @@ using namespace coal::python;
namespace dv = doxygen::visitor;
struct DistanceResultWrapper {
- static Vec3f getNearestPoint1(const DistanceResult& res) {
+ static Vec3s getNearestPoint1(const DistanceResult& res) {
return res.nearest_points[0];
}
- static Vec3f getNearestPoint2(const DistanceResult& res) {
+ static Vec3s getNearestPoint2(const DistanceResult& res) {
return res.nearest_points[1];
}
};
diff --git a/python/gjk.cc b/python/gjk.cc
index 9f705fb9..a098f2b1 100644
--- a/python/gjk.cc
+++ b/python/gjk.cc
@@ -52,7 +52,7 @@ using coal::details::MinkowskiDiff;
using coal::details::SupportOptions;
struct MinkowskiDiffWrapper {
- static void support0(MinkowskiDiff& self, const Vec3f& dir, int& hint,
+ static void support0(MinkowskiDiff& self, const Vec3s& dir, int& hint,
bool compute_swept_sphere_support = false) {
if (compute_swept_sphere_support) {
self.support0<SupportOptions::WithSweptSphere>(dir, hint);
@@ -61,7 +61,7 @@ struct MinkowskiDiffWrapper {
}
}
- static void support1(MinkowskiDiff& self, const Vec3f& dir, int& hint,
+ static void support1(MinkowskiDiff& self, const Vec3s& dir, int& hint,
bool compute_swept_sphere_support = false) {
if (compute_swept_sphere_support) {
self.support1<SupportOptions::WithSweptSphere>(dir, hint);
diff --git a/python/math.cc b/python/math.cc
index 85c365d4..119d29e0 100644
--- a/python/math.cc
+++ b/python/math.cc
@@ -74,17 +74,17 @@ void exposeMaths() {
if (!eigenpy::register_symbolic_link_to_registered_type<Eigen::AngleAxisd>())
eigenpy::exposeAngleAxis();
- eigenpy::enableEigenPySpecific<Matrix3f>();
- eigenpy::enableEigenPySpecific<Vec3f>();
+ eigenpy::enableEigenPySpecific<Matrix3s>();
+ eigenpy::enableEigenPySpecific<Vec3s>();
class_<Transform3f>("Transform3f", doxygen::class_doc<Transform3f>(), no_init)
.def(dv::init<Transform3f>())
- .def(dv::init<Transform3f, const Matrix3f::MatrixBase&,
- const Vec3f::MatrixBase&>())
- .def(dv::init<Transform3f, const Quatf&, const Vec3f::MatrixBase&>())
- .def(dv::init<Transform3f, const Matrix3f&>())
+ .def(dv::init<Transform3f, const Matrix3s::MatrixBase&,
+ const Vec3s::MatrixBase&>())
+ .def(dv::init<Transform3f, const Quatf&, const Vec3s::MatrixBase&>())
+ .def(dv::init<Transform3f, const Matrix3s&>())
.def(dv::init<Transform3f, const Quatf&>())
- .def(dv::init<Transform3f, const Vec3f&>())
+ .def(dv::init<Transform3f, const Vec3s&>())
.def(dv::init<Transform3f, const Transform3f&>())
.def(dv::member_func("getQuatRotation", &Transform3f::getQuatRotation))
@@ -99,19 +99,19 @@ void exposeMaths() {
doxygen::member_func_doc(&Transform3f::getTranslation))
.def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation))
- .def("setTranslation", &Transform3f::setTranslation<Vec3f>)
- .def("setRotation", &Transform3f::setRotation<Matrix3f>)
+ .def("setTranslation", &Transform3f::setTranslation<Vec3s>)
+ .def("setRotation", &Transform3f::setRotation<Matrix3s>)
.def(dv::member_func("setTransform",
- &Transform3f::setTransform<Matrix3f, Vec3f>))
+ &Transform3f::setTransform<Matrix3s, Vec3s>))
.def(dv::member_func(
"setTransform",
- static_cast<void (Transform3f::*)(const Quatf&, const Vec3f&)>(
+ static_cast<void (Transform3f::*)(const Quatf&, const Vec3s&)>(
&Transform3f::setTransform)))
.def(dv::member_func("setIdentity", &Transform3f::setIdentity))
.def(dv::member_func("Identity", &Transform3f::Identity))
.staticmethod("Identity")
- .def(dv::member_func("transform", &Transform3f::transform<Vec3f>))
+ .def(dv::member_func("transform", &Transform3f::transform<Vec3s>))
.def("inverseInPlace", &Transform3f::inverseInPlace,
return_internal_reference<>(),
doxygen::member_func_doc(&Transform3f::inverseInPlace))
@@ -137,9 +137,9 @@ void exposeMaths() {
.def(self == self);
if (!eigenpy::register_symbolic_link_to_registered_type<
- std::vector<Vec3f> >()) {
- class_<std::vector<Vec3f> >("StdVec_Vec3f")
- .def(vector_indexing_suite<std::vector<Vec3f> >());
+ std::vector<Vec3s> >()) {
+ class_<std::vector<Vec3s> >("StdVec_Vec3s")
+ .def(vector_indexing_suite<std::vector<Vec3s> >());
}
if (!eigenpy::register_symbolic_link_to_registered_type<
std::vector<Triangle> >()) {
diff --git a/python/octree.cc b/python/octree.cc
index a569bba5..a33ac166 100644
--- a/python/octree.cc
+++ b/python/octree.cc
@@ -53,7 +53,7 @@ void exposeOctree() {
.def("tobytes", tobytes, doxygen::member_func_doc(&OcTree::tobytes));
doxygen::def("makeOctree", &makeOctree);
- eigenpy::enableEigenPySpecific<Vec6f>();
- eigenpy::StdVectorPythonVisitor<std::vector<Vec6f>, true>::expose(
+ eigenpy::enableEigenPySpecific<Vec6s>();
+ eigenpy::StdVectorPythonVisitor<std::vector<Vec6s>, true>::expose(
"StdVec_Vec6");
}
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index de3d582d..4e28a254 100644
--- a/src/BV/AABB.cpp
+++ b/src/BV/AABB.cpp
@@ -44,8 +44,8 @@
namespace coal {
AABB::AABB()
- : min_(Vec3f::Constant((std::numeric_limits<CoalScalar>::max)())),
- max_(Vec3f::Constant(-(std::numeric_limits<CoalScalar>::max)())) {}
+ : min_(Vec3s::Constant((std::numeric_limits<CoalScalar>::max)())),
+ max_(Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)())) {}
bool AABB::overlap(const AABB& other, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) const {
@@ -53,7 +53,7 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request,
request.break_distance * request.break_distance;
sqrDistLowerBound =
- (min_ - other.max_ - Vec3f::Constant(request.security_margin))
+ (min_ - other.max_ - Vec3s::Constant(request.security_margin))
.array()
.max(CoalScalar(0))
.matrix()
@@ -61,7 +61,7 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request,
if (sqrDistLowerBound > break_distance_squared) return false;
sqrDistLowerBound =
- (other.min_ - max_ - Vec3f::Constant(request.security_margin))
+ (other.min_ - max_ - Vec3s::Constant(request.security_margin))
.array()
.max(CoalScalar(0))
.matrix()
@@ -71,7 +71,7 @@ bool AABB::overlap(const AABB& other, const CollisionRequest& request,
return true;
}
-CoalScalar AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const {
+CoalScalar AABB::distance(const AABB& other, Vec3s* P, Vec3s* Q) const {
CoalScalar result = 0;
for (Eigen::DenseIndex i = 0; i < 3; ++i) {
const CoalScalar& amin = min_[i];
@@ -131,13 +131,13 @@ CoalScalar AABB::distance(const AABB& other) const {
return std::sqrt(result);
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
const AABB& b2) {
AABB bb1(translate(rotate(b1, R0), T0));
return bb1.overlap(b2);
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) {
AABB bb1(translate(rotate(b1, R0), T0));
@@ -149,11 +149,11 @@ bool AABB::overlap(const Plane& p) const {
// points in the directions normal and -normal.
// If both points lie on different sides of the plane, there is an overlap
// between the AABB and the plane. Otherwise, there is no overlap.
- const Vec3f halfside = (this->max_ - this->min_) / 2;
- const Vec3f center = (this->max_ + this->min_) / 2;
+ const Vec3s halfside = (this->max_ - this->min_) / 2;
+ const Vec3s center = (this->max_ + this->min_) / 2;
- const Vec3f support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
- const Vec3f support2 =
+ const Vec3s support1 = (p.n.array() > 0).select(halfside, -halfside) + center;
+ const Vec3s support2 =
((-p.n).array() > 0).select(halfside, -halfside) + center;
const CoalScalar dist1 = p.n.dot(support1) - p.d;
@@ -185,9 +185,9 @@ bool AABB::overlap(const Halfspace& hs) const {
// If the support is below the plane defined by the halfspace, there is an
// overlap between the AABB and the halfspace. Otherwise, there is no
// overlap.
- Vec3f halfside = (this->max_ - this->min_) / 2;
- Vec3f center = (this->max_ + this->min_) / 2;
- Vec3f support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
+ Vec3s halfside = (this->max_ - this->min_) / 2;
+ Vec3s center = (this->max_ + this->min_) / 2;
+ Vec3s support = ((-hs.n).array() > 0).select(halfside, -halfside) + center;
return (hs.signedDistance(support) < 0);
}
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 621b059c..6ae95bcf 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -47,32 +47,32 @@
namespace coal {
/// @brief Compute the 8 vertices of a OBB
-inline void computeVertices(const OBB& b, Vec3f vertices[8]) {
- Matrix3f extAxes(b.axes * b.extent.asDiagonal());
- vertices[0].noalias() = b.To + extAxes * Vec3f(-1, -1, -1);
- vertices[1].noalias() = b.To + extAxes * Vec3f(1, -1, -1);
- vertices[2].noalias() = b.To + extAxes * Vec3f(1, 1, -1);
- vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1, -1);
- vertices[4].noalias() = b.To + extAxes * Vec3f(-1, -1, 1);
- vertices[5].noalias() = b.To + extAxes * Vec3f(1, -1, 1);
- vertices[6].noalias() = b.To + extAxes * Vec3f(1, 1, 1);
- vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1);
+inline void computeVertices(const OBB& b, Vec3s vertices[8]) {
+ Matrix3s extAxes(b.axes * b.extent.asDiagonal());
+ vertices[0].noalias() = b.To + extAxes * Vec3s(-1, -1, -1);
+ vertices[1].noalias() = b.To + extAxes * Vec3s(1, -1, -1);
+ vertices[2].noalias() = b.To + extAxes * Vec3s(1, 1, -1);
+ vertices[3].noalias() = b.To + extAxes * Vec3s(-1, 1, -1);
+ vertices[4].noalias() = b.To + extAxes * Vec3s(-1, -1, 1);
+ vertices[5].noalias() = b.To + extAxes * Vec3s(1, -1, 1);
+ vertices[6].noalias() = b.To + extAxes * Vec3s(1, 1, 1);
+ vertices[7].noalias() = b.To + extAxes * Vec3s(-1, 1, 1);
}
/// @brief OBB merge method when the centers of two smaller OBB are far away
inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
OBB b;
- Vec3f vertex[16];
+ Vec3s vertex[16];
computeVertices(b1, vertex);
computeVertices(b2, vertex + 8);
- Matrix3f M;
- Vec3f E[3];
+ Matrix3s M;
+ Vec3s E[3];
CoalScalar s[3] = {0, 0, 0};
// obb axes
b.axes.col(0).noalias() = (b1.To - b2.To).normalized();
- Vec3f vertex_proj[16];
+ Vec3s vertex_proj[16];
for (int i = 0; i < 16; ++i)
vertex_proj[i].noalias() =
vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0));
@@ -102,7 +102,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid];
// set obb centers and extensions
- Vec3f center, extent;
+ Vec3s center, extent;
getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent);
b.To.noalias() = center;
@@ -121,10 +121,10 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
Quatf q((q0.coeffs() + q1.coeffs()).normalized());
b.axes = q.toRotationMatrix();
- Vec3f vertex[8], diff;
+ Vec3s vertex[8], diff;
CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
- Vec3f pmin(real_max, real_max, real_max);
- Vec3f pmax(-real_max, -real_max, -real_max);
+ Vec3s pmin(real_max, real_max, real_max);
+ Vec3s pmax(-real_max, -real_max, -real_max);
computeVertices(b1, vertex);
for (int i = 0; i < 8; ++i) {
@@ -158,12 +158,12 @@ inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
return b;
}
-bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
- const Vec3f& b) {
+bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b) {
CoalScalar t, s;
const CoalScalar reps = 1e-6;
- Matrix3f Bf(B.array().abs() + reps);
+ Matrix3s Bf(B.array().abs() + reps);
// Bf += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
@@ -286,17 +286,17 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
}
namespace internal {
-inline CoalScalar obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a,
- const Vec3f& b, const Matrix3f& Bf) {
+inline CoalScalar obbDisjoint_check_A_axis(const Vec3s& T, const Vec3s& a,
+ const Vec3s& b, const Matrix3s& Bf) {
// |T| - |B| * b - a
- Vec3f AABB_corner(T.cwiseAbs() - a);
+ Vec3s AABB_corner(T.cwiseAbs() - a);
AABB_corner.noalias() -= Bf * b;
return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm();
}
-inline CoalScalar obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
- const Matrix3f& Bf) {
+inline CoalScalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
+ const Matrix3s& Bf) {
// Bf = |B|
// | B^T T| - Bf^T * a - b
CoalScalar s, t = 0;
@@ -311,9 +311,9 @@ inline CoalScalar obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T,
template <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 CoalScalar& breakDistance2,
+ static inline bool run(int ia, int ja, int ka, const Matrix3s& B,
+ const Vec3s& T, const Vec3s& a, const Vec3s& b,
+ const Matrix3s& Bf, const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
CoalScalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
if (sinus2 < 1e-6) return false;
@@ -341,8 +341,8 @@ struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi {
//
// This function tests whether bounding boxes should be broken down.
//
-bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a_, const Vec3f& b_,
+bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a_, const Vec3s& b_,
const CollisionRequest& request,
CoalScalar& squaredLowerBoundDistance) {
assert(request.security_margin >
@@ -354,11 +354,11 @@ bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T,
const CoalScalar breakDistance2 =
request.break_distance * request.break_distance;
- Matrix3f Bf(B.cwiseAbs());
- const Vec3f a((a_ + Vec3f::Constant(request.security_margin / 2))
+ Matrix3s Bf(B.cwiseAbs());
+ const Vec3s a((a_ + Vec3s::Constant(request.security_margin / 2))
.array()
.max(CoalScalar(0)));
- const Vec3f b((b_ + Vec3f::Constant(request.security_margin / 2))
+ const Vec3s b((b_ + Vec3s::Constant(request.security_margin / 2))
.array()
.max(CoalScalar(0)));
@@ -396,8 +396,8 @@ bool OBB::overlap(const OBB& other) const {
/// compute what transform [R,T] that takes us from cs1 to cs2.
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
/// First compute the rotation part, then translation part
- Vec3f T(axes.transpose() * (other.To - To));
- Matrix3f R(axes.transpose() * other.axes);
+ Vec3s T(axes.transpose() * (other.To - To));
+ Matrix3s R(axes.transpose() * other.axes);
return !obbDisjoint(R, T, extent, other.extent);
}
@@ -407,23 +407,23 @@ bool OBB::overlap(const OBB& other, const CollisionRequest& request,
/// compute what transform [R,T] that takes us from cs1 to cs2.
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
/// First compute the rotation part, then translation part
- // Vec3f t = other.To - To; // T2 - T1
- // Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- // Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
+ // Vec3s t = other.To - To; // T2 - T1
+ // Vec3s T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
+ // Matrix3s R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
// axis[0].dot(other.axis[2]),
// axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
// axis[1].dot(other.axis[2]),
// axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
// axis[2].dot(other.axis[2]));
- Vec3f T(axes.transpose() * (other.To - To));
- Matrix3f R(axes.transpose() * other.axes);
+ Vec3s T(axes.transpose() * (other.To - To));
+ Matrix3s R(axes.transpose() * other.axes);
return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request,
sqrDistLowerBound);
}
-bool OBB::contain(const Vec3f& p) const {
- Vec3f local_p(p - To);
+bool OBB::contain(const Vec3s& p) const {
+ Vec3s local_p(p - To);
CoalScalar proj = local_p.dot(axes.col(0));
if ((proj > extent[0]) || (proj < -extent[0])) return false;
@@ -436,7 +436,7 @@ bool OBB::contain(const Vec3f& p) const {
return true;
}
-OBB& OBB::operator+=(const Vec3f& p) {
+OBB& OBB::operator+=(const Vec3s& p) {
OBB bvp;
bvp.To = p;
bvp.axes.noalias() = axes;
@@ -447,7 +447,7 @@ OBB& OBB::operator+=(const Vec3f& p) {
}
OBB OBB::operator+(const OBB& other) const {
- Vec3f center_diff = To - other.To;
+ Vec3s center_diff = To - other.To;
CoalScalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
CoalScalar max_extent2 =
std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
@@ -458,32 +458,32 @@ OBB OBB::operator+(const OBB& other) const {
}
}
-CoalScalar OBB::distance(const OBB& /*other*/, Vec3f* /*P*/,
- Vec3f* /*Q*/) const {
+CoalScalar OBB::distance(const OBB& /*other*/, Vec3s* /*P*/,
+ Vec3s* /*Q*/) const {
std::cerr << "OBB distance not implemented!" << std::endl;
return 0.0;
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
const OBB& b2) {
- Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
- Vec3f T(b1.axes.transpose() * Ttemp);
- Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+ Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
+ Vec3s T(b1.axes.transpose() * Ttemp);
+ Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
return !obbDisjoint(R, T, b1.extent, b2.extent);
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,
const CollisionRequest& request, CoalScalar& sqrDistLowerBound) {
- Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
- Vec3f T(b1.axes.transpose() * Ttemp);
- Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+ Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
+ Vec3s T(b1.axes.transpose() * Ttemp);
+ Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request,
sqrDistLowerBound);
}
-OBB translate(const OBB& bv, const Vec3f& t) {
+OBB translate(const OBB& bv, const Vec3s& t) {
OBB res(bv);
res.To += t;
return res;
diff --git a/src/BV/OBB.h b/src/BV/OBB.h
index 907272a5..a7d3a381 100644
--- a/src/BV/OBB.h
+++ b/src/BV/OBB.h
@@ -38,13 +38,13 @@
namespace coal {
-bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
+bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
const CollisionRequest& request,
CoalScalar& squaredLowerBoundDistance);
-bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
- const Vec3f& b);
+bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b);
} // namespace coal
#endif // COAL_SRC_OBB_H
diff --git a/src/BV/OBBRSS.cpp b/src/BV/OBBRSS.cpp
index 18a0c83f..1842e60f 100644
--- a/src/BV/OBBRSS.cpp
+++ b/src/BV/OBBRSS.cpp
@@ -39,7 +39,7 @@
namespace coal {
-OBBRSS translate(const OBBRSS& bv, const Vec3f& t) {
+OBBRSS translate(const OBBRSS& bv, const Vec3s& t) {
OBBRSS res(bv);
res.obb.To += t;
res.rss.Tr += t;
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 15c3043a..97d5d1ad 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -117,9 +117,9 @@ bool inVoronoi(CoalScalar a, CoalScalar b, CoalScalar Anorm_dot_B,
/// @brief Distance between two oriented rectangles; P and Q (optional return
/// values) are the closest points in the rectangles, both are in the local
/// frame of the first rectangle.
-CoalScalar rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
+CoalScalar rectDistance(const Matrix3s& Rab, Vec3s const& Tab,
const CoalScalar a[2], const CoalScalar b[2],
- Vec3f* P = NULL, Vec3f* Q = NULL) {
+ Vec3s* P = NULL, Vec3s* Q = NULL) {
CoalScalar A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
A0_dot_B0 = Rab(0, 0);
@@ -139,9 +139,9 @@ CoalScalar rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
bA0_dot_B1 = b[1] * A0_dot_B1;
bA1_dot_B1 = b[1] * A1_dot_B1;
- Vec3f Tba(Rab.transpose() * Tab);
+ Vec3s Tba(Rab.transpose() * Tab);
- Vec3f S;
+ Vec3s S;
CoalScalar t, u;
// determine if any edge pair contains the closest points
@@ -687,8 +687,8 @@ CoalScalar rectDistance(const Matrix3f& Rab, Vec3f const& Tab,
}
if (sep2 >= sep1 && sep2 >= 0) {
- Vec3f Q_(Tab[0], Tab[1], Tab[2]);
- Vec3f P_;
+ Vec3s Q_(Tab[0], Tab[1], Tab[2]);
+ Vec3s P_;
if (Tba[2] < 0) {
P_[0] = Rab(0, 2) * sep2 + Tab[0];
P_[1] = Rab(1, 2) * sep2 + Tab[1];
@@ -717,40 +717,40 @@ bool RSS::overlap(const RSS& other) const {
/// First compute the rotation part, then translation part
/// Then compute R1'(T2 - T1)
- Vec3f T(axes.transpose() * (other.Tr - Tr));
+ Vec3s T(axes.transpose() * (other.Tr - Tr));
/// Now compute R1'R2
- Matrix3f R(axes.transpose() * other.axes);
+ Matrix3s R(axes.transpose() * other.axes);
CoalScalar dist = rectDistance(R, T, length, other.length);
return (dist <= (radius + other.radius));
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
const RSS& b2) {
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
- Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
- Vec3f T(b1.axes.transpose() * Ttemp);
- Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+ Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
+ Vec3s T(b1.axes.transpose() * Ttemp);
+ Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
CoalScalar dist = rectDistance(R, T, b1.length, b2.length);
return (dist <= (b1.radius + b2.radius));
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2,
const CollisionRequest& request, CoalScalar& sqrDistLowerBound) {
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
- Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
- Vec3f T(b1.axes.transpose() * Ttemp);
- Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes);
+ Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
+ Vec3s T(b1.axes.transpose() * Ttemp);
+ Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
CoalScalar dist = rectDistance(R, T, b1.length, b2.length) - b1.radius -
b2.radius - request.security_margin;
@@ -759,14 +759,14 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
return false;
}
-bool RSS::contain(const Vec3f& p) const {
- Vec3f local_p = p - Tr;
- // FIXME: Vec3f proj (axes.transpose() * local_p);
+bool RSS::contain(const Vec3s& p) const {
+ Vec3s local_p = p - Tr;
+ // FIXME: Vec3s proj (axes.transpose() * local_p);
CoalScalar proj0 = local_p.dot(axes.col(0));
CoalScalar proj1 = local_p.dot(axes.col(1));
CoalScalar proj2 = local_p.dot(axes.col(2));
CoalScalar abs_proj2 = fabs(proj2);
- Vec3f proj(proj0, proj1, proj2);
+ Vec3s proj(proj0, proj1, proj2);
/// projection is within the rectangle
if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) &&
@@ -775,28 +775,28 @@ bool RSS::contain(const Vec3f& p) const {
} else if ((proj0 < length[0]) && (proj0 > 0) &&
((proj1 < 0) || (proj1 > length[1]))) {
CoalScalar y = (proj1 > 0) ? length[1] : 0;
- Vec3f v(proj0, y, 0);
+ Vec3s v(proj0, y, 0);
return ((proj - v).squaredNorm() < radius * radius);
} else if ((proj1 < length[1]) && (proj1 > 0) &&
((proj0 < 0) || (proj0 > length[0]))) {
CoalScalar x = (proj0 > 0) ? length[0] : 0;
- Vec3f v(x, proj1, 0);
+ Vec3s v(x, proj1, 0);
return ((proj - v).squaredNorm() < radius * radius);
} else {
CoalScalar x = (proj0 > 0) ? length[0] : 0;
CoalScalar y = (proj1 > 0) ? length[1] : 0;
- Vec3f v(x, y, 0);
+ Vec3s v(x, y, 0);
return ((proj - v).squaredNorm() < radius * radius);
}
}
-RSS& RSS::operator+=(const Vec3f& p) {
- Vec3f local_p = p - Tr;
+RSS& RSS::operator+=(const Vec3s& p) {
+ Vec3s local_p = p - Tr;
CoalScalar proj0 = local_p.dot(axes.col(0));
CoalScalar proj1 = local_p.dot(axes.col(1));
CoalScalar proj2 = local_p.dot(axes.col(2));
CoalScalar abs_proj2 = fabs(proj2);
- Vec3f proj(proj0, proj1, proj2);
+ Vec3s proj(proj0, proj1, proj2);
// projection is within the rectangle
if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) &&
@@ -814,7 +814,7 @@ RSS& RSS::operator+=(const Vec3f& p) {
} else if ((proj0 < length[0]) && (proj0 > 0) &&
((proj1 < 0) || (proj1 > length[1]))) {
CoalScalar y = (proj1 > 0) ? length[1] : 0;
- Vec3f v(proj0, y, 0);
+ Vec3s v(proj0, y, 0);
CoalScalar new_r_sqr = (proj - v).squaredNorm();
if (new_r_sqr < radius * radius)
; // do nothing
@@ -838,7 +838,7 @@ RSS& RSS::operator+=(const Vec3f& p) {
} else if ((proj1 < length[1]) && (proj1 > 0) &&
((proj0 < 0) || (proj0 > length[0]))) {
CoalScalar x = (proj0 > 0) ? length[0] : 0;
- Vec3f v(x, proj1, 0);
+ Vec3s v(x, proj1, 0);
CoalScalar new_r_sqr = (proj - v).squaredNorm();
if (new_r_sqr < radius * radius)
; // do nothing
@@ -862,7 +862,7 @@ RSS& RSS::operator+=(const Vec3f& p) {
} else {
CoalScalar x = (proj0 > 0) ? length[0] : 0;
CoalScalar y = (proj1 > 0) ? length[1] : 0;
- Vec3f v(x, y, 0);
+ Vec3s v(x, y, 0);
CoalScalar new_r_sqr = (proj - v).squaredNorm();
if (new_r_sqr < radius * radius)
; // do nothing
@@ -907,13 +907,13 @@ RSS& RSS::operator+=(const Vec3f& p) {
RSS RSS::operator+(const RSS& other) const {
RSS bv;
- Vec3f v[16];
- Vec3f d0_pos(other.axes.col(0) * (other.length[0] + other.radius));
- Vec3f d1_pos(other.axes.col(1) * (other.length[1] + other.radius));
- Vec3f d0_neg(other.axes.col(0) * (-other.radius));
- Vec3f d1_neg(other.axes.col(1) * (-other.radius));
- Vec3f d2_pos(other.axes.col(2) * other.radius);
- Vec3f d2_neg(other.axes.col(2) * (-other.radius));
+ Vec3s v[16];
+ Vec3s d0_pos(other.axes.col(0) * (other.length[0] + other.radius));
+ Vec3s d1_pos(other.axes.col(1) * (other.length[1] + other.radius));
+ Vec3s d0_neg(other.axes.col(0) * (-other.radius));
+ Vec3s d1_neg(other.axes.col(1) * (-other.radius));
+ Vec3s d2_pos(other.axes.col(2) * other.radius);
+ Vec3s d2_neg(other.axes.col(2) * (-other.radius));
v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos;
v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg;
@@ -940,8 +940,8 @@ RSS RSS::operator+(const RSS& other) const {
v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos;
v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg;
- Matrix3f M; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
+ Matrix3s M; // row first matrix
+ Vec3s E[3]; // row first eigen-vectors
CoalScalar s[3] = {0, 0, 0};
getCovariance(v, NULL, NULL, NULL, 16, M);
@@ -979,31 +979,31 @@ RSS RSS::operator+(const RSS& other) const {
return bv;
}
-CoalScalar RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const {
+CoalScalar RSS::distance(const RSS& other, Vec3s* P, Vec3s* Q) const {
// compute what transform [R,T] that takes us from cs1 to cs2.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
// First compute the rotation part, then translation part
- Matrix3f R(axes.transpose() * other.axes);
- Vec3f T(axes.transpose() * (other.Tr - Tr));
+ Matrix3s R(axes.transpose() * other.axes);
+ Vec3s T(axes.transpose() * (other.Tr - Tr));
CoalScalar dist = rectDistance(R, T, length, other.length, P, Q);
dist -= (radius + other.radius);
return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist;
}
-CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1,
- const RSS& b2, Vec3f* P, Vec3f* Q) {
- Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
- Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr);
+CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
+ const RSS& b2, Vec3s* P, Vec3s* Q) {
+ Matrix3s R(b1.axes.transpose() * R0 * b2.axes);
+ Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr);
- Vec3f T(b1.axes.transpose() * Ttemp);
+ Vec3s T(b1.axes.transpose() * Ttemp);
CoalScalar dist = rectDistance(R, T, b1.length, b2.length, P, Q);
dist -= (b1.radius + b2.radius);
return (dist < (CoalScalar)0.0) ? (CoalScalar)0.0 : dist;
}
-RSS translate(const RSS& bv, const Vec3f& t) {
+RSS translate(const RSS& bv, const Vec3s& t) {
RSS res(bv);
res.Tr += t;
return res;
diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
index cc581a84..d59ccc6b 100644
--- a/src/BV/kDOP.cpp
+++ b/src/BV/kDOP.cpp
@@ -63,11 +63,11 @@ inline void minmax(CoalScalar p, CoalScalar& minv, CoalScalar& maxv) {
/// @brief Compute the distances to planes with normals from KDOP vectors except
/// those of AABB face planes
template <short N>
-void getDistances(const Vec3f& /*p*/, CoalScalar* /*d*/) {}
+void getDistances(const Vec3s& /*p*/, CoalScalar* /*d*/) {}
/// @brief Specification of getDistances
template <>
-inline void getDistances<5>(const Vec3f& p, CoalScalar* d) {
+inline void getDistances<5>(const Vec3s& p, CoalScalar* d) {
d[0] = p[0] + p[1];
d[1] = p[0] + p[2];
d[2] = p[1] + p[2];
@@ -76,7 +76,7 @@ inline void getDistances<5>(const Vec3f& p, CoalScalar* d) {
}
template <>
-inline void getDistances<6>(const Vec3f& p, CoalScalar* d) {
+inline void getDistances<6>(const Vec3s& p, CoalScalar* d) {
d[0] = p[0] + p[1];
d[1] = p[0] + p[2];
d[2] = p[1] + p[2];
@@ -86,7 +86,7 @@ inline void getDistances<6>(const Vec3f& p, CoalScalar* d) {
}
template <>
-inline void getDistances<9>(const Vec3f& p, CoalScalar* d) {
+inline void getDistances<9>(const Vec3s& p, CoalScalar* d) {
d[0] = p[0] + p[1];
d[1] = p[0] + p[2];
d[2] = p[1] + p[2];
@@ -106,7 +106,7 @@ KDOP<N>::KDOP() {
}
template <short N>
-KDOP<N>::KDOP(const Vec3f& v) {
+KDOP<N>::KDOP(const Vec3s& v) {
for (short i = 0; i < 3; ++i) {
dist_[i] = dist_[N / 2 + i] = v[i];
}
@@ -119,7 +119,7 @@ KDOP<N>::KDOP(const Vec3f& v) {
}
template <short N>
-KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b) {
+KDOP<N>::KDOP(const Vec3s& a, const Vec3s& b) {
for (short i = 0; i < 3; ++i) {
minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
}
@@ -168,7 +168,7 @@ bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
}
template <short N>
-bool KDOP<N>::inside(const Vec3f& p) const {
+bool KDOP<N>::inside(const Vec3s& p) const {
if ((p.array() < dist_.template head<3>()).any()) return false;
if ((p.array() > dist_.template segment<3>(N / 2)).any()) return false;
@@ -183,7 +183,7 @@ bool KDOP<N>::inside(const Vec3f& p) const {
}
template <short N>
-KDOP<N>& KDOP<N>::operator+=(const Vec3f& p) {
+KDOP<N>& KDOP<N>::operator+=(const Vec3s& p) {
for (short i = 0; i < 3; ++i) {
minmax(p[i], dist_[i], dist_[N / 2 + i]);
}
@@ -213,14 +213,14 @@ KDOP<N> KDOP<N>::operator+(const KDOP<N>& other) const {
}
template <short N>
-CoalScalar KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/,
- Vec3f* /*Q*/) const {
+CoalScalar KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3s* /*P*/,
+ Vec3s* /*Q*/) const {
std::cerr << "KDOP distance not implemented!" << std::endl;
return 0.0;
}
template <short N>
-KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t) {
+KDOP<N> translate(const KDOP<N>& bv, const Vec3s& t) {
KDOP<N> res(bv);
for (short i = 0; i < 3; ++i) {
res.dist(i) += t[i];
@@ -241,8 +241,8 @@ template class KDOP<16>;
template class KDOP<18>;
template class KDOP<24>;
-template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&);
-template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&);
-template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&);
+template KDOP<16> translate<16>(const KDOP<16>&, const Vec3s&);
+template KDOP<18> translate<18>(const KDOP<18>&, const Vec3s&);
+template KDOP<24> translate<24>(const KDOP<24>&, const Vec3s&);
} // namespace coal
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index 10e9d7fd..802496e0 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -73,7 +73,7 @@ bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
return obb.overlap(other.obb, request, sqrDistLowerBound);
}
-bool kIOS::contain(const Vec3f& p) const {
+bool kIOS::contain(const Vec3s& p) const {
for (unsigned int i = 0; i < num_spheres; ++i) {
CoalScalar r = spheres[i].r;
if ((spheres[i].o - p).squaredNorm() > r * r) return false;
@@ -82,7 +82,7 @@ bool kIOS::contain(const Vec3f& p) const {
return true;
}
-kIOS& kIOS::operator+=(const Vec3f& p) {
+kIOS& kIOS::operator+=(const Vec3s& p) {
for (unsigned int i = 0; i < num_spheres; ++i) {
CoalScalar r = spheres[i].r;
CoalScalar new_r_sqr = (p - spheres[i].o).squaredNorm();
@@ -119,7 +119,7 @@ CoalScalar kIOS::volume() const { return obb.volume(); }
CoalScalar kIOS::size() const { return volume(); }
-CoalScalar kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const {
+CoalScalar kIOS::distance(const kIOS& other, Vec3s* P, Vec3s* Q) const {
CoalScalar d_max = 0;
long id_a = -1, id_b = -1;
for (unsigned int i = 0; i < num_spheres; ++i) {
@@ -138,7 +138,7 @@ CoalScalar kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const {
if (P && Q) {
if (id_a != -1 && id_b != -1) {
- const Vec3f v = spheres[id_a].o - spheres[id_b].o;
+ const Vec3s v = spheres[id_a].o - spheres[id_b].o;
CoalScalar len_v = v.norm();
*P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
*Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
@@ -148,7 +148,7 @@ CoalScalar kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const {
return d_max;
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
const kIOS& b2) {
kIOS b2_temp = b2;
for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
@@ -162,7 +162,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
return b1.overlap(b2_temp);
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
+bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
const kIOS& b2, const CollisionRequest& request,
CoalScalar& sqrDistLowerBound) {
kIOS b2_temp = b2;
@@ -177,8 +177,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
return b1.overlap(b2_temp, request, sqrDistLowerBound);
}
-CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
- const kIOS& b2, Vec3f* P, Vec3f* Q) {
+CoalScalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1,
+ const kIOS& b2, Vec3s* P, Vec3s* Q) {
kIOS b2_temp = b2;
for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
@@ -187,7 +187,7 @@ CoalScalar distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
return b1.distance(b2_temp, P, Q);
}
-kIOS translate(const kIOS& bv, const Vec3f& t) {
+kIOS translate(const kIOS& bv, const Vec3s& t) {
kIOS res(bv);
for (size_t i = 0; i < res.num_spheres; ++i) {
res.spheres[i].o += t;
diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp
index 807549e6..9cfdcab9 100644
--- a/src/BVH/BVH_model.cpp
+++ b/src/BVH/BVH_model.cpp
@@ -66,7 +66,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other)
num_tris_allocated(other.num_tris),
num_vertices_allocated(other.num_vertices) {
if (other.vertices.get() && other.vertices->size() > 0) {
- vertices.reset(new std::vector<Vec3f>(*(other.vertices)));
+ vertices.reset(new std::vector<Vec3s>(*(other.vertices)));
} else
vertices.reset();
@@ -76,7 +76,7 @@ BVHModelBase::BVHModelBase(const BVHModelBase& other)
tri_indices.reset();
if (other.prev_vertices.get() && other.prev_vertices->size() > 0) {
- prev_vertices.reset(new std::vector<Vec3f>(*(other.prev_vertices)));
+ prev_vertices.reset(new std::vector<Vec3s>(*(other.prev_vertices)));
} else
prev_vertices.reset();
}
@@ -105,8 +105,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
(vertices.get() && !(other.vertices.get())))
return false;
if (vertices.get() && other.vertices.get()) {
- const std::vector<Vec3f>& vertices_ = *(vertices);
- const std::vector<Vec3f>& other_vertices_ = *(other.vertices);
+ const std::vector<Vec3s>& vertices_ = *(vertices);
+ const std::vector<Vec3s>& other_vertices_ = *(other.vertices);
for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k)
if (vertices_[k] != other_vertices_[k]) return false;
}
@@ -115,8 +115,8 @@ bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
(prev_vertices.get() && !(other.prev_vertices.get())))
return false;
if (prev_vertices.get() && other.prev_vertices.get()) {
- const std::vector<Vec3f>& prev_vertices_ = *(prev_vertices);
- const std::vector<Vec3f>& other_prev_vertices_ = *(other.prev_vertices);
+ const std::vector<Vec3s>& prev_vertices_ = *(prev_vertices);
+ const std::vector<Vec3s>& other_prev_vertices_ = *(other.prev_vertices);
for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k) {
if (prev_vertices_[k] != other_prev_vertices_[k]) return false;
}
@@ -140,10 +140,10 @@ void BVHModelBase::buildConvexRepresentation(bool share_memory) {
}
if (!convex) {
- std::shared_ptr<std::vector<Vec3f>> points = vertices;
+ std::shared_ptr<std::vector<Vec3s>> points = vertices;
std::shared_ptr<std::vector<Triangle>> polygons = tri_indices;
if (!share_memory) {
- points.reset(new std::vector<Vec3f>(*(vertices)));
+ points.reset(new std::vector<Vec3s>(*(vertices)));
polygons.reset(new std::vector<Triangle>(*(tri_indices)));
}
convex.reset(
@@ -206,7 +206,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_,
tri_indices.reset();
if (num_vertices_allocated > 0) {
- vertices.reset(new std::vector<Vec3f>(num_vertices_allocated));
+ vertices.reset(new std::vector<Vec3s>(num_vertices_allocated));
if (!(vertices.get())) {
std::cerr
<< "BVH Error! Out of memory for vertices array on BeginModel() call!"
@@ -232,7 +232,7 @@ int BVHModelBase::beginModel(unsigned int num_tris_,
return BVH_OK;
}
-int BVHModelBase::addVertex(const Vec3f& p) {
+int BVHModelBase::addVertex(const Vec3s& p) {
if (build_state != BVH_BUILD_STATE_BEGUN) {
std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() "
"was ignored. Must do a beginModel() to clear the model for "
@@ -242,8 +242,8 @@ int BVHModelBase::addVertex(const Vec3f& p) {
}
if (num_vertices >= num_vertices_allocated) {
- std::shared_ptr<std::vector<Vec3f>> temp(
- new std::vector<Vec3f>(num_vertices_allocated * 2));
+ std::shared_ptr<std::vector<Vec3s>> temp(
+ new std::vector<Vec3s>(num_vertices_allocated * 2));
if (!(temp.get())) {
std::cerr
<< "BVH Error! Out of memory for vertices array on addVertex() call!"
@@ -304,7 +304,7 @@ int BVHModelBase::addTriangles(const Matrixx3i& triangles) {
return BVH_OK;
}
-int BVHModelBase::addVertices(const Matrixx3f& points) {
+int BVHModelBase::addVertices(const MatrixX3s& points) {
if (build_state != BVH_BUILD_STATE_BEGUN) {
std::cerr << "BVH Warning! Call addVertex() in a wrong order. "
"addVertices() was ignored. Must do a beginModel() to clear "
@@ -315,8 +315,8 @@ int BVHModelBase::addVertices(const Matrixx3f& points) {
if (num_vertices + points.rows() > num_vertices_allocated) {
num_vertices_allocated = num_vertices + (unsigned int)points.rows();
- std::shared_ptr<std::vector<Vec3f>> temp(
- new std::vector<Vec3f>(num_vertices_allocated));
+ std::shared_ptr<std::vector<Vec3s>> temp(
+ new std::vector<Vec3s>(num_vertices_allocated));
if (!(temp.get())) {
std::cerr
<< "BVH Error! Out of memory for vertices array on addVertex() call!"
@@ -330,15 +330,15 @@ int BVHModelBase::addVertices(const Matrixx3f& points) {
vertices = temp;
}
- std::vector<Vec3f>& vertices_ = *vertices;
+ std::vector<Vec3s>& vertices_ = *vertices;
for (Eigen::DenseIndex id = 0; id < points.rows(); ++id)
vertices_[num_vertices++] = points.row(id).transpose();
return BVH_OK;
}
-int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
- const Vec3f& p3) {
+int BVHModelBase::addTriangle(const Vec3s& p1, const Vec3s& p2,
+ const Vec3s& p3) {
if (build_state == BVH_BUILD_STATE_PROCESSED) {
std::cerr << "BVH Warning! Call addTriangle() in a wrong order. "
"addTriangle() was ignored. Must do a beginModel() to clear "
@@ -348,8 +348,8 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
}
if (num_vertices + 2 >= num_vertices_allocated) {
- std::shared_ptr<std::vector<Vec3f>> temp(
- new std::vector<Vec3f>(num_vertices_allocated * 2 + 2));
+ std::shared_ptr<std::vector<Vec3s>> temp(
+ new std::vector<Vec3s>(num_vertices_allocated * 2 + 2));
if (!(temp.get())) {
std::cerr << "BVH Error! Out of memory for vertices array on "
"addTriangle() call!"
@@ -398,7 +398,7 @@ int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
return BVH_OK;
}
-int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
+int BVHModelBase::addSubModel(const std::vector<Vec3s>& ps) {
if (build_state == BVH_BUILD_STATE_PROCESSED) {
std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. "
"addSubModel() was ignored. Must do a beginModel() to clear "
@@ -410,7 +410,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
const unsigned int num_vertices_to_add = (unsigned int)ps.size();
if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
- std::shared_ptr<std::vector<Vec3f>> temp(new std::vector<Vec3f>(
+ std::shared_ptr<std::vector<Vec3s>> temp(new std::vector<Vec3s>(
num_vertices_allocated * 2 + num_vertices_to_add - 1));
if (!(temp.get())) {
std::cerr << "BVH Error! Out of memory for vertices array on "
@@ -427,7 +427,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
num_vertices_allocated * 2 + num_vertices_to_add - 1;
}
- std::vector<Vec3f>& vertices_ = *vertices;
+ std::vector<Vec3s>& vertices_ = *vertices;
for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
vertices_[num_vertices] = ps[i];
num_vertices++;
@@ -436,7 +436,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
return BVH_OK;
}
-int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
+int BVHModelBase::addSubModel(const std::vector<Vec3s>& ps,
const std::vector<Triangle>& ts) {
if (build_state == BVH_BUILD_STATE_PROCESSED) {
std::cerr << "BVH Warning! Calling addSubModel() in a wrong order. "
@@ -449,7 +449,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
const unsigned int num_vertices_to_add = (unsigned int)ps.size();
if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
- std::shared_ptr<std::vector<Vec3f>> temp(new std::vector<Vec3f>(
+ std::shared_ptr<std::vector<Vec3s>> temp(new std::vector<Vec3s>(
num_vertices_allocated * 2 + num_vertices_to_add - 1));
if (!(temp.get())) {
std::cerr << "BVH Error! Out of memory for vertices array on "
@@ -468,7 +468,7 @@ int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
const unsigned int offset = num_vertices;
- std::vector<Vec3f>& vertices_ = *vertices;
+ std::vector<Vec3s>& vertices_ = *vertices;
for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
vertices_[num_vertices] = ps[i];
num_vertices++;
@@ -543,8 +543,8 @@ int BVHModelBase::endModel() {
if (num_vertices_allocated > num_vertices) {
if (num_vertices > 0) {
- std::shared_ptr<std::vector<Vec3f>> new_vertices(
- new std::vector<Vec3f>(num_vertices));
+ std::shared_ptr<std::vector<Vec3s>> new_vertices(
+ new std::vector<Vec3s>(num_vertices));
if (!(new_vertices.get())) {
std::cerr
<< "BVH Error! Out of memory for vertices array in endModel() call!"
@@ -591,7 +591,7 @@ int BVHModelBase::beginReplaceModel() {
return BVH_OK;
}
-int BVHModelBase::replaceVertex(const Vec3f& p) {
+int BVHModelBase::replaceVertex(const Vec3s& p) {
if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. "
"replaceVertex() was ignored. Must do a beginReplaceModel() "
@@ -606,8 +606,8 @@ int BVHModelBase::replaceVertex(const Vec3f& p) {
return BVH_OK;
}
-int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2,
- const Vec3f& p3) {
+int BVHModelBase::replaceTriangle(const Vec3s& p1, const Vec3s& p2,
+ const Vec3s& p3) {
if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. "
"replaceTriangle() was ignored. Must do a beginReplaceModel() "
@@ -625,7 +625,7 @@ int BVHModelBase::replaceTriangle(const Vec3f& p1, const Vec3f& p2,
return BVH_OK;
}
-int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps) {
+int BVHModelBase::replaceSubModel(const std::vector<Vec3s>& ps) {
if (build_state != BVH_BUILD_STATE_REPLACE_BEGUN) {
std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. "
"replaceSubModel() was ignored. Must do a beginReplaceModel() "
@@ -634,7 +634,7 @@ int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps) {
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
- std::vector<Vec3f>& vertices_ = *vertices;
+ std::vector<Vec3s>& vertices_ = *vertices;
for (unsigned int i = 0; i < ps.size(); ++i) {
vertices_[num_vertex_updated] = ps[i];
num_vertex_updated++;
@@ -680,12 +680,12 @@ int BVHModelBase::beginUpdateModel() {
}
if (prev_vertices.get()) {
- std::shared_ptr<std::vector<Vec3f>> temp = prev_vertices;
+ std::shared_ptr<std::vector<Vec3s>> temp = prev_vertices;
prev_vertices = vertices;
vertices = temp;
} else {
prev_vertices = vertices;
- vertices.reset(new std::vector<Vec3f>(num_vertices));
+ vertices.reset(new std::vector<Vec3s>(num_vertices));
}
num_vertex_updated = 0;
@@ -695,7 +695,7 @@ int BVHModelBase::beginUpdateModel() {
return BVH_OK;
}
-int BVHModelBase::updateVertex(const Vec3f& p) {
+int BVHModelBase::updateVertex(const Vec3s& p) {
if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
std::cerr
<< "BVH Warning! Call updateVertex() in a wrong order. updateVertex() "
@@ -710,8 +710,8 @@ int BVHModelBase::updateVertex(const Vec3f& p) {
return BVH_OK;
}
-int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2,
- const Vec3f& p3) {
+int BVHModelBase::updateTriangle(const Vec3s& p1, const Vec3s& p2,
+ const Vec3s& p3) {
if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. "
"updateTriangle() was ignored. Must do a beginUpdateModel() "
@@ -729,7 +729,7 @@ int BVHModelBase::updateTriangle(const Vec3f& p1, const Vec3f& p2,
return BVH_OK;
}
-int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps) {
+int BVHModelBase::updateSubModel(const std::vector<Vec3s>& ps) {
if (build_state != BVH_BUILD_STATE_UPDATE_BEGUN) {
std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. "
"updateSubModel() was ignored. Must do a beginUpdateModel() "
@@ -738,7 +738,7 @@ int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps) {
return BVH_ERR_BUILD_OUT_OF_SEQUENCE;
}
- std::vector<Vec3f>& vertices_ = *vertices;
+ std::vector<Vec3s>& vertices_ = *vertices;
for (unsigned int i = 0; i < ps.size(); ++i) {
vertices_[num_vertex_updated] = ps[i];
num_vertex_updated++;
@@ -780,7 +780,7 @@ int BVHModelBase::endUpdateModel(bool refit, bool bottomup) {
void BVHModelBase::computeLocalAABB() {
AABB aabb_;
- const std::vector<Vec3f>& vertices_ = *vertices;
+ const std::vector<Vec3s>& vertices_ = *vertices;
for (unsigned int i = 0; i < num_vertices; ++i) {
aabb_ += vertices_[i];
}
@@ -840,7 +840,7 @@ template <typename BV>
int BVHModel<BV>::memUsage(const bool msg) const {
unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs;
unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris;
- unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3f) * num_vertices;
+ unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3s) * num_vertices;
unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list +
(unsigned int)sizeof(BVHModel<BV>);
@@ -857,7 +857,7 @@ int BVHModel<BV>::memUsage(const bool msg) const {
template <typename BV>
int BVHModel<BV>::buildTree() {
// set BVFitter
- Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL;
+ Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
bv_fitter->set(vertices_, tri_indices_, getModelType());
// set SplitRule
@@ -911,17 +911,17 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive,
num_bvs += 2;
unsigned int c1 = 0;
- const std::vector<Vec3f>& vertices_ = *vertices;
+ const std::vector<Vec3s>& vertices_ = *vertices;
const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_primitives; ++i) {
- Vec3f p;
+ Vec3s p;
if (type == BVH_MODEL_POINTCLOUD)
p = vertices_[cur_primitive_indices[i]];
else if (type == BVH_MODEL_TRIANGLES) {
const Triangle& t = tri_indices_[cur_primitive_indices[i]];
- const Vec3f& p1 = vertices_[t[0]];
- const Vec3f& p2 = vertices_[t[1]];
- const Vec3f& p3 = vertices_[t[2]];
+ const Vec3s& p1 = vertices_[t[0]];
+ const Vec3s& p2 = vertices_[t[1]];
+ const Vec3s& p3 = vertices_[t[2]];
p = (p1 + p2 + p3) / 3.;
} else {
@@ -989,7 +989,7 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
BV bv;
if (prev_vertices.get()) {
- Vec3f v[2];
+ Vec3s v[2];
v[0] = (*prev_vertices)[static_cast<size_t>(primitive_id)];
v[1] = (*vertices)[static_cast<size_t>(primitive_id)];
fit(v, 2, bv);
@@ -1003,7 +1003,7 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
(*tri_indices)[static_cast<size_t>(primitive_id)];
if (prev_vertices.get()) {
- Vec3f v[6];
+ Vec3s v[6];
for (Triangle::index_type i = 0; i < 3; ++i) {
v[i] = (*prev_vertices)[triangle[i]];
v[i + 3] = (*vertices)[triangle[i]];
@@ -1015,7 +1015,7 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
// unsigned int* cur_primitive_indices = primitive_indices +
// bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices,
// bvnode->num_primitives);
- Vec3f v[3];
+ Vec3s v[3];
for (int i = 0; i < 3; ++i) {
v[i] = (*vertices)[triangle[(Triangle::index_type)i]];
}
@@ -1044,8 +1044,8 @@ int BVHModel<BV>::recursiveRefitTree_bottomup(int bv_id) {
template <typename BV>
int BVHModel<BV>::refitTree_topdown() {
- Vec3f* vertices_ = vertices.get() ? vertices->data() : NULL;
- Vec3f* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL;
+ Vec3s* vertices_ = vertices.get() ? vertices->data() : NULL;
+ Vec3s* prev_vertices_ = prev_vertices.get() ? prev_vertices->data() : NULL;
Triangle* tri_indices_ = tri_indices.get() ? tri_indices->data() : NULL;
bv_fitter->set(vertices_, prev_vertices_, tri_indices_, getModelType());
BVNode<BV>* bvs_ = bvs->data();
@@ -1062,8 +1062,8 @@ int BVHModel<BV>::refitTree_topdown() {
}
template <>
-void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
- const Vec3f& parent_c) {
+void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+ const Vec3s& parent_c) {
bv_node_vector_t& bvs_ = *bvs;
OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv;
if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
@@ -1078,13 +1078,13 @@ void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
// obb.axes = parent_axes.transpose() * obb.axes;
obb.axes.applyOnTheLeft(parent_axes.transpose());
- Vec3f t(obb.To - parent_c);
+ Vec3s t(obb.To - parent_c);
obb.To.noalias() = parent_axes.transpose() * t;
}
template <>
-void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
- const Vec3f& parent_c) {
+void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3s& parent_axes,
+ const Vec3s& parent_c) {
bv_node_vector_t& bvs_ = *bvs;
RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv;
if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
@@ -1099,14 +1099,14 @@ void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
// rss.axes = parent_axes.transpose() * rss.axes;
rss.axes.applyOnTheLeft(parent_axes.transpose());
- Vec3f t(rss.Tr - parent_c);
+ Vec3s t(rss.Tr - parent_c);
rss.Tr.noalias() = parent_axes.transpose() * t;
}
template <>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
- Matrix3f& parent_axes,
- const Vec3f& parent_c) {
+ Matrix3s& parent_axes,
+ const Vec3s& parent_c) {
bv_node_vector_t& bvs_ = *bvs;
OBB& obb = bvs_[static_cast<size_t>(bv_id)].bv.obb;
RSS& rss = bvs_[static_cast<size_t>(bv_id)].bv.rss;
@@ -1122,7 +1122,7 @@ void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id,
rss.axes.noalias() = parent_axes.transpose() * obb.axes;
obb.axes = rss.axes;
- Vec3f t(obb.To - parent_c);
+ Vec3s t(obb.To - parent_c);
obb.To.noalias() = parent_axes.transpose() * t;
rss.Tr = obb.To;
}
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index 9a919431..5ce7c2cc 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -47,7 +47,7 @@ template <typename BV>
BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
const AABB& _aabb) {
assert(model.getModelType() == BVH_MODEL_TRIANGLES);
- const Matrix3f& q = pose.getRotation();
+ const Matrix3s& q = pose.getRotation();
AABB aabb = translate(_aabb, -pose.getTranslation());
Transform3f box_pose;
@@ -64,7 +64,7 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
std::vector<bool> keep_vertex(model.num_vertices, false);
std::vector<bool> keep_tri(model.num_tris, false);
unsigned int ntri = 0;
- const std::vector<Vec3f>& model_vertices_ = *(model.vertices);
+ const std::vector<Vec3s>& model_vertices_ = *(model.vertices);
const std::vector<Triangle>& model_tri_indices_ = *(model.tri_indices);
for (unsigned int i = 0; i < model.num_tris; ++i) {
const Triangle& t = model_tri_indices_[i];
@@ -111,7 +111,7 @@ BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose,
new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices));
std::vector<unsigned int> idxConversion(model.num_vertices);
assert(new_model->num_vertices == 0);
- std::vector<Vec3f>& new_model_vertices_ = *(new_model->vertices);
+ std::vector<Vec3s>& new_model_vertices_ = *(new_model->vertices);
for (unsigned int i = 0; i < keep_vertex.size(); ++i) {
if (keep_vertex[i]) {
idxConversion[i] = new_model->num_vertices;
@@ -179,18 +179,18 @@ BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model,
return details::BVHExtract(model, pose, aabb);
}
-void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
- unsigned int n, Matrix3f& M) {
- Vec3f S1(Vec3f::Zero());
- Vec3f S2[3] = {Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero()};
+void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices,
+ unsigned int n, Matrix3s& M) {
+ Vec3s S1(Vec3s::Zero());
+ Vec3s S2[3] = {Vec3s::Zero(), Vec3s::Zero(), Vec3s::Zero()};
if (ts) {
for (unsigned int i = 0; i < n; ++i) {
const Triangle& t = (indices) ? ts[indices[i]] : ts[i];
- const Vec3f& p1 = ps[t[0]];
- const Vec3f& p2 = ps[t[1]];
- const Vec3f& p3 = ps[t[2]];
+ const Vec3s& p1 = ps[t[0]];
+ const Vec3s& p2 = ps[t[1]];
+ const Vec3s& p3 = ps[t[2]];
S1[0] += (p1[0] + p2[0] + p3[0]);
S1[1] += (p1[1] + p2[1] + p3[1]);
@@ -203,9 +203,9 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
if (ps2) {
- const Vec3f& p1 = ps2[t[0]];
- const Vec3f& p2 = ps2[t[1]];
- const Vec3f& p3 = ps2[t[2]];
+ const Vec3s& p1 = ps2[t[0]];
+ const Vec3s& p2 = ps2[t[1]];
+ const Vec3s& p3 = ps2[t[2]];
S1[0] += (p1[0] + p2[0] + p3[0]);
S1[1] += (p1[1] + p2[1] + p3[1]);
@@ -221,7 +221,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
}
} else {
for (unsigned int i = 0; i < n; ++i) {
- const Vec3f& p = (indices) ? ps[indices[i]] : ps[i];
+ const Vec3s& p = (indices) ? ps[indices[i]] : ps[i];
S1 += p;
S2[0][0] += (p[0] * p[0]);
S2[1][1] += (p[1] * p[1]);
@@ -232,7 +232,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
if (ps2) // another frame
{
- const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i];
+ const Vec3s& p = (indices) ? ps2[indices[i]] : ps2[i];
S1 += p;
S2[0][0] += (p[0] * p[0]);
S2[1][1] += (p[1] * p[1]);
@@ -260,9 +260,9 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices,
/** @brief Compute the RSS bounding volume parameters: radius, rectangle size
* and the origin. The bounding volume axes are known.
*/
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+void getRadiusAndOriginAndRectangleSize(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
- const Matrix3f& axes, Vec3f& origin,
+ const Matrix3s& axes, Vec3s& origin,
CoalScalar l[2], CoalScalar& r) {
bool indirect_index = true;
if (!indices) indirect_index = false;
@@ -280,8 +280,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
for (Triangle::index_type j = 0; j < 3; ++j) {
Triangle::index_type point_id = t[j];
- const Vec3f& p = ps[point_id];
- Vec3f v(p[0], p[1], p[2]);
+ const Vec3s& p = ps[point_id];
+ Vec3s v(p[0], p[1], p[2]);
P[P_id][0] = axes.col(0).dot(v);
P[P_id][1] = axes.col(1).dot(v);
P[P_id][2] = axes.col(2).dot(v);
@@ -291,9 +291,9 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
if (ps2) {
for (Triangle::index_type j = 0; j < 3; ++j) {
Triangle::index_type point_id = t[j];
- const Vec3f& p = ps2[point_id];
+ const Vec3s& p = ps2[point_id];
// FIXME Is this right ?????
- Vec3f v(p[0], p[1], p[2]);
+ Vec3s v(p[0], p[1], p[2]);
P[P_id][0] = axes.col(0).dot(v);
P[P_id][1] = axes.col(0).dot(v);
P[P_id][2] = axes.col(1).dot(v);
@@ -305,15 +305,15 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
for (unsigned int i = 0; i < n; ++i) {
unsigned int index = indirect_index ? indices[i] : i;
- const Vec3f& p = ps[index];
- Vec3f v(p[0], p[1], p[2]);
+ const Vec3s& p = ps[index];
+ Vec3s v(p[0], p[1], p[2]);
P[P_id][0] = axes.col(0).dot(v);
P[P_id][1] = axes.col(1).dot(v);
P[P_id][2] = axes.col(2).dot(v);
P_id++;
if (ps2) {
- const Vec3f& v = ps2[index];
+ const Vec3s& v = ps2[index];
P[P_id][0] = axes.col(0).dot(v);
P[P_id][1] = axes.col(1).dot(v);
P[P_id][2] = axes.col(2).dot(v);
@@ -472,7 +472,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
}
}
- origin.noalias() = axes * Vec3f(minx, miny, cz);
+ origin.noalias() = axes * Vec3s(minx, miny, cz);
l[0] = std::max<CoalScalar>(maxx - minx, 0);
l[1] = std::max<CoalScalar>(maxy - miny, 0);
@@ -483,23 +483,23 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts,
/** @brief Compute the bounding volume extent and center for a set or subset of
* points. The bounding volume axes are known.
*/
-static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2,
+static inline void getExtentAndCenter_pointcloud(Vec3s* ps, Vec3s* ps2,
unsigned int* indices,
- unsigned int n, Matrix3f& axes,
- Vec3f& center, Vec3f& extent) {
+ unsigned int n, Matrix3s& axes,
+ Vec3s& center, Vec3s& extent) {
bool indirect_index = true;
if (!indices) indirect_index = false;
CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
- Vec3f min_coord(real_max, real_max, real_max);
- Vec3f max_coord(-real_max, -real_max, -real_max);
+ Vec3s min_coord(real_max, real_max, real_max);
+ Vec3s max_coord(-real_max, -real_max, -real_max);
for (unsigned int i = 0; i < n; ++i) {
unsigned int index = indirect_index ? indices[i] : i;
- const Vec3f& p = ps[index];
- Vec3f proj(axes.transpose() * p);
+ const Vec3s& p = ps[index];
+ Vec3s proj(axes.transpose() * p);
for (int j = 0; j < 3; ++j) {
if (proj[j] > max_coord[j]) max_coord[j] = proj[j];
@@ -507,7 +507,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2,
}
if (ps2) {
- const Vec3f& v = ps2[index];
+ const Vec3s& v = ps2[index];
proj.noalias() = axes.transpose() * v;
for (int j = 0; j < 3; ++j) {
@@ -525,17 +525,17 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2,
/** @brief Compute the bounding volume extent and center for a set or subset of
* points. The bounding volume axes are known.
*/
-static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+static inline void getExtentAndCenter_mesh(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices,
- unsigned int n, Matrix3f& axes,
- Vec3f& center, Vec3f& extent) {
+ unsigned int n, Matrix3s& axes,
+ Vec3s& center, Vec3s& extent) {
bool indirect_index = true;
if (!indices) indirect_index = false;
CoalScalar real_max = (std::numeric_limits<CoalScalar>::max)();
- Vec3f min_coord(real_max, real_max, real_max);
- Vec3f max_coord(-real_max, -real_max, -real_max);
+ Vec3s min_coord(real_max, real_max, real_max);
+ Vec3s max_coord(-real_max, -real_max, -real_max);
for (unsigned int i = 0; i < n; ++i) {
unsigned int index = indirect_index ? indices[i] : i;
@@ -543,8 +543,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
for (Triangle::index_type j = 0; j < 3; ++j) {
Triangle::index_type point_id = t[j];
- const Vec3f& p = ps[point_id];
- Vec3f proj(axes.transpose() * p);
+ const Vec3s& p = ps[point_id];
+ Vec3s proj(axes.transpose() * p);
for (int k = 0; k < 3; ++k) {
if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
@@ -555,8 +555,8 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
if (ps2) {
for (Triangle::index_type j = 0; j < 3; ++j) {
Triangle::index_type point_id = t[j];
- const Vec3f& p = ps2[point_id];
- Vec3f proj(axes.transpose() * p);
+ const Vec3s& p = ps2[point_id];
+ Vec3s proj(axes.transpose() * p);
for (int k = 0; k < 3; ++k) {
if (proj[k] > max_coord[k]) max_coord[k] = proj[k];
@@ -566,29 +566,29 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
}
}
- Vec3f o((max_coord + min_coord) / 2);
+ Vec3s o((max_coord + min_coord) / 2);
center.noalias() = axes * o;
extent.noalias() = (max_coord - min_coord) / 2;
}
-void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts,
- unsigned int* indices, unsigned int n, Matrix3f& axes,
- Vec3f& center, Vec3f& extent) {
+void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts,
+ unsigned int* indices, unsigned int n, Matrix3s& axes,
+ Vec3s& center, Vec3s& extent) {
if (ts)
getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent);
else
getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent);
}
-void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c,
- Vec3f& center, CoalScalar& radius) {
- Vec3f e1 = a - c;
- Vec3f e2 = b - c;
+void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c,
+ Vec3s& center, CoalScalar& radius) {
+ Vec3s e1 = a - c;
+ Vec3s e2 = b - c;
CoalScalar e1_len2 = e1.squaredNorm();
CoalScalar e2_len2 = e2.squaredNorm();
- Vec3f e3 = e1.cross(e2);
+ Vec3s e3 = e1.cross(e2);
CoalScalar e3_len2 = e3.squaredNorm();
radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2;
radius = std::sqrt(radius) * 0.5;
@@ -596,11 +596,11 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c,
center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
}
-static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2,
+static inline CoalScalar maximumDistance_mesh(Vec3s* ps, Vec3s* ps2,
Triangle* ts,
unsigned int* indices,
unsigned int n,
- const Vec3f& query) {
+ const Vec3s& query) {
bool indirect_index = true;
if (!indices) indirect_index = false;
@@ -611,7 +611,7 @@ static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2,
for (Triangle::index_type j = 0; j < 3; ++j) {
Triangle::index_type point_id = t[j];
- const Vec3f& p = ps[point_id];
+ const Vec3s& p = ps[point_id];
CoalScalar d = (p - query).squaredNorm();
if (d > maxD) maxD = d;
@@ -620,7 +620,7 @@ static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2,
if (ps2) {
for (Triangle::index_type j = 0; j < 3; ++j) {
Triangle::index_type point_id = t[j];
- const Vec3f& p = ps2[point_id];
+ const Vec3s& p = ps2[point_id];
CoalScalar d = (p - query).squaredNorm();
if (d > maxD) maxD = d;
@@ -631,10 +631,10 @@ static inline CoalScalar maximumDistance_mesh(Vec3f* ps, Vec3f* ps2,
return std::sqrt(maxD);
}
-static inline CoalScalar maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2,
+static inline CoalScalar maximumDistance_pointcloud(Vec3s* ps, Vec3s* ps2,
unsigned int* indices,
unsigned int n,
- const Vec3f& query) {
+ const Vec3s& query) {
bool indirect_index = true;
if (!indices) indirect_index = false;
@@ -642,12 +642,12 @@ static inline CoalScalar maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2,
for (unsigned int i = 0; i < n; ++i) {
unsigned int index = indirect_index ? indices[i] : i;
- const Vec3f& p = ps[index];
+ const Vec3s& p = ps[index];
CoalScalar d = (p - query).squaredNorm();
if (d > maxD) maxD = d;
if (ps2) {
- const Vec3f& v = ps2[index];
+ const Vec3s& v = ps2[index];
CoalScalar d = (v - query).squaredNorm();
if (d > maxD) maxD = d;
}
@@ -656,9 +656,9 @@ static inline CoalScalar maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2,
return std::sqrt(maxD);
}
-CoalScalar maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts,
+CoalScalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts,
unsigned int* indices, unsigned int n,
- const Vec3f& query) {
+ const Vec3s& query) {
if (ts)
return maximumDistance_mesh(ps, ps2, ts, indices, n, query);
else
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index deb74700..02a8bc8c 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -47,8 +47,8 @@ static const double kIOS_RATIO = 1.5;
static const double invSinA = 2;
static const double cosA = sqrt(3.0) / 2.0;
-static inline void axisFromEigen(Vec3f eigenV[3], CoalScalar eigenS[3],
- Matrix3f& axes) {
+static inline void axisFromEigen(Vec3s eigenV[3], CoalScalar eigenS[3],
+ Matrix3s& axes) {
int min, mid, max;
if (eigenS[0] > eigenS[1]) {
max = 0;
@@ -77,16 +77,16 @@ static inline void axisFromEigen(Vec3f eigenV[3], CoalScalar eigenS[3],
namespace OBB_fit_functions {
-void fit1(Vec3f* ps, OBB& bv) {
+void fit1(Vec3s* ps, OBB& bv) {
bv.To.noalias() = ps[0];
bv.axes.setIdentity();
bv.extent.setZero();
}
-void fit2(Vec3f* ps, OBB& bv) {
- const Vec3f& p1 = ps[0];
- const Vec3f& p2 = ps[1];
- Vec3f p1p2 = p1 - p2;
+void fit2(Vec3s* ps, OBB& bv) {
+ const Vec3s& p1 = ps[0];
+ const Vec3s& p2 = ps[1];
+ Vec3s p1p2 = p1 - p2;
CoalScalar len_p1p2 = p1p2.norm();
p1p2.normalize();
@@ -97,11 +97,11 @@ void fit2(Vec3f* ps, OBB& bv) {
bv.To.noalias() = (p1 + p2) / 2;
}
-void fit3(Vec3f* ps, OBB& bv) {
- const Vec3f& p1 = ps[0];
- const Vec3f& p2 = ps[1];
- const Vec3f& p3 = ps[2];
- Vec3f e[3];
+void fit3(Vec3s* ps, OBB& bv) {
+ const Vec3s& p1 = ps[0];
+ const Vec3s& p2 = ps[1];
+ const Vec3s& p3 = ps[2];
+ Vec3s e[3];
e[0] = p1 - p2;
e[1] = p2 - p3;
e[2] = p3 - p1;
@@ -121,16 +121,16 @@ void fit3(Vec3f* ps, OBB& bv) {
getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent);
}
-void fit6(Vec3f* ps, OBB& bv) {
+void fit6(Vec3s* ps, OBB& bv) {
OBB bv1, bv2;
fit3(ps, bv1);
fit3(ps + 3, bv2);
bv = bv1 + bv2;
}
-void fitn(Vec3f* ps, unsigned int n, OBB& bv) {
- Matrix3f M;
- Vec3f E[3];
+void fitn(Vec3s* ps, unsigned int n, OBB& bv) {
+ Matrix3s M;
+ Vec3s E[3];
CoalScalar s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, NULL, n, M);
@@ -144,7 +144,7 @@ void fitn(Vec3f* ps, unsigned int n, OBB& bv) {
} // namespace OBB_fit_functions
namespace RSS_fit_functions {
-void fit1(Vec3f* ps, RSS& bv) {
+void fit1(Vec3s* ps, RSS& bv) {
bv.Tr.noalias() = ps[0];
bv.axes.setIdentity();
bv.length[0] = 0;
@@ -152,9 +152,9 @@ void fit1(Vec3f* ps, RSS& bv) {
bv.radius = 0;
}
-void fit2(Vec3f* ps, RSS& bv) {
- const Vec3f& p1 = ps[0];
- const Vec3f& p2 = ps[1];
+void fit2(Vec3s* ps, RSS& bv) {
+ const Vec3s& p1 = ps[0];
+ const Vec3s& p2 = ps[1];
bv.axes.col(0).noalias() = p1 - p2;
CoalScalar len_p1p2 = bv.axes.col(0).norm();
bv.axes.col(0) /= len_p1p2;
@@ -167,11 +167,11 @@ void fit2(Vec3f* ps, RSS& bv) {
bv.radius = 0;
}
-void fit3(Vec3f* ps, RSS& bv) {
- const Vec3f& p1 = ps[0];
- const Vec3f& p2 = ps[1];
- const Vec3f& p3 = ps[2];
- Vec3f e[3];
+void fit3(Vec3s* ps, RSS& bv) {
+ const Vec3s& p1 = ps[0];
+ const Vec3s& p2 = ps[1];
+ const Vec3s& p3 = ps[2];
+ Vec3s e[3];
e[0] = p1 - p2;
e[1] = p2 - p3;
e[2] = p3 - p1;
@@ -192,16 +192,16 @@ void fit3(Vec3f* ps, RSS& bv) {
bv.length, bv.radius);
}
-void fit6(Vec3f* ps, RSS& bv) {
+void fit6(Vec3s* ps, RSS& bv) {
RSS bv1, bv2;
fit3(ps, bv1);
fit3(ps + 3, bv2);
bv = bv1 + bv2;
}
-void fitn(Vec3f* ps, unsigned int n, RSS& bv) {
- Matrix3f M; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
+void fitn(Vec3s* ps, unsigned int n, RSS& bv) {
+ Matrix3s M; // row first matrix
+ Vec3s E[3]; // row first eigen-vectors
CoalScalar s[3] = {0, 0, 0};
getCovariance(ps, NULL, NULL, NULL, n, M);
@@ -217,7 +217,7 @@ void fitn(Vec3f* ps, unsigned int n, RSS& bv) {
namespace kIOS_fit_functions {
-void fit1(Vec3f* ps, kIOS& bv) {
+void fit1(Vec3s* ps, kIOS& bv) {
bv.num_spheres = 1;
bv.spheres[0].o.noalias() = ps[0];
bv.spheres[0].r = 0;
@@ -227,16 +227,16 @@ void fit1(Vec3f* ps, kIOS& bv) {
bv.obb.To.noalias() = ps[0];
}
-void fit2(Vec3f* ps, kIOS& bv) {
+void fit2(Vec3s* ps, kIOS& bv) {
bv.num_spheres = 5;
- const Vec3f& p1 = ps[0];
- const Vec3f& p2 = ps[1];
- Vec3f p1p2 = p1 - p2;
+ const Vec3s& p1 = ps[0];
+ const Vec3s& p2 = ps[1];
+ Vec3s p1p2 = p1 - p2;
CoalScalar len_p1p2 = p1p2.norm();
p1p2.normalize();
- Matrix3f& axes = bv.obb.axes;
+ Matrix3s& axes = bv.obb.axes;
axes.col(0).noalias() = p1p2;
generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2));
@@ -251,7 +251,7 @@ void fit2(Vec3f* ps, kIOS& bv) {
CoalScalar r1cosA = r1 * cosA;
bv.spheres[1].r = r1;
bv.spheres[2].r = r1;
- Vec3f delta = axes.col(1) * r1cosA;
+ Vec3s delta = axes.col(1) * r1cosA;
bv.spheres[1].o = bv.spheres[0].o - delta;
bv.spheres[2].o = bv.spheres[0].o + delta;
@@ -262,13 +262,13 @@ void fit2(Vec3f* ps, kIOS& bv) {
bv.spheres[4].o = bv.spheres[0].o + delta;
}
-void fit3(Vec3f* ps, kIOS& bv) {
+void fit3(Vec3s* ps, kIOS& bv) {
bv.num_spheres = 3;
- const Vec3f& p1 = ps[0];
- const Vec3f& p2 = ps[1];
- const Vec3f& p3 = ps[2];
- Vec3f e[3];
+ const Vec3s& p1 = ps[0];
+ const Vec3s& p2 = ps[1];
+ const Vec3s& p3 = ps[2];
+ Vec3s e[3];
e[0] = p1 - p2;
e[1] = p2 - p3;
e[2] = p3 - p1;
@@ -290,14 +290,14 @@ void fit3(Vec3f* ps, kIOS& bv) {
// compute radius and center
CoalScalar r0;
- Vec3f center;
+ Vec3s center;
circumCircleComputation(p1, p2, p3, center, r0);
bv.spheres[0].o = center;
bv.spheres[0].r = r0;
CoalScalar r1 = r0 * invSinA;
- Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA);
+ Vec3s delta = bv.obb.axes.col(2) * (r1 * cosA);
bv.spheres[1].r = r1;
bv.spheres[1].o = center - delta;
@@ -305,22 +305,22 @@ void fit3(Vec3f* ps, kIOS& bv) {
bv.spheres[2].o = center + delta;
}
-void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
- Matrix3f M;
- Vec3f E[3];
+void fitn(Vec3s* ps, unsigned int n, kIOS& bv) {
+ Matrix3s M;
+ Vec3s E[3];
CoalScalar s[3] = {0, 0, 0}; // three eigen values;
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
- Matrix3f& axes = bv.obb.axes;
+ Matrix3s& axes = bv.obb.axes;
axisFromEigen(E, s, axes);
getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent);
// get center and extension
- const Vec3f& center = bv.obb.To;
- const Vec3f& extent = bv.obb.extent;
+ const Vec3s& center = bv.obb.To;
+ const Vec3s& extent = bv.obb.extent;
CoalScalar r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
// decide the k in kIOS
@@ -337,7 +337,7 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
if (bv.num_spheres >= 3) {
CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
- Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
+ Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
@@ -353,7 +353,7 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
if (bv.num_spheres >= 5) {
CoalScalar r10 = bv.spheres[1].r;
- Vec3f delta =
+ Vec3s delta =
axes.col(1) *
(sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) -
extent[1]);
@@ -375,22 +375,22 @@ void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
} // namespace kIOS_fit_functions
namespace OBBRSS_fit_functions {
-void fit1(Vec3f* ps, OBBRSS& bv) {
+void fit1(Vec3s* ps, OBBRSS& bv) {
OBB_fit_functions::fit1(ps, bv.obb);
RSS_fit_functions::fit1(ps, bv.rss);
}
-void fit2(Vec3f* ps, OBBRSS& bv) {
+void fit2(Vec3s* ps, OBBRSS& bv) {
OBB_fit_functions::fit2(ps, bv.obb);
RSS_fit_functions::fit2(ps, bv.rss);
}
-void fit3(Vec3f* ps, OBBRSS& bv) {
+void fit3(Vec3s* ps, OBBRSS& bv) {
OBB_fit_functions::fit3(ps, bv.obb);
RSS_fit_functions::fit3(ps, bv.rss);
}
-void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) {
+void fitn(Vec3s* ps, unsigned int n, OBBRSS& bv) {
OBB_fit_functions::fitn(ps, n, bv.obb);
RSS_fit_functions::fitn(ps, n, bv.rss);
}
@@ -398,7 +398,7 @@ void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) {
} // namespace OBBRSS_fit_functions
template <>
-void fit(Vec3f* ps, unsigned int n, OBB& bv) {
+void fit(Vec3s* ps, unsigned int n, OBB& bv) {
switch (n) {
case 1:
OBB_fit_functions::fit1(ps, bv);
@@ -418,7 +418,7 @@ void fit(Vec3f* ps, unsigned int n, OBB& bv) {
}
template <>
-void fit(Vec3f* ps, unsigned int n, RSS& bv) {
+void fit(Vec3s* ps, unsigned int n, RSS& bv) {
switch (n) {
case 1:
RSS_fit_functions::fit1(ps, bv);
@@ -435,7 +435,7 @@ void fit(Vec3f* ps, unsigned int n, RSS& bv) {
}
template <>
-void fit(Vec3f* ps, unsigned int n, kIOS& bv) {
+void fit(Vec3s* ps, unsigned int n, kIOS& bv) {
switch (n) {
case 1:
kIOS_fit_functions::fit1(ps, bv);
@@ -452,7 +452,7 @@ void fit(Vec3f* ps, unsigned int n, kIOS& bv) {
}
template <>
-void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) {
+void fit(Vec3s* ps, unsigned int n, OBBRSS& bv) {
switch (n) {
case 1:
OBBRSS_fit_functions::fit1(ps, bv);
@@ -469,7 +469,7 @@ void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) {
}
template <>
-void fit(Vec3f* ps, unsigned int n, AABB& bv) {
+void fit(Vec3s* ps, unsigned int n, AABB& bv) {
if (n <= 0) return;
bv = AABB(ps[0]);
for (unsigned int i = 1; i < n; ++i) {
@@ -481,8 +481,8 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices,
unsigned int num_primitives) {
OBB bv;
- Matrix3f M; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
+ Matrix3s M; // row first matrix
+ Vec3s E[3]; // row first eigen-vectors
CoalScalar s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
@@ -501,8 +501,8 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices,
OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices,
unsigned int num_primitives) {
OBBRSS bv;
- Matrix3f M;
- Vec3f E[3];
+ Matrix3s M;
+ Vec3s E[3];
CoalScalar s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
@@ -515,7 +515,7 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices,
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices,
num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent);
- Vec3f origin;
+ Vec3s origin;
CoalScalar l[2];
CoalScalar r;
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices,
@@ -534,8 +534,8 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices,
unsigned int num_primitives) {
RSS bv;
- Matrix3f M; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
+ Matrix3s M; // row first matrix
+ Vec3s E[3]; // row first eigen-vectors
CoalScalar s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
num_primitives, M);
@@ -544,7 +544,7 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices,
// set rss origin, rectangle size and radius
- Vec3f origin;
+ Vec3s origin;
CoalScalar l[2];
CoalScalar r;
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices,
@@ -563,23 +563,23 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices,
unsigned int num_primitives) {
kIOS bv;
- Matrix3f M; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
+ Matrix3s M; // row first matrix
+ Vec3s E[3]; // row first eigen-vectors
CoalScalar s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
num_primitives, M);
eigen(M, s, E);
- Matrix3f& axes = bv.obb.axes;
+ Matrix3s& axes = bv.obb.axes;
axisFromEigen(E, s, axes);
// get centers and extensions
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices,
num_primitives, axes, bv.obb.To, bv.obb.extent);
- const Vec3f& center = bv.obb.To;
- const Vec3f& extent = bv.obb.extent;
+ const Vec3s& center = bv.obb.To;
+ const Vec3s& extent = bv.obb.extent;
CoalScalar r0 = maximumDistance(vertices, prev_vertices, tri_indices,
primitive_indices, num_primitives, center);
@@ -597,7 +597,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices,
if (bv.num_spheres >= 3) {
CoalScalar r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
- Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
+ Vec3s delta = axes.col(2) * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
@@ -617,7 +617,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices,
if (bv.num_spheres >= 5) {
CoalScalar r10 = bv.spheres[1].r;
- Vec3f delta =
+ Vec3s delta =
axes.col(1) *
(sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) -
extent[1]);
diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index 48c28433..50d40242 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -40,21 +40,21 @@
namespace coal {
template <typename BV>
-void computeSplitVector(const BV& bv, Vec3f& split_vector) {
+void computeSplitVector(const BV& bv, Vec3s& split_vector) {
split_vector = bv.axes.col(0);
}
template <>
-void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector) {
+void computeSplitVector<kIOS>(const kIOS& bv, Vec3s& split_vector) {
/*
switch(bv.num_spheres)
{
case 1:
- split_vector = Vec3f(1, 0, 0);
+ split_vector = Vec3s(1, 0, 0);
break;
case 3:
{
- Vec3f v[3];
+ Vec3s v[3];
v[0] = bv.spheres[1].o - bv.spheres[0].o;
v[0].normalize();
generateCoordinateSystem(v[0], v[1], v[2]);
@@ -63,7 +63,7 @@ void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector) {
break;
case 5:
{
- Vec3f v[2];
+ Vec3s v[2];
v[0] = bv.spheres[1].o - bv.spheres[0].o;
v[1] = bv.spheres[3].o - bv.spheres[0].o;
split_vector = v[0].cross(v[1]);
@@ -78,30 +78,30 @@ void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector) {
}
template <>
-void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3f& split_vector) {
+void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3s& split_vector) {
split_vector = bv.obb.axes.col(0);
}
template <typename BV>
void computeSplitValue_bvcenter(const BV& bv, CoalScalar& split_value) {
- Vec3f center = bv.center();
+ Vec3s center = bv.center();
split_value = center[0];
}
template <typename BV>
-void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles,
+void computeSplitValue_mean(const BV&, Vec3s* vertices, Triangle* triangles,
unsigned int* primitive_indices,
unsigned int num_primitives, BVHModelType type,
- const Vec3f& split_vector,
+ const Vec3s& split_vector,
CoalScalar& split_value) {
if (type == BVH_MODEL_TRIANGLES) {
- Vec3f c(Vec3f::Zero());
+ Vec3s c(Vec3s::Zero());
for (unsigned int i = 0; i < num_primitives; ++i) {
const Triangle& t = triangles[primitive_indices[i]];
- const Vec3f& p1 = vertices[t[0]];
- const Vec3f& p2 = vertices[t[1]];
- const Vec3f& p3 = vertices[t[2]];
+ const Vec3s& p1 = vertices[t[0]];
+ const Vec3s& p2 = vertices[t[1]];
+ const Vec3s& p3 = vertices[t[2]];
c += p1 + p2 + p3;
}
@@ -109,7 +109,7 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles,
} else if (type == BVH_MODEL_POINTCLOUD) {
CoalScalar sum = 0;
for (unsigned int i = 0; i < num_primitives; ++i) {
- const Vec3f& p = vertices[primitive_indices[i]];
+ const Vec3s& p = vertices[primitive_indices[i]];
sum += p.dot(split_vector);
}
@@ -118,28 +118,28 @@ void computeSplitValue_mean(const BV&, Vec3f* vertices, Triangle* triangles,
}
template <typename BV>
-void computeSplitValue_median(const BV&, Vec3f* vertices, Triangle* triangles,
+void computeSplitValue_median(const BV&, Vec3s* vertices, Triangle* triangles,
unsigned int* primitive_indices,
unsigned int num_primitives, BVHModelType type,
- const Vec3f& split_vector,
+ const Vec3s& split_vector,
CoalScalar& split_value) {
std::vector<CoalScalar> proj(num_primitives);
if (type == BVH_MODEL_TRIANGLES) {
for (unsigned int i = 0; i < num_primitives; ++i) {
const Triangle& t = triangles[primitive_indices[i]];
- const Vec3f& p1 = vertices[t[0]];
- const Vec3f& p2 = vertices[t[1]];
- const Vec3f& p3 = vertices[t[2]];
- Vec3f centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1],
+ const Vec3s& p1 = vertices[t[0]];
+ const Vec3s& p2 = vertices[t[1]];
+ const Vec3s& p3 = vertices[t[2]];
+ Vec3s centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1],
p1[2] + p2[2] + p3[2]);
proj[i] = centroid3.dot(split_vector) / 3;
}
} else if (type == BVH_MODEL_POINTCLOUD) {
for (unsigned int i = 0; i < num_primitives; ++i) {
- const Vec3f& p = vertices[primitive_indices[i]];
- Vec3f v(p[0], p[1], p[2]);
+ const Vec3s& p = vertices[primitive_indices[i]];
+ Vec3s v(p[0], p[1], p[2]);
proj[i] = v.dot(split_vector);
}
}
@@ -259,23 +259,23 @@ void BVSplitter<OBBRSS>::computeRule_median(const OBBRSS& bv,
}
template <>
-bool BVSplitter<OBB>::apply(const Vec3f& q) const {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<OBB>::apply(const Vec3s& q) const {
+ return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
}
template <>
-bool BVSplitter<RSS>::apply(const Vec3f& q) const {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<RSS>::apply(const Vec3s& q) const {
+ return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
}
template <>
-bool BVSplitter<kIOS>::apply(const Vec3f& q) const {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<kIOS>::apply(const Vec3s& q) const {
+ return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
}
template <>
-bool BVSplitter<OBBRSS>::apply(const Vec3f& q) const {
- return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+bool BVSplitter<OBBRSS>::apply(const Vec3s& q) const {
+ return split_vector.dot(Vec3s(q[0], q[1], q[2])) > split_value;
}
template class BVSplitter<RSS>;
diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
index ca1ba324..a3995383 100644
--- a/src/broadphase/broadphase_SSaP.cpp
+++ b/src/broadphase/broadphase_SSaP.cpp
@@ -265,10 +265,10 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj,
DistanceCallBackBase* callback,
CoalScalar& min_dist) const {
static const unsigned int CUTOFF = 100;
- Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
- Vec3f dummy_vector = obj->getAABB().max_;
+ Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ Vec3s dummy_vector = obj->getAABB().max_;
if (min_dist < (std::numeric_limits<CoalScalar>::max)())
- dummy_vector += Vec3f(min_dist, min_dist, min_dist);
+ dummy_vector += Vec3s(min_dist, min_dist, min_dist);
typename std::vector<CollisionObject*>::const_iterator pos_start1 =
objs_x.begin();
@@ -335,7 +335,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj,
// to check the possible missed ones to the right of the objs array
if (min_dist < old_min_distance) {
dummy_vector =
- obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
+ obj->getAABB().max_ + Vec3s(min_dist, min_dist, min_dist);
status = 0;
} else // need more loop
{
diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
index 9e0d64c7..b2b27f5f 100644
--- a/src/broadphase/broadphase_SaP.cpp
+++ b/src/broadphase/broadphase_SaP.cpp
@@ -292,8 +292,8 @@ void SaPCollisionManager::update_(SaPAABB* updated_aabb) {
const AABB current_aabb = current->obj->getAABB();
- const Vec3f& new_min = current_aabb.min_;
- const Vec3f& new_max = current_aabb.max_;
+ const Vec3s& new_min = current_aabb.min_;
+ const Vec3s& new_max = current_aabb.max_;
for (int coord = 0; coord < 3; ++coord) {
int direction; // -1 reverse, 0 nochange, 1 forward
@@ -585,11 +585,11 @@ void SaPCollisionManager::collide(CollisionObject* obj,
bool SaPCollisionManager::distance_(CollisionObject* obj,
DistanceCallBackBase* callback,
CoalScalar& min_dist) const {
- Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
AABB aabb = obj->getAABB();
if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
}
@@ -656,7 +656,7 @@ bool SaPCollisionManager::distance_(CollisionObject* obj,
break;
else {
if (min_dist < old_min_distance) {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb = AABB(obj->getAABB(), min_dist_delta);
status = 0;
} else {
@@ -779,7 +779,7 @@ bool SaPCollisionManager::empty() const { return AABB_arr.empty(); }
size_t SaPCollisionManager::size() const { return AABB_arr.size(); }
//==============================================================================
-const Vec3f& SaPCollisionManager::EndPoint::getVal() const {
+const Vec3s& SaPCollisionManager::EndPoint::getVal() const {
if (minmax)
return aabb->cached.max_;
else
@@ -787,7 +787,7 @@ const Vec3f& SaPCollisionManager::EndPoint::getVal() const {
}
//==============================================================================
-Vec3f& SaPCollisionManager::EndPoint::getVal() {
+Vec3s& SaPCollisionManager::EndPoint::getVal() {
if (minmax)
return aabb->cached.max_;
else
diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp
index 8a431315..5f65e1be 100644
--- a/src/broadphase/broadphase_interval_tree.cpp
+++ b/src/broadphase/broadphase_interval_tree.cpp
@@ -372,10 +372,10 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj,
CoalScalar& min_dist) const {
static const unsigned int CUTOFF = 100;
- Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
AABB aabb = obj->getAABB();
if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
}
@@ -429,7 +429,7 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj,
break;
else {
if (min_dist < old_min_distance) {
- Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb = AABB(obj->getAABB(), min_dist_delta);
status = 0;
} else {
diff --git a/src/broadphase/detail/morton-inl.h b/src/broadphase/detail/morton-inl.h
index 4f27aef9..b646f299 100644
--- a/src/broadphase/detail/morton-inl.h
+++ b/src/broadphase/detail/morton-inl.h
@@ -62,7 +62,7 @@ morton_functor<S, uint32_t>::morton_functor(const AABB& bbox)
//==============================================================================
template <typename S>
-uint32_t morton_functor<S, uint32_t>::operator()(const Vec3f& point) const {
+uint32_t morton_functor<S, uint32_t>::operator()(const Vec3s& point) const {
uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
@@ -82,7 +82,7 @@ morton_functor<S, uint64_t>::morton_functor(const AABB& bbox)
//==============================================================================
template <typename S>
-uint64_t morton_functor<S, uint64_t>::operator()(const Vec3f& point) const {
+uint64_t morton_functor<S, uint64_t>::operator()(const Vec3s& point) const {
uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
@@ -115,7 +115,7 @@ morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox)
//==============================================================================
template <typename S, size_t N>
std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
- const Vec3f& point) const {
+ const Vec3s& point) const {
S x = (point[0] - base[0]) * inv[0];
S y = (point[1] - base[1]) * inv[1];
S z = (point[2] - base[2]) * inv[2];
diff --git a/src/distance/box_halfspace.cpp b/src/distance/box_halfspace.cpp
index f4dbc806..8a4c3a18 100644
--- a/src/distance/box_halfspace.cpp
+++ b/src/distance/box_halfspace.cpp
@@ -51,7 +51,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Box& s1 = static_cast<const Box&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -64,7 +64,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Box& s2 = static_cast<const Box&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/box_plane.cpp b/src/distance/box_plane.cpp
index abce82fb..cdcde09e 100644
--- a/src/distance/box_plane.cpp
+++ b/src/distance/box_plane.cpp
@@ -52,7 +52,7 @@ CoalScalar ShapeShapeDistance<Box, Plane>(const CollisionGeometry* o1,
const CollisionGeometry* o2,
const Transform3f& tf2,
const GJKSolver*, const bool,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+ Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Box& s1 = static_cast<const Box&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -67,7 +67,7 @@ CoalScalar ShapeShapeDistance<Plane, Box>(const CollisionGeometry* o1,
const CollisionGeometry* o2,
const Transform3f& tf2,
const GJKSolver*, const bool,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+ Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<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 aafda9c1..4c249b40 100644
--- a/src/distance/box_sphere.cpp
+++ b/src/distance/box_sphere.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& 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);
@@ -60,7 +60,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Sphere& s1 = static_cast<const Sphere&>(*o1);
const Box& s2 = static_cast<const Box&>(*o2);
const CoalScalar distance =
diff --git a/src/distance/capsule_capsule.cpp b/src/distance/capsule_capsule.cpp
index b09a3522..ad5a2c05 100644
--- a/src/distance/capsule_capsule.cpp
+++ b/src/distance/capsule_capsule.cpp
@@ -59,8 +59,8 @@ CoalScalar clamp(const CoalScalar& num, const CoalScalar& denom) {
}
/// Clamp s=s_n/s_d in [0, 1] and stores a + s * d in a_sd
-void clamped_linear(Vec3f& a_sd, const Vec3f& a, const CoalScalar& s_n,
- const CoalScalar& s_d, const Vec3f& d) {
+void clamped_linear(Vec3s& a_sd, const Vec3s& a, const CoalScalar& s_n,
+ const CoalScalar& s_d, const Vec3s& d) {
assert(s_d >= 0.);
if (s_n <= 0.)
a_sd = a;
@@ -80,15 +80,15 @@ template <>
CoalScalar ShapeShapeDistance<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 bool, Vec3s& wp1, Vec3s& wp2, Vec3s& normal) {
const Capsule* capsule1 = static_cast<const Capsule*>(o1);
const Capsule* capsule2 = static_cast<const Capsule*>(o2);
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();
+ const coal::Vec3s& c1 = tf1.getTranslation();
+ const coal::Vec3s& c2 = tf2.getTranslation();
// We assume that capsules are oriented along z-axis.
CoalScalar halfLength1 = capsule1->halfLength;
CoalScalar halfLength2 = capsule2->halfLength;
@@ -96,14 +96,14 @@ CoalScalar ShapeShapeDistance<Capsule, Capsule>(
CoalScalar radius2 = (capsule2->radius + capsule2->getSweptSphereRadius());
// direction of capsules
// ||d1|| = 2 * halfLength1
- const coal::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2);
- const coal::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2);
+ const coal::Vec3s d1 = 2 * halfLength1 * tf1.getRotation().col(2);
+ const coal::Vec3s d2 = 2 * halfLength2 * tf2.getRotation().col(2);
// Starting point of the segments
// p1 + d1 is the end point of the segment
- const coal::Vec3f p1 = c1 - d1 / 2;
- const coal::Vec3f p2 = c2 - d2 / 2;
- const coal::Vec3f r = p1 - p2;
+ const coal::Vec3s p1 = c1 - d1 / 2;
+ const coal::Vec3s p2 = c2 - d2 / 2;
+ const coal::Vec3s r = p1 - p2;
CoalScalar a = d1.dot(d1);
CoalScalar b = d1.dot(d2);
CoalScalar c = d1.dot(r);
@@ -112,7 +112,7 @@ CoalScalar ShapeShapeDistance<Capsule, Capsule>(
// S1 is parametrized by the equation p1 + s * d1
// S2 is parametrized by the equation p2 + t * d2
- Vec3f w1, w2;
+ Vec3s w1, w2;
if (a <= EPSILON) {
w1 = p1;
if (e <= EPSILON)
diff --git a/src/distance/capsule_halfspace.cpp b/src/distance/capsule_halfspace.cpp
index 3d794ba2..16b42d25 100644
--- a/src/distance/capsule_halfspace.cpp
+++ b/src/distance/capsule_halfspace.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Capsule& s1 = static_cast<const Capsule&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Capsule& s2 = static_cast<const Capsule&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/capsule_plane.cpp b/src/distance/capsule_plane.cpp
index 35c1b8ee..2d587cfb 100644
--- a/src/distance/capsule_plane.cpp
+++ b/src/distance/capsule_plane.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Capsule& s1 = static_cast<const Capsule&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<const Plane&>(*o1);
const Capsule& s2 = static_cast<const Capsule&>(*o2);
return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/cone_halfspace.cpp b/src/distance/cone_halfspace.cpp
index 6a3430d1..10159a98 100644
--- a/src/distance/cone_halfspace.cpp
+++ b/src/distance/cone_halfspace.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Cone& s1 = static_cast<const Cone&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Cone& s2 = static_cast<const Cone&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/cone_plane.cpp b/src/distance/cone_plane.cpp
index 350552fe..9e905bc3 100644
--- a/src/distance/cone_plane.cpp
+++ b/src/distance/cone_plane.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Cone& s1 = static_cast<const Cone&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& 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 55acbfa7..27463259 100644
--- a/src/distance/convex_halfspace.cpp
+++ b/src/distance/convex_halfspace.cpp
@@ -46,7 +46,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const ConvexBase& s1 = static_cast<const ConvexBase&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -59,7 +59,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const ConvexBase& s2 = static_cast<const ConvexBase&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/convex_plane.cpp b/src/distance/convex_plane.cpp
index b0514489..a4907880 100644
--- a/src/distance/convex_plane.cpp
+++ b/src/distance/convex_plane.cpp
@@ -46,7 +46,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const ConvexBase& s1 = static_cast<const ConvexBase&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -59,7 +59,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<const Plane&>(*o1);
const ConvexBase& s2 = static_cast<const ConvexBase&>(*o2);
return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/cylinder_halfspace.cpp b/src/distance/cylinder_halfspace.cpp
index 3c06d8c8..bdff1a2e 100644
--- a/src/distance/cylinder_halfspace.cpp
+++ b/src/distance/cylinder_halfspace.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Cylinder& s1 = static_cast<const Cylinder&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Cylinder& s2 = static_cast<const Cylinder&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/cylinder_plane.cpp b/src/distance/cylinder_plane.cpp
index 125d3d4e..807d3bf9 100644
--- a/src/distance/cylinder_plane.cpp
+++ b/src/distance/cylinder_plane.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Cylinder& s1 = static_cast<const Cylinder&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<const Plane&>(*o1);
const Cylinder& s2 = static_cast<const Cylinder&>(*o2);
return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/ellipsoid_halfspace.cpp b/src/distance/ellipsoid_halfspace.cpp
index 5dc3e8b7..9c2674dc 100644
--- a/src/distance/ellipsoid_halfspace.cpp
+++ b/src/distance/ellipsoid_halfspace.cpp
@@ -48,7 +48,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Ellipsoid& s1 = static_cast<const Ellipsoid&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -61,7 +61,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Ellipsoid& s2 = static_cast<const Ellipsoid&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/ellipsoid_plane.cpp b/src/distance/ellipsoid_plane.cpp
index 16947904..4cc31f3a 100644
--- a/src/distance/ellipsoid_plane.cpp
+++ b/src/distance/ellipsoid_plane.cpp
@@ -47,7 +47,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Ellipsoid& s1 = static_cast<const Ellipsoid&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -60,7 +60,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<const Plane&>(*o1);
const Ellipsoid& s2 = static_cast<const Ellipsoid&>(*o2);
return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/halfspace_halfspace.cpp b/src/distance/halfspace_halfspace.cpp
index 07648d70..e1fa824f 100644
--- a/src/distance/halfspace_halfspace.cpp
+++ b/src/distance/halfspace_halfspace.cpp
@@ -47,7 +47,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
return details::halfspaceHalfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/halfspace_plane.cpp b/src/distance/halfspace_plane.cpp
index abea962c..7fdf4bf9 100644
--- a/src/distance/halfspace_plane.cpp
+++ b/src/distance/halfspace_plane.cpp
@@ -47,7 +47,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
return details::halfspacePlaneDistance(s1, tf1, s2, tf2, p1, p2, normal);
@@ -57,7 +57,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<const Plane&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
CoalScalar distance =
diff --git a/src/distance/plane_plane.cpp b/src/distance/plane_plane.cpp
index 98b23b76..5598e92f 100644
--- a/src/distance/plane_plane.cpp
+++ b/src/distance/plane_plane.cpp
@@ -47,7 +47,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& 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 2724700f..b2b79d7a 100644
--- a/src/distance/sphere_capsule.cpp
+++ b/src/distance/sphere_capsule.cpp
@@ -47,7 +47,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Sphere& s1 = static_cast<const Sphere&>(*o1);
const Capsule& s2 = static_cast<const Capsule&>(*o2);
return details::sphereCapsuleDistance(s1, tf1, s2, tf2, p1, p2, normal);
@@ -57,7 +57,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Capsule& s1 = static_cast<const Capsule&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*o2);
const CoalScalar distance =
diff --git a/src/distance/sphere_cylinder.cpp b/src/distance/sphere_cylinder.cpp
index 93281080..6844c201 100644
--- a/src/distance/sphere_cylinder.cpp
+++ b/src/distance/sphere_cylinder.cpp
@@ -50,7 +50,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Sphere& s1 = static_cast<const Sphere&>(*o1);
const Cylinder& s2 = static_cast<const Cylinder&>(*o2);
return details::sphereCylinderDistance(s1, tf1, s2, tf2, p1, p2, normal);
@@ -60,7 +60,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Cylinder& s1 = static_cast<const Cylinder&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*o2);
const CoalScalar distance =
diff --git a/src/distance/sphere_halfspace.cpp b/src/distance/sphere_halfspace.cpp
index e0fdbc64..63283657 100644
--- a/src/distance/sphere_halfspace.cpp
+++ b/src/distance/sphere_halfspace.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Sphere& s1 = static_cast<const Sphere&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/sphere_plane.cpp b/src/distance/sphere_plane.cpp
index c28df4d3..d423786e 100644
--- a/src/distance/sphere_plane.cpp
+++ b/src/distance/sphere_plane.cpp
@@ -50,7 +50,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Sphere& s1 = static_cast<const Sphere&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -63,7 +63,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<const Plane&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*o2);
return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp
index 365f8af5..6546ab18 100644
--- a/src/distance/sphere_sphere.cpp
+++ b/src/distance/sphere_sphere.cpp
@@ -56,7 +56,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Sphere& s1 = static_cast<const Sphere&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*o2);
return details::sphereSphereDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/triangle_halfspace.cpp b/src/distance/triangle_halfspace.cpp
index f4220b25..8f4b0892 100644
--- a/src/distance/triangle_halfspace.cpp
+++ b/src/distance/triangle_halfspace.cpp
@@ -46,7 +46,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
const Halfspace& s2 = static_cast<const Halfspace&>(*o2);
const CoalScalar distance =
@@ -59,7 +59,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Halfspace& s1 = static_cast<const Halfspace&>(*o1);
const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
return details::halfspaceDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/triangle_plane.cpp b/src/distance/triangle_plane.cpp
index 8e3ba7e2..8db569fb 100644
--- a/src/distance/triangle_plane.cpp
+++ b/src/distance/triangle_plane.cpp
@@ -46,7 +46,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
const Plane& s2 = static_cast<const Plane&>(*o2);
const CoalScalar distance =
@@ -59,7 +59,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Plane& s1 = static_cast<const Plane&>(*o1);
const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
return details::planeDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/triangle_sphere.cpp b/src/distance/triangle_sphere.cpp
index 8e7e1ce7..278e54db 100644
--- a/src/distance/triangle_sphere.cpp
+++ b/src/distance/triangle_sphere.cpp
@@ -46,7 +46,7 @@ template <>
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 bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
const Sphere& s2 = static_cast<const Sphere&>(*o2);
const CoalScalar distance =
@@ -59,7 +59,7 @@ template <>
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) {
+ const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
const Sphere& s1 = static_cast<const Sphere&>(*o1);
const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
return details::sphereTriangleDistance(s1, tf1, s2, tf2, p1, p2, normal);
diff --git a/src/distance/triangle_triangle.cpp b/src/distance/triangle_triangle.cpp
index ae737e6d..9f316664 100644
--- a/src/distance/triangle_triangle.cpp
+++ b/src/distance/triangle_triangle.cpp
@@ -46,7 +46,7 @@ template <>
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) {
+ const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
// Transform the triangles in world frame
const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b),
@@ -64,7 +64,7 @@ CoalScalar ShapeShapeDistance<TriangleP, TriangleP>(
solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance);
// Get GJK initial guess
- Vec3f guess;
+ Vec3s guess;
if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
solver->enable_cached_guess) {
guess = solver->cached_guess;
diff --git a/src/intersect.cpp b/src/intersect.cpp
index feca0843..6440ab5d 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -45,9 +45,9 @@
namespace coal {
-bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2,
- const Vec3f& v3, Vec3f* n, CoalScalar* t) {
- Vec3f n_ = (v2 - v1).cross(v3 - v1);
+bool Intersect::buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
+ const Vec3s& v3, Vec3s* n, CoalScalar* t) {
+ Vec3s n_ = (v2 - v1).cross(v3 - v1);
CoalScalar norm2 = n_.squaredNorm();
if (norm2 > 0) {
*n = n_ / sqrt(norm2);
@@ -57,12 +57,12 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2,
return false;
}
-void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
- const Vec3f& B, Vec3f& VEC, Vec3f& X,
- Vec3f& Y) {
- Vec3f T;
+void TriangleDistance::segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
+ const Vec3s& B, Vec3s& VEC, Vec3s& X,
+ Vec3s& Y) {
+ Vec3s T;
CoalScalar A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
- Vec3f TMP;
+ Vec3s TMP;
T = Q - P;
A_dot_A = A.dot(A);
@@ -153,13 +153,13 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
}
}
-CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
- Vec3f& P, Vec3f& Q) {
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+ Vec3s& P, Vec3s& Q) {
// Compute vectors along the 6 sides
- Vec3f Sv[3];
- Vec3f Tv[3];
- Vec3f VEC;
+ Vec3s Sv[3];
+ Vec3s Tv[3];
+ Vec3s VEC;
Sv[0] = S[1] - S[0];
Sv[1] = S[2] - S[1];
@@ -177,7 +177,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
// Even if these tests fail, it may be helpful to know the closest
// points found, and whether the triangles were shown disjoint
- Vec3f V, Z, minP, minQ;
+ Vec3s V, Z, minP, minQ;
CoalScalar mindd;
int shown_disjoint = 0;
@@ -232,7 +232,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
// First check for case 1
- Vec3f Sn;
+ Vec3s Sn;
CoalScalar Snl;
Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle
@@ -243,7 +243,7 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
if (Snl > 1e-15) {
// Get projection lengths of T points
- Vec3f Tp;
+ Vec3s Tp;
V = S[0] - T[0];
Tp[0] = V.dot(Sn);
@@ -300,14 +300,14 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
}
}
- Vec3f Tn;
+ Vec3s Tn;
CoalScalar Tnl;
Tn = Tv[0].cross(Tv[1]);
Tnl = Tn.dot(Tn);
if (Tnl > 1e-15) {
- Vec3f Sp;
+ Vec3s Sp;
V = T[0] - S[0];
Sp[0] = V.dot(Tn);
@@ -367,12 +367,12 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
return 0;
}
-CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
- const Vec3f& S3, const Vec3f& T1,
- const Vec3f& T2, const Vec3f& T3,
- Vec3f& P, Vec3f& Q) {
- Vec3f S[3];
- Vec3f T[3];
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+ const Vec3s& S3, const Vec3s& T1,
+ const Vec3s& T2, const Vec3s& T3,
+ Vec3s& P, Vec3s& Q) {
+ Vec3s S[3];
+ Vec3s T[3];
S[0] = S1;
S[1] = S2;
S[2] = S3;
@@ -383,10 +383,10 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
return sqrTriDistance(S, T, P, Q);
}
-CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
- const Matrix3f& R, const Vec3f& Tl,
- Vec3f& P, Vec3f& Q) {
- Vec3f T_transformed[3];
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+ const Matrix3s& R, const Vec3s& Tl,
+ Vec3s& P, Vec3s& Q) {
+ Vec3s T_transformed[3];
T_transformed[0] = R * T[0] + Tl;
T_transformed[1] = R * T[1] + Tl;
T_transformed[2] = R * T[2] + Tl;
@@ -394,10 +394,10 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
return sqrTriDistance(S, T_transformed, P, Q);
}
-CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
- const Transform3f& tf, Vec3f& P,
- Vec3f& Q) {
- Vec3f T_transformed[3];
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
+ const Transform3f& tf, Vec3s& P,
+ Vec3s& Q) {
+ Vec3s T_transformed[3];
T_transformed[0] = tf.transform(T[0]);
T_transformed[1] = tf.transform(T[1]);
T_transformed[2] = tf.transform(T[2]);
@@ -405,35 +405,35 @@ CoalScalar TriangleDistance::sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
return sqrTriDistance(S, T_transformed, P, Q);
}
-CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
- const Vec3f& S3, const Vec3f& T1,
- const Vec3f& T2, const Vec3f& T3,
- const Matrix3f& R, const Vec3f& Tl,
- Vec3f& P, Vec3f& Q) {
- Vec3f T1_transformed = R * T1 + Tl;
- Vec3f T2_transformed = R * T2 + Tl;
- Vec3f T3_transformed = R * T3 + Tl;
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+ const Vec3s& S3, const Vec3s& T1,
+ const Vec3s& T2, const Vec3s& T3,
+ const Matrix3s& R, const Vec3s& Tl,
+ Vec3s& P, Vec3s& Q) {
+ Vec3s T1_transformed = R * T1 + Tl;
+ Vec3s T2_transformed = R * T2 + Tl;
+ Vec3s T3_transformed = R * T3 + Tl;
return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
T3_transformed, P, Q);
}
-CoalScalar TriangleDistance::sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
- const Vec3f& S3, const Vec3f& T1,
- const Vec3f& T2, const Vec3f& T3,
- const Transform3f& tf, Vec3f& P,
- Vec3f& Q) {
- Vec3f T1_transformed = tf.transform(T1);
- Vec3f T2_transformed = tf.transform(T2);
- Vec3f T3_transformed = tf.transform(T3);
+CoalScalar TriangleDistance::sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
+ const Vec3s& S3, const Vec3s& T1,
+ const Vec3s& T2, const Vec3s& T3,
+ const Transform3f& tf, Vec3s& P,
+ Vec3s& Q) {
+ Vec3s T1_transformed = tf.transform(T1);
+ Vec3s T2_transformed = tf.transform(T2);
+ Vec3s T3_transformed = tf.transform(T3);
return sqrTriDistance(S1, S2, S3, T1_transformed, T2_transformed,
T3_transformed, P, Q);
}
-Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b,
- const Vec3f& p) {
+Project::ProjectResult Project::projectLine(const Vec3s& a, const Vec3s& b,
+ const Vec3s& p) {
ProjectResult res;
- const Vec3f d = b - a;
+ const Vec3s d = b - a;
const CoalScalar l = d.squaredNorm();
if (l > 0) {
@@ -455,15 +455,15 @@ Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b,
return res;
}
-Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
- const Vec3f& c,
- const Vec3f& p) {
+Project::ProjectResult Project::projectTriangle(const Vec3s& a, const Vec3s& b,
+ const Vec3s& c,
+ const Vec3s& p) {
ProjectResult res;
static const size_t nexti[3] = {1, 2, 0};
- const Vec3f* vt[] = {&a, &b, &c};
- const Vec3f dl[] = {a - b, b - c, c - a};
- const Vec3f& n = dl[0].cross(dl[1]);
+ const Vec3s* vt[] = {&a, &b, &c};
+ const Vec3s dl[] = {a - b, b - c, c - a};
+ const Vec3s& n = dl[0].cross(dl[1]);
const CoalScalar l = n.squaredNorm();
if (l > 0) {
@@ -492,7 +492,7 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
{
CoalScalar d = (a - p).dot(n);
CoalScalar s = sqrt(l);
- Vec3f p_to_project = n * (d / l);
+ Vec3s p_to_project = n * (d / l);
mindist = p_to_project.squaredNorm();
res.encode = 7; // m = 0x111
res.parameterization[0] = dl[1].cross(b - p - p_to_project).norm() / s;
@@ -507,16 +507,16 @@ Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b,
return res;
}
-Project::ProjectResult Project::projectTetrahedra(const Vec3f& a,
- const Vec3f& b,
- const Vec3f& c,
- const Vec3f& d,
- const Vec3f& p) {
+Project::ProjectResult Project::projectTetrahedra(const Vec3s& a,
+ const Vec3s& b,
+ const Vec3s& c,
+ const Vec3s& d,
+ const Vec3s& p) {
ProjectResult res;
static const size_t nexti[] = {1, 2, 0};
- const Vec3f* vt[] = {&a, &b, &c, &d};
- const Vec3f dl[3] = {a - d, b - d, c - d};
+ const Vec3s* vt[] = {&a, &b, &c, &d};
+ const Vec3s dl[3] = {a - d, b - d, c - d};
CoalScalar vl = triple(dl[0], dl[1], dl[2]);
bool ng = (vl * (a - p).dot((b - c).cross(a - b))) <= 0;
if (ng &&
@@ -567,11 +567,11 @@ Project::ProjectResult Project::projectTetrahedra(const Vec3f& a,
return res;
}
-Project::ProjectResult Project::projectLineOrigin(const Vec3f& a,
- const Vec3f& b) {
+Project::ProjectResult Project::projectLineOrigin(const Vec3s& a,
+ const Vec3s& b) {
ProjectResult res;
- const Vec3f d = b - a;
+ const Vec3s d = b - a;
const CoalScalar l = d.squaredNorm();
if (l > 0) {
@@ -593,15 +593,15 @@ Project::ProjectResult Project::projectLineOrigin(const Vec3f& a,
return res;
}
-Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a,
- const Vec3f& b,
- const Vec3f& c) {
+Project::ProjectResult Project::projectTriangleOrigin(const Vec3s& a,
+ const Vec3s& b,
+ const Vec3s& c) {
ProjectResult res;
static const size_t nexti[3] = {1, 2, 0};
- const Vec3f* vt[] = {&a, &b, &c};
- const Vec3f dl[] = {a - b, b - c, c - a};
- const Vec3f& n = dl[0].cross(dl[1]);
+ const Vec3s* vt[] = {&a, &b, &c};
+ const Vec3s dl[] = {a - b, b - c, c - a};
+ const Vec3s& n = dl[0].cross(dl[1]);
const CoalScalar l = n.squaredNorm();
if (l > 0) {
@@ -630,7 +630,7 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a,
{
CoalScalar d = a.dot(n);
CoalScalar s = sqrt(l);
- Vec3f o_to_project = n * (d / l);
+ Vec3s o_to_project = n * (d / l);
mindist = o_to_project.squaredNorm();
res.encode = 7; // m = 0x111
res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s;
@@ -645,15 +645,15 @@ Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a,
return res;
}
-Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a,
- const Vec3f& b,
- const Vec3f& c,
- const Vec3f& d) {
+Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3s& a,
+ const Vec3s& b,
+ const Vec3s& c,
+ const Vec3s& d) {
ProjectResult res;
static const size_t nexti[] = {1, 2, 0};
- const Vec3f* vt[] = {&a, &b, &c, &d};
- const Vec3f dl[3] = {a - d, b - d, c - d};
+ const Vec3s* vt[] = {&a, &b, &c, &d};
+ const Vec3s dl[3] = {a - d, b - d, c - d};
CoalScalar vl = triple(dl[0], dl[1], dl[2]);
bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0;
if (ng &&
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index 5843b03a..844c9c18 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -46,7 +46,7 @@ void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf) {
- Matrix3f R(tf2.getRotation() * tf1.getRotation().transpose());
+ Matrix3s R(tf2.getRotation() * tf1.getRotation().transpose());
tf = Transform3f(R, tf2.getTranslation() - R * tf1.getTranslation());
}
diff --git a/src/mesh_loader/assimp.cpp b/src/mesh_loader/assimp.cpp
index dc947ab0..a0012d76 100644
--- a/src/mesh_loader/assimp.cpp
+++ b/src/mesh_loader/assimp.cpp
@@ -106,7 +106,7 @@ void Loader::load(const std::string& resource_path) {
* @param[in] vertices_offset Current number of vertices in the model
* @param tv Triangles and Vertices of the mesh submodels
*/
-unsigned recurseBuildMesh(const coal::Vec3f& scale, const aiScene* scene,
+unsigned recurseBuildMesh(const coal::Vec3s& scale, const aiScene* scene,
const aiNode* node, unsigned vertices_offset,
TriangleAndVertices& tv) {
if (!node) return 0;
@@ -131,7 +131,7 @@ unsigned recurseBuildMesh(const coal::Vec3f& scale, const aiScene* scene,
aiVector3D p = input_mesh->mVertices[j];
p *= transform;
tv.vertices_.push_back(
- coal::Vec3f(p.x * scale[0], p.y * scale[1], p.z * scale[2]));
+ coal::Vec3s(p.x * scale[0], p.y * scale[1], p.z * scale[2]));
}
// add the indices
@@ -155,7 +155,7 @@ unsigned recurseBuildMesh(const coal::Vec3f& scale, const aiScene* scene,
return nbVertices;
}
-void buildMesh(const coal::Vec3f& scale, const aiScene* scene,
+void buildMesh(const coal::Vec3s& scale, const aiScene* scene,
unsigned vertices_offset, TriangleAndVertices& tv) {
recurseBuildMesh(scale, scene, scene->mRootNode, vertices_offset, tv);
}
diff --git a/src/mesh_loader/loader.cpp b/src/mesh_loader/loader.cpp
index 5ebe872d..9b38398b 100644
--- a/src/mesh_loader/loader.cpp
+++ b/src/mesh_loader/loader.cpp
@@ -58,14 +58,14 @@ bool CachedMeshLoader::Key::operator<(const CachedMeshLoader::Key& b) const {
}
template <typename BV>
-BVHModelPtr_t _load(const std::string& filename, const Vec3f& scale) {
+BVHModelPtr_t _load(const std::string& filename, const Vec3s& scale) {
shared_ptr<BVHModel<BV> > polyhedron(new BVHModel<BV>);
loadPolyhedronFromResource(filename, scale, polyhedron);
return polyhedron;
}
BVHModelPtr_t MeshLoader::load(const std::string& filename,
- const Vec3f& scale) {
+ const Vec3s& scale) {
switch (bvType_) {
case BV_AABB:
return _load<AABB>(filename, scale);
@@ -100,7 +100,7 @@ CollisionGeometryPtr_t MeshLoader::loadOctree(const std::string& filename) {
}
BVHModelPtr_t CachedMeshLoader::load(const std::string& filename,
- const Vec3f& scale) {
+ const Vec3s& scale) {
Key key(filename, scale);
std::time_t mtime = 0;
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index 4cc1601e..b6a8b4ae 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -48,11 +48,11 @@ namespace details {
// segment to to another point. The code is inspired by the explanation
// given by Dan Sunday's page:
// http://geomalgorithms.com/a02-_lines.html
-static inline void lineSegmentPointClosestToPoint(const Vec3f& p,
- const Vec3f& s1,
- const Vec3f& s2, Vec3f& sp) {
- Vec3f v = s2 - s1;
- Vec3f w = p - s1;
+static inline void lineSegmentPointClosestToPoint(const Vec3s& p,
+ const Vec3s& s1,
+ const Vec3s& s2, Vec3s& sp) {
+ Vec3s v = s2 - s1;
+ Vec3s w = p - s1;
CoalScalar c1 = w.dot(v);
CoalScalar c2 = v.dot(v);
@@ -63,7 +63,7 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p,
sp = s2;
} else {
CoalScalar b = c1 / c2;
- Vec3f Pb = s1 + v * b;
+ Vec3s Pb = s1 + v * b;
sp = Pb;
}
}
@@ -75,13 +75,13 @@ static inline void lineSegmentPointClosestToPoint(const Vec3f& p,
inline CoalScalar sphereCapsuleDistance(const Sphere& s1,
const Transform3f& tf1,
const Capsule& s2,
- const Transform3f& tf2, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
- Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength)));
- Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength)));
- Vec3f s_c = tf1.getTranslation();
+ const Transform3f& tf2, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) {
+ Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength)));
+ Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength)));
+ Vec3s s_c = tf1.getTranslation();
- Vec3f segment_point;
+ Vec3s segment_point;
lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point);
normal = segment_point - s_c;
@@ -108,31 +108,31 @@ inline CoalScalar sphereCapsuleDistance(const Sphere& s1,
inline CoalScalar sphereCylinderDistance(const Sphere& s1,
const Transform3f& tf1,
const Cylinder& s2,
- const Transform3f& tf2, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
+ const Transform3f& tf2, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) {
static const CoalScalar eps(sqrt(std::numeric_limits<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)));
+ Vec3s A(tf2.transform(Vec3s(0, 0, -lz2)));
+ Vec3s B(tf2.transform(Vec3s(0, 0, lz2)));
// Position of the center of the sphere
- Vec3f S(tf1.getTranslation());
+ Vec3s S(tf1.getTranslation());
// axis of the cylinder
- Vec3f u(tf2.getRotation().col(2));
+ Vec3s u(tf2.getRotation().col(2));
/// @todo a tiny performance improvement could be achieved using the abscissa
/// with S as the origin
assert((B - A - (s2.halfLength * 2) * u).norm() < eps);
- Vec3f AS(S - A);
+ Vec3s AS(S - A);
// abscissa of S on cylinder axis with A as the origin
CoalScalar s(u.dot(AS));
- Vec3f P(A + s * u);
- Vec3f PS(S - P);
+ Vec3s P(A + s * u);
+ Vec3s PS(S - P);
CoalScalar dPS = PS.norm();
// Normal to cylinder axis such that plane (A, u, v) contains sphere
// center
- Vec3f v(0, 0, 0);
+ Vec3s v(0, 0, 0);
CoalScalar dist;
if (dPS > eps) {
// S is not on cylinder axis
@@ -148,7 +148,7 @@ inline CoalScalar sphereCylinderDistance(const Sphere& s1,
} else {
// closest point on cylinder is on cylinder circle basis
p2 = A + r2 * v;
- Vec3f Sp2(p2 - S);
+ Vec3s Sp2(p2 - S);
CoalScalar dSp2 = Sp2.norm();
if (dSp2 > eps) {
normal = (1 / dSp2) * Sp2;
@@ -181,7 +181,7 @@ inline CoalScalar sphereCylinderDistance(const Sphere& s1,
} else {
// closest point on cylinder is on cylinder circle basis
p2 = B + r2 * v;
- Vec3f Sp2(p2 - S);
+ Vec3s Sp2(p2 - S);
CoalScalar dSp2 = Sp2.norm();
if (dSp2 > eps) {
normal = (1 / dSp2) * Sp2;
@@ -216,15 +216,15 @@ inline CoalScalar sphereCylinderDistance(const Sphere& s1,
/// @return the distance between the two spheres (negative if penetration).
inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
- const coal::Vec3f& center1 = tf1.getTranslation();
- const coal::Vec3f& center2 = tf2.getTranslation();
+ Vec3s& p1, Vec3s& p2, Vec3s& normal) {
+ const coal::Vec3s& center1 = tf1.getTranslation();
+ const coal::Vec3s& center2 = tf2.getTranslation();
CoalScalar r1 = (s1.radius + s1.getSweptSphereRadius());
CoalScalar r2 = (s2.radius + s2.getSweptSphereRadius());
- Vec3f c1c2 = center2 - center1;
+ Vec3s c1c2 = center2 - center1;
CoalScalar cdist = c1c2.norm();
- Vec3f unit(1, 0, 0);
+ Vec3s unit(1, 0, 0);
if (cdist > Eigen::NumTraits<CoalScalar>::epsilon()) unit = c1c2 / cdist;
CoalScalar dist = cdist - r1 - r2;
normal = unit;
@@ -234,10 +234,10 @@ inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
}
/** @brief the minimum distance from a point to a line */
-inline CoalScalar segmentSqrDistance(const Vec3f& from, const Vec3f& to,
- const Vec3f& p, Vec3f& nearest) {
- Vec3f diff = p - from;
- Vec3f v = to - from;
+inline CoalScalar segmentSqrDistance(const Vec3s& from, const Vec3s& to,
+ const Vec3s& p, Vec3s& nearest) {
+ Vec3s diff = p - from;
+ Vec3s v = to - from;
CoalScalar t = v.dot(diff);
if (t > 0) {
@@ -257,19 +257,19 @@ inline CoalScalar segmentSqrDistance(const Vec3f& from, const Vec3f& to,
}
/// @brief Whether a point's projection is in a triangle
-inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
- const Vec3f& normal, const Vec3f& p) {
- Vec3f edge1(p2 - p1);
- Vec3f edge2(p3 - p2);
- Vec3f edge3(p1 - p3);
+inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3,
+ const Vec3s& normal, const Vec3s& p) {
+ Vec3s edge1(p2 - p1);
+ Vec3s edge2(p3 - p2);
+ Vec3s edge3(p1 - p3);
- Vec3f p1_to_p(p - p1);
- Vec3f p2_to_p(p - p2);
- Vec3f p3_to_p(p - p3);
+ Vec3s p1_to_p(p - p1);
+ Vec3s p2_to_p(p - p2);
+ Vec3s p3_to_p(p - p3);
- Vec3f edge1_normal(edge1.cross(normal));
- Vec3f edge2_normal(edge2.cross(normal));
- Vec3f edge3_normal(edge3.cross(normal));
+ Vec3s edge1_normal(edge1.cross(normal));
+ Vec3s edge2_normal(edge2.cross(normal));
+ Vec3s edge3_normal(edge3.cross(normal));
CoalScalar r1, r2, r3;
r1 = edge1_normal.dot(p1_to_p);
@@ -288,15 +288,15 @@ inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
inline CoalScalar sphereTriangleDistance(const Sphere& s,
const Transform3f& tf1,
const TriangleP& tri,
- const Transform3f& tf2, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
- const Vec3f& P1 = tf2.transform(tri.a);
- const Vec3f& P2 = tf2.transform(tri.b);
- const Vec3f& P3 = tf2.transform(tri.c);
+ const Transform3f& tf2, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) {
+ const Vec3s& P1 = tf2.transform(tri.a);
+ const Vec3s& P2 = tf2.transform(tri.b);
+ const Vec3s& P3 = tf2.transform(tri.c);
- Vec3f tri_normal = (P2 - P1).cross(P3 - P1);
+ Vec3s tri_normal = (P2 - P1).cross(P3 - P1);
tri_normal.normalize();
- const Vec3f& center = tf1.getTranslation();
+ const Vec3s& center = tf1.getTranslation();
// Note: comparing an object with a swept-sphere radius of r1 against another
// object with a swept-sphere radius of r2 is equivalent to comparing the
// first object with a swept-sphere radius of r1 + r2 against the second
@@ -305,10 +305,10 @@ inline CoalScalar sphereTriangleDistance(const Sphere& s,
s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius();
assert(radius >= 0);
assert(s.radius >= 0);
- Vec3f p1_to_center = center - P1;
+ Vec3s p1_to_center = center - P1;
CoalScalar distance_from_plane = p1_to_center.dot(tri_normal);
- Vec3f closest_point(
- Vec3f::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
+ Vec3s closest_point(
+ Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
CoalScalar min_distance_sqr, distance_sqr;
if (distance_from_plane < 0) {
@@ -321,7 +321,7 @@ inline CoalScalar sphereTriangleDistance(const Sphere& s,
min_distance_sqr = distance_from_plane * distance_from_plane;
} else {
// Compute distance to each edge and take minimal distance
- Vec3f nearest_on_edge;
+ Vec3s nearest_on_edge;
min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point);
distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
@@ -349,7 +349,7 @@ inline CoalScalar sphereTriangleDistance(const Sphere& s,
/// @return the distance between the two shapes (negative if penetration).
inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
const ShapeBase& s, const Transform3f& tf2,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+ Vec3s& p1, Vec3s& p2, Vec3s& normal) {
// TODO(louis): handle multiple contact points when the halfspace normal is
// parallel to the shape's surface (every primitive except sphere and
// ellipsoid).
@@ -358,7 +358,7 @@ inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
Halfspace new_h = transform(h, tf1);
// Express halfspace normal in shape frame
- Vec3f n_2(tf2.getRotation().transpose() * new_h.n);
+ Vec3s n_2(tf2.getRotation().transpose() * new_h.n);
// Compute support of shape in direction of halfspace normal
int hint = 0;
@@ -383,7 +383,7 @@ inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
/// @return the distance between the two shapes (negative if penetration).
inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1,
const ShapeBase& s, const Transform3f& tf2,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+ Vec3s& p1, Vec3s& p2, Vec3s& normal) {
// TODO(louis): handle multiple contact points when the plane normal is
// parallel to the shape's surface (every primitive except sphere and
// ellipsoid).
@@ -392,17 +392,17 @@ inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1,
std::array<Halfspace, 2> new_h = transformToHalfspaces(plane, tf1);
// Express halfspace normals in shape frame
- Vec3f n_h1(tf2.getRotation().transpose() * new_h[0].n);
- Vec3f n_h2(tf2.getRotation().transpose() * new_h[1].n);
+ Vec3s n_h1(tf2.getRotation().transpose() * new_h[0].n);
+ Vec3s n_h2(tf2.getRotation().transpose() * new_h[1].n);
// Compute support of shape in direction of halfspace normal and its opposite
int hint = 0;
- Vec3f p2h1 =
+ Vec3s p2h1 =
getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h1, hint);
p2h1 = tf2.transform(p2h1);
hint = 0;
- Vec3f p2h2 =
+ Vec3s p2h2 =
getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h2, hint);
p2h2 = tf2.transform(p2h2);
@@ -437,15 +437,15 @@ inline CoalScalar planeDistance(const Plane& plane, const Transform3f& tf1,
/// @return the distance between the two shapes (negative if penetration).
inline CoalScalar boxSphereDistance(const Box& b, const Transform3f& tfb,
const Sphere& s, const Transform3f& tfs,
- Vec3f& pb, Vec3f& ps, Vec3f& normal) {
- const Vec3f& os = tfs.getTranslation();
- const Vec3f& ob = tfb.getTranslation();
- const Matrix3f& Rb = tfb.getRotation();
+ Vec3s& pb, Vec3s& ps, Vec3s& normal) {
+ const Vec3s& os = tfs.getTranslation();
+ const Vec3s& ob = tfb.getTranslation();
+ const Matrix3s& Rb = tfb.getRotation();
pb = ob;
bool outside = false;
- const Vec3f os_in_b_frame(Rb.transpose() * (os - ob));
+ const Vec3s os_in_b_frame(Rb.transpose() * (os - ob));
int axis = -1;
CoalScalar min_d = (std::numeric_limits<CoalScalar>::max)();
for (int i = 0; i < 3; ++i) {
@@ -512,13 +512,13 @@ inline CoalScalar boxSphereDistance(const Box& b, const Transform3f& tfb,
inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1,
const Transform3f& tf1,
const Halfspace& s2,
- const Transform3f& tf2, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
+ const Transform3f& tf2, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) {
Halfspace new_s1 = transform(s1, tf1);
Halfspace new_s2 = transform(s2, tf2);
CoalScalar distance;
- Vec3f dir = (new_s1.n).cross(new_s2.n);
+ Vec3s dir = (new_s1.n).cross(new_s2.n);
CoalScalar dir_sq_norm = dir.squaredNorm();
if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon()) // parallel
@@ -588,13 +588,13 @@ inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1,
inline CoalScalar halfspacePlaneDistance(const Halfspace& s1,
const Transform3f& tf1,
const Plane& s2,
- const Transform3f& tf2, Vec3f& p1,
- Vec3f& p2, Vec3f& normal) {
+ const Transform3f& tf2, Vec3s& p1,
+ Vec3s& p2, Vec3s& normal) {
Halfspace new_s1 = transform(s1, tf1);
Plane new_s2 = transform(s2, tf2);
CoalScalar distance;
- Vec3f dir = (new_s1.n).cross(new_s2.n);
+ Vec3s dir = (new_s1.n).cross(new_s2.n);
CoalScalar dir_sq_norm = dir.squaredNorm();
if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon()) // parallel
@@ -649,12 +649,12 @@ inline CoalScalar halfspacePlaneDistance(const Halfspace& s1,
/// line.
inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
- Vec3f& p1, Vec3f& p2, Vec3f& normal) {
+ Vec3s& p1, Vec3s& p2, Vec3s& normal) {
Plane new_s1 = transform(s1, tf1);
Plane new_s2 = transform(s2, tf2);
CoalScalar distance;
- Vec3f dir = (new_s1.n).cross(new_s2.n);
+ Vec3s dir = (new_s1.n).cross(new_s2.n);
CoalScalar dir_sq_norm = dir.squaredNorm();
if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon()) // parallel
@@ -700,11 +700,11 @@ inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3f& tf1,
}
/// See the prototype below
-inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2,
- const Vec3f& P3, const Vec3f& Q1,
- const Vec3f& Q2, const Vec3f& Q3,
- Vec3f& normal) {
- Vec3f u((P2 - P1).cross(P3 - P1));
+inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2,
+ const Vec3s& P3, const Vec3s& Q1,
+ const Vec3s& Q2, const Vec3s& Q3,
+ Vec3s& normal) {
+ Vec3s u((P2 - P1).cross(P3 - P1));
normal = u.normalized();
CoalScalar depth1((P1 - Q1).dot(normal));
CoalScalar depth2((P1 - Q2).dot(normal));
@@ -719,17 +719,17 @@ inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2,
//
// Note that we compute here an upper bound of the penetration distance,
// not the exact value.
-inline CoalScalar computePenetration(const Vec3f& P1, const Vec3f& P2,
- const Vec3f& P3, const Vec3f& Q1,
- const Vec3f& Q2, const Vec3f& Q3,
+inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2,
+ const Vec3s& P3, const Vec3s& Q1,
+ const Vec3s& Q2, const Vec3s& Q3,
const Transform3f& tf1,
- const Transform3f& tf2, Vec3f& normal) {
- Vec3f globalP1(tf1.transform(P1));
- Vec3f globalP2(tf1.transform(P2));
- Vec3f globalP3(tf1.transform(P3));
- Vec3f globalQ1(tf2.transform(Q1));
- Vec3f globalQ2(tf2.transform(Q2));
- Vec3f globalQ3(tf2.transform(Q3));
+ const Transform3f& tf2, Vec3s& normal) {
+ Vec3s globalP1(tf1.transform(P1));
+ Vec3s globalP2(tf1.transform(P2));
+ Vec3s globalP3(tf1.transform(P3));
+ Vec3s globalQ1(tf2.transform(Q1));
+ Vec3s globalQ2(tf2.transform(Q2));
+ Vec3s globalQ3(tf2.transform(Q3));
return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2,
globalQ3, normal);
}
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index e8093de6..0e81fce2 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -67,7 +67,7 @@ void GJK::reset(size_t max_iterations_, CoalScalar tolerance_) {
iterations_momentum_stop = 0;
}
-Vec3f GJK::getGuessFromSimplex() const { return ray; }
+Vec3s GJK::getGuessFromSimplex() const { return ray; }
namespace details {
@@ -90,7 +90,7 @@ namespace details {
// w0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0
// w1 = alpha * w[0].w1 + (1 - alpha) * w[1].w1
// clang-format on
-void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
+void getClosestPoints(const GJK::Simplex& simplex, Vec3s& w0, Vec3s& w1) {
GJK::SimplexV* const* vs = simplex.vertex;
for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
@@ -104,10 +104,10 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
w1 = vs[0]->w1;
return;
case 2: {
- const Vec3f &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w,
+ const Vec3s &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w,
b0 = vs[1]->w0, b1 = vs[1]->w1;
CoalScalar la, lb;
- Vec3f N(b - a);
+ Vec3s N(b - a);
la = N.dot(-a);
if (la <= 0) {
assert(false);
@@ -155,8 +155,8 @@ void getClosestPoints(const GJK::Simplex& simplex, Vec3f& w0, Vec3f& w1) {
/// The normal should follow coal convention: it points from shape0 to
/// shape1.
template <bool Separated>
-void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0,
- Vec3f& w1) {
+void inflate(const MinkowskiDiff& shape, const Vec3s& normal, Vec3s& w0,
+ Vec3s& w1) {
#ifndef NDEBUG
const CoalScalar dummy_precision =
Eigen::NumTraits<CoalScalar>::dummy_precision();
@@ -173,8 +173,8 @@ void inflate(const MinkowskiDiff& shape, const Vec3f& normal, Vec3f& w0,
} // namespace details
-void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
- Vec3f& w1, Vec3f& normal) const {
+void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+ Vec3s& w1, Vec3s& normal) const {
details::getClosestPoints(*simplex, w0, w1);
if ((w1 - w0).norm() > Eigen::NumTraits<CoalScalar>::dummy_precision()) {
normal = (w1 - w0).normalized();
@@ -184,7 +184,7 @@ void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
details::inflate<true>(shape, normal, w0, w1);
}
-GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
+GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3s& guess,
const support_func_guess_t& supportHint) {
CoalScalar alpha = 0;
iterations = 0;
@@ -206,16 +206,16 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
CoalScalar rl = guess.norm();
if (rl < tolerance) {
- ray = Vec3f(-1, 0, 0);
+ ray = Vec3s(-1, 0, 0);
rl = 1;
} else
ray = guess;
// Momentum
GJKVariant current_gjk_variant = gjk_variant;
- Vec3f w = ray;
- Vec3f dir = ray;
- Vec3f y;
+ Vec3s w = ray;
+ Vec3s dir = ray;
+ Vec3s y;
CoalScalar momentum;
bool normalize_support_direction = shape->normalize_support_direction;
do {
@@ -370,7 +370,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
return status;
}
-bool GJK::checkConvergence(const Vec3f& w, const CoalScalar& rl,
+bool GJK::checkConvergence(const Vec3s& w, const CoalScalar& rl,
CoalScalar& alpha, const CoalScalar& omega) const {
// x^* is the optimal solution (projection of origin onto the Minkowski
// difference).
@@ -429,14 +429,14 @@ inline void GJK::removeVertex(Simplex& simplex) {
free_v[nfree++] = simplex.vertex[--simplex.rank];
}
-inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v,
+inline void GJK::appendVertex(Simplex& simplex, const Vec3s& v,
support_func_guess_t& hint) {
simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
getSupport(v, *simplex.vertex[simplex.rank++], hint);
}
bool GJK::encloseOrigin() {
- Vec3f axis(Vec3f::Zero());
+ Vec3s axis(Vec3s::Zero());
support_func_guess_t hint = support_func_guess_t::Zero();
switch (simplex->rank) {
case 1:
@@ -453,10 +453,10 @@ bool GJK::encloseOrigin() {
}
break;
case 2: {
- Vec3f d = simplex->vertex[1]->w - simplex->vertex[0]->w;
+ Vec3s d = simplex->vertex[1]->w - simplex->vertex[0]->w;
for (int i = 0; i < 3; ++i) {
axis[i] = 1;
- Vec3f p = d.cross(axis);
+ Vec3s p = d.cross(axis);
if (!p.isZero()) {
appendVertex(*simplex, p, hint);
if (encloseOrigin()) return true;
@@ -493,7 +493,7 @@ bool GJK::encloseOrigin() {
}
inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a,
- const Vec3f& A, GJK::Simplex& next, Vec3f& ray) {
+ const Vec3s& A, GJK::Simplex& next, Vec3s& ray) {
// A is the closest to the origin
ray = A;
next.vertex[0] = current.vertex[a];
@@ -501,9 +501,9 @@ inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a,
}
inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a,
- GJK::vertex_id_t b, const Vec3f& A, const Vec3f& B,
- const Vec3f& AB, const CoalScalar& ABdotAO,
- GJK::Simplex& next, Vec3f& ray) {
+ GJK::vertex_id_t b, const Vec3s& A, const Vec3s& B,
+ const Vec3s& AB, const CoalScalar& ABdotAO,
+ GJK::Simplex& next, Vec3s& ray) {
// ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
ray = AB.dot(B) * A + ABdotAO * B;
@@ -517,8 +517,8 @@ inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a,
inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a,
GJK::vertex_id_t b, GJK::vertex_id_t c,
- const Vec3f& ABC, const CoalScalar& ABCdotAO,
- GJK::Simplex& next, Vec3f& ray) {
+ const Vec3s& ABC, const CoalScalar& ABCdotAO,
+ GJK::Simplex& next, Vec3s& ray) {
next.rank = 3;
next.vertex[2] = current.vertex[a];
@@ -544,10 +544,10 @@ inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a,
bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) {
const vertex_id_t a = 1, b = 0;
// A is the last point we added.
- const Vec3f& A = current.vertex[a]->w;
- const Vec3f& B = current.vertex[b]->w;
+ const Vec3s& A = current.vertex[a]->w;
+ const Vec3s& B = current.vertex[b]->w;
- const Vec3f AB = B - A;
+ const Vec3s AB = B - A;
const CoalScalar d = AB.dot(-A);
assert(d <= AB.squaredNorm());
@@ -572,10 +572,10 @@ bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) {
bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) {
const vertex_id_t a = 2, b = 1, c = 0;
// A is the last point we added.
- const Vec3f &A = current.vertex[a]->w, B = current.vertex[b]->w,
+ const Vec3s &A = current.vertex[a]->w, B = current.vertex[b]->w,
C = current.vertex[c]->w;
- const Vec3f AB = B - A, AC = C - A, ABC = AB.cross(AC);
+ const Vec3s AB = B - A, AC = C - A, ABC = AB.cross(AC);
CoalScalar edgeAC2o = ABC.cross(AC).dot(-A);
if (edgeAC2o >= 0) {
@@ -614,10 +614,10 @@ bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) {
bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
// The code of this function was generated using doc/gjk.py
const vertex_id_t a = 3, b = 2, c = 1, d = 0;
- const Vec3f& A(current.vertex[a]->w);
- const Vec3f& B(current.vertex[b]->w);
- const Vec3f& C(current.vertex[c]->w);
- const Vec3f& D(current.vertex[d]->w);
+ const Vec3s& A(current.vertex[a]->w);
+ const Vec3s& B(current.vertex[b]->w);
+ const Vec3s& C(current.vertex[c]->w);
+ const Vec3s& D(current.vertex[d]->w);
const CoalScalar aa = A.squaredNorm();
const CoalScalar da = D.dot(A);
const CoalScalar db = D.dot(B);
@@ -637,8 +637,8 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
const CoalScalar ba_ca = ba - ca;
const CoalScalar ca_da = ca - da;
const CoalScalar da_ba = da - ba;
- const Vec3f a_cross_b = A.cross(B);
- const Vec3f a_cross_c = A.cross(C);
+ const Vec3s a_cross_b = A.cross(B);
+ const Vec3s a_cross_c = A.cross(C);
const CoalScalar dummy_precision(
3 * std::sqrt(std::numeric_limits<CoalScalar>::epsilon()));
@@ -1039,8 +1039,8 @@ void EPA::reset(size_t max_iterations_, CoalScalar tolerance_) {
bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a,
const SimplexVertex& b, CoalScalar& dist) {
- Vec3f ab = b.w - a.w;
- Vec3f n_ab = ab.cross(face->n);
+ Vec3s ab = b.w - a.w;
+ Vec3s n_ab = ab.cross(face->n);
CoalScalar a_dot_nab = a.w.dot(n_ab);
if (a_dot_nab < 0) // the origin is on the outside part of ab
@@ -1154,7 +1154,7 @@ EPA::SimplexFace* EPA::findClosestFace() {
return minf;
}
-EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
+EPA::Status EPA::evaluate(GJK& gjk, const Vec3s& guess) {
GJK::Simplex& simplex = *gjk.getSimplex();
support_hint = gjk.support_hint;
@@ -1309,7 +1309,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
if (nl > 0)
normal /= nl;
else
- normal = Vec3f(1, 0, 0);
+ normal = Vec3s(1, 0, 0);
depth = 0;
result.rank = 1;
result.vertex[0] = simplex.vertex[0];
@@ -1449,8 +1449,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
return false;
}
-void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
- Vec3f& w1, Vec3f& normal) const {
+void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3s& w0,
+ Vec3s& w1, Vec3s& normal) const {
details::getClosestPoints(result, w0, w1);
if ((w0 - w1).norm() > Eigen::NumTraits<CoalScalar>::dummy_precision()) {
if (this->depth >= 0) {
@@ -1477,13 +1477,13 @@ void ConvexBase::buildSupportWarmStart() {
this->support_warm_starts.indices.reserve(
ConvexBase::num_support_warm_starts);
- Vec3f axiis(0, 0, 0);
+ Vec3s axiis(0, 0, 0);
details::ShapeSupportData support_data;
int support_hint = 0;
for (int i = 0; i < 3; ++i) {
axiis(i) = 1;
{
- Vec3f support;
+ Vec3s support;
coal::details::getShapeSupport<false>(this, axiis, support, support_hint,
support_data);
this->support_warm_starts.points.emplace_back(support);
@@ -1492,7 +1492,7 @@ void ConvexBase::buildSupportWarmStart() {
axiis(i) = -1;
{
- Vec3f support;
+ Vec3s support;
coal::details::getShapeSupport<false>(this, axiis, support, support_hint,
support_data);
this->support_warm_starts.points.emplace_back(support);
@@ -1502,14 +1502,14 @@ void ConvexBase::buildSupportWarmStart() {
axiis(i) = 0;
}
- std::array<Vec3f, 4> eis = {Vec3f(1, 1, 1), //
- Vec3f(-1, 1, 1), //
- Vec3f(-1, -1, 1), //
- Vec3f(1, -1, 1)};
+ std::array<Vec3s, 4> eis = {Vec3s(1, 1, 1), //
+ Vec3s(-1, 1, 1), //
+ Vec3s(-1, -1, 1), //
+ Vec3s(1, -1, 1)};
for (size_t ei_index = 0; ei_index < 4; ++ei_index) {
{
- Vec3f support;
+ Vec3s support;
coal::details::getShapeSupport<false>(this, eis[ei_index], support,
support_hint, support_data);
this->support_warm_starts.points.emplace_back(support);
@@ -1517,7 +1517,7 @@ void ConvexBase::buildSupportWarmStart() {
}
{
- Vec3f support;
+ Vec3s support;
coal::details::getShapeSupport<false>(this, -eis[ei_index], support,
support_hint, support_data);
this->support_warm_starts.points.emplace_back(support);
diff --git a/src/narrowphase/minkowski_difference.cpp b/src/narrowphase/minkowski_difference.cpp
index 73307e12..baa3f4dc 100644
--- a/src/narrowphase/minkowski_difference.cpp
+++ b/src/narrowphase/minkowski_difference.cpp
@@ -45,9 +45,9 @@ namespace details {
// ============================================================================
template <typename Shape0, typename Shape1, bool TransformIsIdentity,
int _SupportOptions>
-void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1,
- const Vec3f& ot1, const Vec3f& dir, Vec3f& support0,
- Vec3f& support1, support_func_guess_t& hint,
+void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3s& oR1,
+ const Vec3s& ot1, const Vec3s& dir, Vec3s& support0,
+ Vec3s& support1, support_func_guess_t& hint,
ShapeSupportData data[2]) {
assert(dir.norm() > Eigen::NumTraits<CoalScalar>::epsilon());
getShapeSupport<_SupportOptions>(s0, dir, support0, hint[0], data[0]);
@@ -64,8 +64,8 @@ void getSupportTpl(const Shape0* s0, const Shape1* s1, const Matrix3f& oR1,
// ============================================================================
template <typename Shape0, typename Shape1, bool TransformIsIdentity,
int _SupportOptions>
-void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3f& dir,
- Vec3f& support0, Vec3f& support1,
+void getSupportFuncTpl(const MinkowskiDiff& md, const Vec3s& dir,
+ Vec3s& support0, Vec3s& support1,
support_func_guess_t& hint, ShapeSupportData data[2]) {
getSupportTpl<Shape0, Shape1, TransformIsIdentity, _SupportOptions>(
static_cast<const Shape0*>(md.shapes[0]),
@@ -310,13 +310,13 @@ template void COAL_DLLAPI MinkowskiDiff::set<SupportOptions::WithSweptSphere>(co
// ============================================================================
// clang-format off
-template Vec3f COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
-template Vec3f COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support0<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
-template Vec3f COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::NoSweptSphere>(const Vec3s&, int&) const;
-template Vec3f COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3f&, int&) const;
+template Vec3s COAL_DLLAPI MinkowskiDiff::support1<SupportOptions::WithSweptSphere>(const Vec3s&, int&) const;
// clang-format on
} // namespace details
diff --git a/src/narrowphase/support_functions.cpp b/src/narrowphase/support_functions.cpp
index 9c7563f4..64be0e0c 100644
--- a/src/narrowphase/support_functions.cpp
+++ b/src/narrowphase/support_functions.cpp
@@ -48,8 +48,8 @@ namespace details {
getShapeSupport<_SupportOptions>(static_cast<const ShapeType*>(shape), dir, \
support, hint, support_data)
template <int _SupportOptions>
-Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) {
- Vec3f support;
+Vec3s getSupport(const ShapeBase* shape, const Vec3s& dir, int& hint) {
+ Vec3s support;
ShapeSupportData support_data;
switch (shape->getNodeType()) {
case GEOM_TRIANGLE:
@@ -89,29 +89,29 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, int& hint) {
// Explicit instantiation
// clang-format off
-template COAL_DLLAPI Vec3f getSupport<SupportOptions::NoSweptSphere>(const ShapeBase*, const Vec3f&, int&);
+template COAL_DLLAPI Vec3s getSupport<SupportOptions::NoSweptSphere>(const ShapeBase*, const Vec3s&, int&);
-template COAL_DLLAPI Vec3f getSupport<SupportOptions::WithSweptSphere>(const ShapeBase*, const Vec3f&, int&);
+template COAL_DLLAPI Vec3s getSupport<SupportOptions::WithSweptSphere>(const ShapeBase*, const Vec3s&, int&);
// clang-format on
// ============================================================================
#define getShapeSupportTplInstantiation(ShapeType) \
template COAL_DLLAPI void getShapeSupport<SupportOptions::NoSweptSphere>( \
- const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \
+ const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \
ShapeSupportData& support_data); \
\
template COAL_DLLAPI void getShapeSupport<SupportOptions::WithSweptSphere>( \
- const ShapeType* shape_, const Vec3f& dir, Vec3f& support, int& hint, \
+ const ShapeType* shape_, const Vec3s& dir, Vec3s& support, int& hint, \
ShapeSupportData& support_data);
// ============================================================================
template <int _SupportOptions>
-void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
+void getShapeSupport(const TriangleP* triangle, const Vec3s& dir,
+ Vec3s& support, int& /*unused*/,
ShapeSupportData& /*unused*/) {
- FCL_REAL dota = dir.dot(triangle->a);
- FCL_REAL dotb = dir.dot(triangle->b);
- FCL_REAL dotc = dir.dot(triangle->c);
+ CoalScalar dota = dir.dot(triangle->a);
+ CoalScalar dotb = dir.dot(triangle->b);
+ CoalScalar dotc = dir.dot(triangle->c);
if (dota > dotb) {
if (dotc > dota) {
support = triangle->c;
@@ -134,15 +134,15 @@ getShapeSupportTplInstantiation(TriangleP);
// ============================================================================
template <int _SupportOptions>
-inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support,
+inline void getShapeSupport(const Box* box, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/) {
// The inflate value is simply to make the specialized functions with box
// have a preferred side for edge cases.
- static const FCL_REAL inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.;
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
- Vec3f support1 = (dir.array() > dummy_precision).select(box->halfSide, 0);
- Vec3f support2 =
+ static const CoalScalar inflate = (dir.array() == 0).any() ? 1 + 1e-10 : 1.;
+ static const CoalScalar dummy_precision =
+ Eigen::NumTraits<CoalScalar>::dummy_precision();
+ Vec3s support1 = (dir.array() > dummy_precision).select(box->halfSide, 0);
+ Vec3s support2 =
(dir.array() < -dummy_precision).select(-inflate * box->halfSide, 0);
support.noalias() = support1 + support2;
@@ -154,8 +154,8 @@ getShapeSupportTplInstantiation(Box);
// ============================================================================
template <int _SupportOptions>
-inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
+inline void getShapeSupport(const Sphere* sphere, const Vec3s& dir,
+ Vec3s& support, int& /*unused*/,
ShapeSupportData& /*unused*/) {
if (_SupportOptions == SupportOptions::WithSweptSphere) {
support.noalias() =
@@ -164,23 +164,23 @@ inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir,
support.setZero();
}
- HPP_FCL_UNUSED_VARIABLE(sphere);
- HPP_FCL_UNUSED_VARIABLE(dir);
+ COAL_UNUSED_VARIABLE(sphere);
+ COAL_UNUSED_VARIABLE(dir);
}
getShapeSupportTplInstantiation(Sphere);
// ============================================================================
template <int _SupportOptions>
-inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
+inline void getShapeSupport(const Ellipsoid* ellipsoid, const Vec3s& dir,
+ Vec3s& support, int& /*unused*/,
ShapeSupportData& /*unused*/) {
- FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
- FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
- FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
+ CoalScalar a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
+ CoalScalar b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
+ CoalScalar c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
- Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
+ Vec3s v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
- FCL_REAL d = std::sqrt(v.dot(dir));
+ CoalScalar d = std::sqrt(v.dot(dir));
support = v / d;
@@ -192,11 +192,11 @@ getShapeSupportTplInstantiation(Ellipsoid);
// ============================================================================
template <int _SupportOptions>
-inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir,
- Vec3f& support, int& /*unused*/,
+inline void getShapeSupport(const Capsule* capsule, const Vec3s& dir,
+ Vec3s& support, int& /*unused*/,
ShapeSupportData& /*unused*/) {
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
+ static const CoalScalar dummy_precision =
+ Eigen::NumTraits<CoalScalar>::dummy_precision();
support.setZero();
if (dir[2] > dummy_precision) {
support[2] = capsule->halfLength;
@@ -213,17 +213,17 @@ getShapeSupportTplInstantiation(Capsule);
// ============================================================================
template <int _SupportOptions>
-void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const Cone* cone, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/) {
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
+ static const CoalScalar dummy_precision =
+ Eigen::NumTraits<CoalScalar>::dummy_precision();
// The cone radius is, for -h < z < h, (h - z) * r / (2*h)
// The inflate value is simply to make the specialized functions with cone
// have a preferred side for edge cases.
- static const FCL_REAL inflate = 1 + 1e-10;
- FCL_REAL h = cone->halfLength;
- FCL_REAL r = cone->radius;
+ static const CoalScalar inflate = 1 + 1e-10;
+ CoalScalar h = cone->halfLength;
+ CoalScalar r = cone->radius;
if (dir.head<2>().isZero(dummy_precision)) {
support.head<2>().setZero();
@@ -233,22 +233,22 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support,
support[2] = -inflate * h;
}
} else {
- FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
- FCL_REAL len = zdist + dir[2] * dir[2];
+ CoalScalar zdist = dir[0] * dir[0] + dir[1] * dir[1];
+ CoalScalar len = zdist + dir[2] * dir[2];
zdist = std::sqrt(zdist);
if (dir[2] <= 0) {
- FCL_REAL rad = r / zdist;
+ CoalScalar rad = r / zdist;
support.head<2>() = rad * dir.head<2>();
support[2] = -h;
} else {
len = std::sqrt(len);
- FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h);
+ CoalScalar sin_a = r / std::sqrt(r * r + 4 * h * h);
if (dir[2] > len * sin_a)
support << 0, 0, h;
else {
- FCL_REAL rad = r / zdist;
+ CoalScalar rad = r / zdist;
support.head<2>() = rad * dir.head<2>();
support[2] = -h;
}
@@ -263,16 +263,16 @@ getShapeSupportTplInstantiation(Cone);
// ============================================================================
template <int _SupportOptions>
-void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const Cylinder* cylinder, const Vec3s& dir, Vec3s& support,
int& /*unused*/, ShapeSupportData& /*unused*/) {
- static const FCL_REAL dummy_precision =
- Eigen::NumTraits<FCL_REAL>::dummy_precision();
+ static const CoalScalar dummy_precision =
+ Eigen::NumTraits<CoalScalar>::dummy_precision();
// The inflate value is simply to make the specialized functions with cylinder
// have a preferred side for edge cases.
- static const FCL_REAL inflate = 1 + 1e-10;
- FCL_REAL half_h = cylinder->halfLength;
- FCL_REAL r = cylinder->radius;
+ static const CoalScalar inflate = 1 + 1e-10;
+ CoalScalar half_h = cylinder->halfLength;
+ CoalScalar r = cylinder->radius;
const bool dir_is_aligned_with_z = dir.head<2>().isZero(dummy_precision);
if (dir_is_aligned_with_z) half_h *= inflate;
@@ -293,7 +293,7 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support,
}
assert(fabs(support[0] * dir[1] - support[1] * dir[0]) <
- sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
+ sqrt(std::numeric_limits<CoalScalar>::epsilon()));
if (_SupportOptions == SupportOptions::WithSweptSphere) {
support += cylinder->getSweptSphereRadius() * dir.normalized();
@@ -303,23 +303,23 @@ getShapeSupportTplInstantiation(Cylinder);
// ============================================================================
template <int _SupportOptions>
-void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
- Vec3f& support, int& hint,
+void getShapeSupportLog(const ConvexBase* convex, const Vec3s& dir,
+ Vec3s& support, int& hint,
ShapeSupportData& support_data) {
assert(convex->neighbors != nullptr && "Convex has no neighbors.");
// Use warm start if current support direction is distant from last support
// direction.
const double use_warm_start_threshold = 0.9;
- Vec3f dir_normalized = dir.normalized();
+ Vec3s dir_normalized = dir.normalized();
if (!support_data.last_dir.isZero() &&
!convex->support_warm_starts.points.empty() &&
support_data.last_dir.dot(dir_normalized) < use_warm_start_threshold) {
// Change hint if last dir is too far from current dir.
- FCL_REAL maxdot = convex->support_warm_starts.points[0].dot(dir);
+ CoalScalar maxdot = convex->support_warm_starts.points[0].dot(dir);
hint = convex->support_warm_starts.indices[0];
for (size_t i = 1; i < convex->support_warm_starts.points.size(); ++i) {
- FCL_REAL dot = convex->support_warm_starts.points[i].dot(dir);
+ CoalScalar dot = convex->support_warm_starts.points[i].dot(dir);
if (dot > maxdot) {
maxdot = dot;
hint = convex->support_warm_starts.indices[i];
@@ -328,13 +328,13 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
}
support_data.last_dir = dir_normalized;
- const std::vector<Vec3f>& pts = *(convex->points);
+ const std::vector<Vec3s>& pts = *(convex->points);
const std::vector<ConvexBase::Neighbors>& nn = *(convex->neighbors);
if (hint < 0 || hint >= (int)convex->num_points) {
hint = 0;
}
- FCL_REAL maxdot = pts[static_cast<size_t>(hint)].dot(dir);
+ CoalScalar maxdot = pts[static_cast<size_t>(hint)].dot(dir);
std::vector<int8_t>& visited = support_data.visited;
if (support_data.visited.size() == convex->num_points) {
std::fill(visited.begin(), visited.end(), false);
@@ -356,7 +356,7 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
const unsigned int ip = n[in];
if (visited[ip]) continue;
visited[ip] = true;
- const FCL_REAL dot = pts[ip].dot(dir);
+ const CoalScalar dot = pts[ip].dot(dir);
bool better = false;
if (dot > maxdot) {
better = true;
@@ -380,15 +380,15 @@ void getShapeSupportLog(const ConvexBase* convex, const Vec3f& dir,
// ============================================================================
template <int _SupportOptions>
-void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
- Vec3f& support, int& hint,
+void getShapeSupportLinear(const ConvexBase* convex, const Vec3s& dir,
+ Vec3s& support, int& hint,
ShapeSupportData& /*unused*/) {
- const std::vector<Vec3f>& pts = *(convex->points);
+ const std::vector<Vec3s>& pts = *(convex->points);
hint = 0;
- FCL_REAL maxdot = pts[0].dot(dir);
+ CoalScalar maxdot = pts[0].dot(dir);
for (int i = 1; i < (int)convex->num_points; ++i) {
- FCL_REAL dot = pts[static_cast<size_t>(i)].dot(dir);
+ CoalScalar dot = pts[static_cast<size_t>(i)].dot(dir);
if (dot > maxdot) {
maxdot = dot;
hint = i;
@@ -404,7 +404,7 @@ void getShapeSupportLinear(const ConvexBase* convex, const Vec3f& dir,
// ============================================================================
template <int _SupportOptions>
-void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
+void getShapeSupport(const ConvexBase* convex, const Vec3s& dir, Vec3s& support,
int& hint, ShapeSupportData& support_data) {
// TODO add benchmark to set a proper value for switching between linear and
// logarithmic.
@@ -417,15 +417,13 @@ void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support,
support_data);
}
}
-// clang-format off
-getShapeSupportTplInstantiation(ConvexBase)
- // clang-format on
-
- // ============================================================================
- template <int _SupportOptions>
- inline void getShapeSupport(const SmallConvex* convex, const Vec3f& dir,
- Vec3f& support, int& hint,
- ShapeSupportData& support_data) {
+getShapeSupportTplInstantiation(ConvexBase);
+
+// ============================================================================
+template <int _SupportOptions>
+inline void getShapeSupport(const SmallConvex* convex, const Vec3s& dir,
+ Vec3s& support, int& hint,
+ ShapeSupportData& support_data) {
getShapeSupportLinear<_SupportOptions>(
reinterpret_cast<const ConvexBase*>(convex), dir, support, hint,
support_data);
@@ -434,8 +432,8 @@ getShapeSupportTplInstantiation(SmallConvex);
// ============================================================================
template <int _SupportOptions>
-inline void getShapeSupport(const LargeConvex* convex, const Vec3f& dir,
- Vec3f& support, int& hint,
+inline void getShapeSupport(const LargeConvex* convex, const Vec3s& dir,
+ Vec3s& support, int& hint,
ShapeSupportData& support_data) {
getShapeSupportLog<_SupportOptions>(
reinterpret_cast<const ConvexBase*>(convex), dir, support, hint,
@@ -450,7 +448,7 @@ getShapeSupportTplInstantiation(LargeConvex);
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) {
+ size_t max_num_supports, CoalScalar tol) {
ShapeSupportData support_data;
switch (shape->getNodeType()) {
case GEOM_TRIANGLE:
@@ -486,38 +484,38 @@ void getSupportSet(const ShapeBase* shape, SupportSet& support_set, int& hint,
// Explicit instantiation
// clang-format off
-template COAL_DLLAPI void getSupportSet<SupportOptions::NoSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL);
+template COAL_DLLAPI void getSupportSet<SupportOptions::NoSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar);
-template COAL_DLLAPI void getSupportSet<SupportOptions::WithSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, FCL_REAL);
+template COAL_DLLAPI void getSupportSet<SupportOptions::WithSweptSphere>(const ShapeBase*, SupportSet&, int&, size_t, CoalScalar);
// clang-format on
// ============================================================================
#define getShapeSupportSetTplInstantiation(ShapeType) \
template COAL_DLLAPI void getShapeSupportSet<SupportOptions::NoSweptSphere>( \
const ShapeType* shape_, SupportSet& support_set, int& hint, \
- ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol); \
+ ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol); \
\
template COAL_DLLAPI void \
getShapeSupportSet<SupportOptions::WithSweptSphere>( \
const ShapeType* shape_, SupportSet& support_set, int& hint, \
- ShapeSupportData& data, size_t num_sampled_supports, FCL_REAL tol);
+ ShapeSupportData& data, size_t num_sampled_supports, CoalScalar tol);
// ============================================================================
template <int _SupportOptions>
void getShapeSupportSet(const TriangleP* triangle, SupportSet& support_set,
int& hint /*unused*/,
ShapeSupportData& support_data /*unused*/,
- size_t /*unused*/, FCL_REAL tol) {
+ size_t /*unused*/, CoalScalar tol) {
assert(tol > 0);
support_set.clear();
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
// We simply want to compute the support value, no need to take the
// swept-sphere radius into account.
getShapeSupport<SupportOptions::NoSweptSphere>(triangle, support_dir, support,
hint, support_data);
- const FCL_REAL support_value = support.dot(support_dir);
+ const CoalScalar support_value = support.dot(support_dir);
if (support_value - support_dir.dot(triangle->a) < tol) {
// Note: at the moment, it's useless to take into account the
@@ -553,36 +551,36 @@ getShapeSupportSetTplInstantiation(TriangleP);
template <int _SupportOptions>
void getShapeSupportSet(const Box* box, SupportSet& support_set,
int& hint /*unused*/, ShapeSupportData& support_data,
- size_t /*unused*/, FCL_REAL tol) {
+ size_t /*unused*/, CoalScalar tol) {
assert(tol > 0);
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupport<SupportOptions::NoSweptSphere>(box, support_dir, support,
hint, support_data);
- const FCL_REAL support_value = support.dot(support_dir);
-
- const FCL_REAL x = box->halfSide[0];
- const FCL_REAL y = box->halfSide[1];
- const FCL_REAL z = box->halfSide[2];
- const std::array<Vec3f, 8> corners = {
- Vec3f(x, y, z), Vec3f(-x, y, z), Vec3f(-x, -y, z), Vec3f(x, -y, z),
- Vec3f(x, y, -z), Vec3f(-x, y, -z), Vec3f(-x, -y, -z), Vec3f(x, -y, -z),
+ const CoalScalar support_value = support.dot(support_dir);
+
+ const CoalScalar x = box->halfSide[0];
+ const CoalScalar y = box->halfSide[1];
+ const CoalScalar z = box->halfSide[2];
+ const std::array<Vec3s, 8> corners = {
+ Vec3s(x, y, z), Vec3s(-x, y, z), Vec3s(-x, -y, z), Vec3s(x, -y, z),
+ Vec3s(x, y, -z), Vec3s(-x, y, -z), Vec3s(-x, -y, -z), Vec3s(x, -y, -z),
};
SupportSet::Polygon& polygon = support_data.polygon;
polygon.clear();
const Transform3f& tf = support_set.tf;
- for (const Vec3f& corner : corners) {
- const FCL_REAL val = corner.dot(support_dir);
+ for (const Vec3s& corner : corners) {
+ const CoalScalar val = corner.dot(support_dir);
if (support_value - val < tol) {
if (_SupportOptions == SupportOptions::WithSweptSphere) {
- const Vec2f p =
+ const Vec2s p =
tf.inverseTransform(corner +
box->getSweptSphereRadius() * support_dir)
.template head<2>();
polygon.emplace_back(p);
} else {
- const Vec2f p = tf.inverseTransform(corner).template head<2>();
+ const Vec2s p = tf.inverseTransform(corner).template head<2>();
polygon.emplace_back(p);
}
}
@@ -596,11 +594,11 @@ template <int _SupportOptions>
void getShapeSupportSet(const Sphere* sphere, SupportSet& support_set,
int& hint /*unused*/,
ShapeSupportData& support_data /*unused*/,
- size_t /*unused*/, FCL_REAL /*unused*/) {
+ size_t /*unused*/, CoalScalar /*unused*/) {
support_set.points().clear();
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupport<_SupportOptions>(sphere, support_dir, support, hint,
support_data);
support_set.addPoint(support);
@@ -611,11 +609,11 @@ getShapeSupportSetTplInstantiation(Sphere);
template <int _SupportOptions>
void getShapeSupportSet(const Ellipsoid* ellipsoid, SupportSet& support_set,
int& hint, ShapeSupportData& support_data /*unused*/,
- size_t /*unused*/, FCL_REAL /*unused*/) {
+ size_t /*unused*/, CoalScalar /*unused*/) {
support_set.points().clear();
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupport<_SupportOptions>(ellipsoid, support_dir, support, hint,
support_data);
support_set.addPoint(support);
@@ -627,28 +625,28 @@ template <int _SupportOptions>
void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
int& hint /*unused*/,
ShapeSupportData& support_data /*unused*/,
- size_t /*unused*/, FCL_REAL tol) {
+ size_t /*unused*/, CoalScalar tol) {
// clang-format on
assert(tol > 0);
support_set.points().clear();
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupport<SupportOptions::NoSweptSphere>(capsule, support_dir, support,
hint, support_data);
- const FCL_REAL support_value =
+ const CoalScalar support_value =
support_dir.dot(support + capsule->radius * support_dir);
// The support set of a capsule has either 2 points or 1 point.
// The two candidate points lie at the frontier between the cylinder and
// sphere parts of the capsule.
- const FCL_REAL h = capsule->halfLength;
- const FCL_REAL r = capsule->radius;
- const Vec3f p1(r * support_dir[0], r * support_dir[1], h);
- const Vec3f p2(r * support_dir[0], r * support_dir[1], -h);
+ const CoalScalar h = capsule->halfLength;
+ const CoalScalar r = capsule->radius;
+ const Vec3s p1(r * support_dir[0], r * support_dir[1], h);
+ const Vec3s p2(r * support_dir[0], r * support_dir[1], -h);
if ((support_value - support_dir.dot(p1) <= tol) &&
(support_value - support_dir.dot(p2) <= tol)) {
if (_SupportOptions == SupportOptions::WithSweptSphere) {
- const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius();
+ const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius();
support_set.addPoint(p1 + ssr_vec);
support_set.addPoint(p2 + ssr_vec);
} else {
@@ -657,7 +655,7 @@ void getShapeSupportSet(const Capsule* capsule, SupportSet& support_set,
}
} else {
if (_SupportOptions == SupportOptions::WithSweptSphere) {
- const Vec3f ssr_vec = support_dir * capsule->getSweptSphereRadius();
+ const Vec3s ssr_vec = support_dir * capsule->getSweptSphereRadius();
support_set.addPoint(support + ssr_vec);
} else {
support_set.addPoint(support);
@@ -671,29 +669,29 @@ template <int _SupportOptions>
void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
int& hint /*unused*/,
ShapeSupportData& support_data /*unused*/,
- size_t num_sampled_supports, FCL_REAL tol) {
+ size_t num_sampled_supports, CoalScalar tol) {
assert(tol > 0);
support_set.points().clear();
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupport<SupportOptions::NoSweptSphere>(cone, support_dir, support,
hint, support_data);
- const FCL_REAL support_value = support.dot(support_dir);
+ const CoalScalar support_value = support.dot(support_dir);
// If the support direction is perpendicular to the cone's basis, there is an
// infinite amount of support points; otherwise there are up to two support
// points (two if direction is perpendicular to the side of the cone and one
// otherwise).
//
- // To check this condition, we look at two points on the cone's basis; these
- // two points are symmetrical w.r.t the center of the circle. If both these
- // points are tol away from the support plane, then all the points of the
- // circle are tol away from the support plane.
- const FCL_REAL r = cone->radius;
- const FCL_REAL z = -cone->halfLength;
- const Vec3f p1(r * support_dir[0], r * support_dir[1], z);
- const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z);
+ // To check this condition, we look at two points on the cone's basis;
+ // these two points are symmetrical w.r.t the center of the circle. If
+ // both these points are tol away from the support plane, then all the
+ // points of the circle are tol away from the support plane.
+ const CoalScalar r = cone->radius;
+ const CoalScalar z = -cone->halfLength;
+ const Vec3s p1(r * support_dir[0], r * support_dir[1], z);
+ const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z);
if ((support_value - support_dir.dot(p1) <= tol) &&
(support_value - support_dir.dot(p2) <= tol)) {
@@ -701,11 +699,11 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
// the basis of the cone. We sample `num_sampled_supports` points on the
// base of the cone. We are guaranteed that these points like at a distance
// tol of the support plane.
- const FCL_REAL angle_increment =
- 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports));
+ const CoalScalar angle_increment =
+ 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports));
for (size_t i = 0; i < num_sampled_supports; ++i) {
- const FCL_REAL theta = (FCL_REAL)(i)*angle_increment;
- Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
+ const CoalScalar theta = (CoalScalar)(i)*angle_increment;
+ Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol);
if (_SupportOptions == SupportOptions::WithSweptSphere) {
point_on_cone_base += cone->getSweptSphereRadius() * support_dir;
@@ -716,7 +714,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
// There are two potential supports to add: the tip of the cone and a point
// on the basis of the cone. We compare each of these points to the support
// value.
- Vec3f cone_tip(0, 0, cone->halfLength);
+ Vec3s cone_tip(0, 0, cone->halfLength);
if (support_value - support_dir.dot(cone_tip) <= tol) {
if (_SupportOptions == SupportOptions::WithSweptSphere) {
cone_tip += cone->getSweptSphereRadius() * support_dir;
@@ -724,7 +722,7 @@ void getShapeSupportSet(const Cone* cone, SupportSet& support_set,
support_set.addPoint(cone_tip);
}
- Vec3f point_on_cone_base = Vec3f(cone->radius * support_dir[0], //
+ Vec3s point_on_cone_base = Vec3s(cone->radius * support_dir[0], //
cone->radius * support_dir[1], //
z);
if (support_value - support_dir.dot(point_on_cone_base) <= tol) {
@@ -742,31 +740,31 @@ template <int _SupportOptions>
void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
int& hint /*unused*/,
ShapeSupportData& support_data /*unused*/,
- size_t num_sampled_supports, FCL_REAL tol) {
+ size_t num_sampled_supports, CoalScalar tol) {
assert(tol > 0);
support_set.points().clear();
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupport<SupportOptions::NoSweptSphere>(cylinder, support_dir, support,
hint, support_data);
- const FCL_REAL support_value = support.dot(support_dir);
+ const CoalScalar support_value = support.dot(support_dir);
// The following is very similar to what is done for Cone's support set
// computation.
- const FCL_REAL r = cylinder->radius;
- const FCL_REAL z =
+ const CoalScalar r = cylinder->radius;
+ const CoalScalar z =
support_dir[2] <= 0 ? -cylinder->halfLength : cylinder->halfLength;
- const Vec3f p1(r * support_dir[0], r * support_dir[1], z);
- const Vec3f p2(-r * support_dir[0], -r * support_dir[1], z);
+ const Vec3s p1(r * support_dir[0], r * support_dir[1], z);
+ const Vec3s p2(-r * support_dir[0], -r * support_dir[1], z);
if ((support_value - support_dir.dot(p1) <= tol) &&
(support_value - support_dir.dot(p2) <= tol)) {
- const FCL_REAL angle_increment =
- 2.0 * (FCL_REAL)(EIGEN_PI) / ((FCL_REAL)(num_sampled_supports));
+ const CoalScalar angle_increment =
+ 2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(num_sampled_supports));
for (size_t i = 0; i < num_sampled_supports; ++i) {
- const FCL_REAL theta = (FCL_REAL)(i)*angle_increment;
- Vec3f point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
+ const CoalScalar theta = (CoalScalar)(i)*angle_increment;
+ Vec3s point_on_cone_base(r * std::cos(theta), r * std::sin(theta), z);
assert(std::abs(support_dir.dot(support - point_on_cone_base)) <= tol);
if (_SupportOptions == SupportOptions::WithSweptSphere) {
point_on_cone_base += cylinder->getSweptSphereRadius() * support_dir;
@@ -776,7 +774,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
} else {
// There are two potential supports to add: one on each circle bases of the
// cylinder.
- Vec3f point_on_lower_circle = Vec3f(cylinder->radius * support_dir[0], //
+ Vec3s point_on_lower_circle = Vec3s(cylinder->radius * support_dir[0], //
cylinder->radius * support_dir[1], //
-cylinder->halfLength);
if (support_value - support_dir.dot(point_on_lower_circle) <= tol) {
@@ -786,7 +784,7 @@ void getShapeSupportSet(const Cylinder* cylinder, SupportSet& support_set,
support_set.addPoint(point_on_lower_circle);
}
- Vec3f point_on_upper_circle = Vec3f(cylinder->radius * support_dir[0], //
+ Vec3s point_on_upper_circle = Vec3s(cylinder->radius * support_dir[0], //
cylinder->radius * support_dir[1], //
cylinder->halfLength);
if (support_value - support_dir.dot(point_on_upper_circle) <= tol) {
@@ -804,29 +802,29 @@ template <int _SupportOptions>
void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set,
int& hint /*unused*/,
ShapeSupportData& support_data, size_t /*unused*/,
- FCL_REAL tol) {
+ CoalScalar tol) {
assert(tol > 0);
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupport<SupportOptions::NoSweptSphere>(convex, support_dir, support,
hint, support_data);
- const FCL_REAL support_value = support_dir.dot(support);
+ const CoalScalar support_value = support_dir.dot(support);
- const std::vector<Vec3f>& points = *(convex->points);
+ const std::vector<Vec3s>& points = *(convex->points);
SupportSet::Polygon& polygon = support_data.polygon;
polygon.clear();
const Transform3f& tf = support_set.tf;
- for (const Vec3f& point : points) {
- const FCL_REAL dot = support_dir.dot(point);
+ for (const Vec3s& point : points) {
+ const CoalScalar dot = support_dir.dot(point);
if (support_value - dot <= tol) {
if (_SupportOptions == SupportOptions::WithSweptSphere) {
- const Vec2f p =
+ const Vec2s p =
tf.inverseTransform(point +
convex->getSweptSphereRadius() * support_dir)
.template head<2>();
polygon.emplace_back(p);
} else {
- const Vec2f p = tf.inverseTransform(point).template head<2>();
+ const Vec2s p = tf.inverseTransform(point).template head<2>();
polygon.emplace_back(p);
}
}
@@ -838,30 +836,30 @@ void getShapeSupportSetLinear(const ConvexBase* convex, SupportSet& support_set,
// ============================================================================
template <int _SupportOptions>
void convexSupportSetRecurse(
- const std::vector<Vec3f>& points,
+ const std::vector<Vec3s>& points,
const std::vector<ConvexBase::Neighbors>& neighbors,
- const FCL_REAL swept_sphere_radius, const size_t vertex_idx,
- const Vec3f& support_dir, const FCL_REAL support_value,
+ const CoalScalar swept_sphere_radius, const size_t vertex_idx,
+ const Vec3s& support_dir, const CoalScalar support_value,
const Transform3f& tf, std::vector<int8_t>& visited,
- SupportSet::Polygon& polygon, FCL_REAL tol) {
- HPP_FCL_UNUSED_VARIABLE(swept_sphere_radius);
+ SupportSet::Polygon& polygon, CoalScalar tol) {
+ COAL_UNUSED_VARIABLE(swept_sphere_radius);
if (visited[vertex_idx]) {
return;
}
visited[vertex_idx] = true;
- const Vec3f& point = points[vertex_idx];
- const FCL_REAL val = point.dot(support_dir);
+ const Vec3s& point = points[vertex_idx];
+ const CoalScalar val = point.dot(support_dir);
if (support_value - val <= tol) {
if (_SupportOptions == SupportOptions::WithSweptSphere) {
- const Vec2f p =
+ const Vec2s p =
tf.inverseTransform(point + swept_sphere_radius * support_dir)
.template head<2>();
polygon.emplace_back(p);
} else {
- const Vec2f p = tf.inverseTransform(point).template head<2>();
+ const Vec2s p = tf.inverseTransform(point).template head<2>();
polygon.emplace_back(p);
}
@@ -879,17 +877,17 @@ void convexSupportSetRecurse(
template <int _SupportOptions>
void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set,
int& hint, ShapeSupportData& support_data,
- size_t /*unused*/, FCL_REAL tol) {
+ size_t /*unused*/, CoalScalar tol) {
assert(tol > 0);
- Vec3f support;
- const Vec3f& support_dir = support_set.getNormal();
+ Vec3s support;
+ const Vec3s& support_dir = support_set.getNormal();
getShapeSupportLog<SupportOptions::NoSweptSphere>(
convex, support_dir, support, hint, support_data);
- const FCL_REAL support_value = support.dot(support_dir);
+ const CoalScalar support_value = support.dot(support_dir);
- const std::vector<Vec3f>& points = *(convex->points);
+ const std::vector<Vec3s>& points = *(convex->points);
const std::vector<ConvexBase::Neighbors>& neighbors = *(convex->neighbors);
- const FCL_REAL swept_sphere_radius = convex->getSweptSphereRadius();
+ const CoalScalar swept_sphere_radius = convex->getSweptSphereRadius();
std::vector<int8_t>& visited = support_data.visited;
// `visited` is guaranteed to be of right size due to previous call to convex
// log support function.
@@ -911,7 +909,8 @@ void getShapeSupportSetLog(const ConvexBase* convex, SupportSet& support_set,
template <int _SupportOptions>
void getShapeSupportSet(const ConvexBase* convex, SupportSet& support_set,
int& hint, ShapeSupportData& support_data,
- size_t num_sampled_supports /*unused*/, FCL_REAL tol) {
+ size_t num_sampled_supports /*unused*/,
+ CoalScalar tol) {
if (convex->num_points > ConvexBase::num_vertices_large_convex_threshold &&
convex->neighbors != nullptr) {
getShapeSupportSetLog<_SupportOptions>(
@@ -928,7 +927,8 @@ template <int _SupportOptions>
void getShapeSupportSet(const SmallConvex* convex, SupportSet& support_set,
int& hint /*unused*/,
ShapeSupportData& support_data /*unused*/,
- size_t num_sampled_supports /*unused*/, FCL_REAL tol) {
+ size_t num_sampled_supports /*unused*/,
+ CoalScalar tol) {
getShapeSupportSetLinear<_SupportOptions>(
reinterpret_cast<const ConvexBase*>(convex), support_set, hint,
support_data, num_sampled_supports, tol);
@@ -939,7 +939,8 @@ 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) {
+ size_t num_sampled_supports /*unused*/,
+ CoalScalar tol) {
getShapeSupportSetLog<_SupportOptions>(
reinterpret_cast<const ConvexBase*>(convex), support_set, hint,
support_data, num_sampled_supports, tol);
@@ -954,7 +955,7 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
if (cloud.size() <= 2) {
// Point or segment, nothing to do.
- for (const Vec2f& point : cloud) {
+ for (const Vec2s& point : cloud) {
cvx_hull.emplace_back(point);
}
return;
@@ -971,16 +972,16 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
if (cloud[0](1) > cloud[2](1)) {
std::swap(cloud[0], cloud[2]);
}
- const Vec2f& a = cloud[0];
- const Vec2f& b = cloud[1];
- const Vec2f& c = cloud[2];
- const FCL_REAL det =
+ const Vec2s& a = cloud[0];
+ const Vec2s& b = cloud[1];
+ const Vec2s& c = cloud[2];
+ const CoalScalar det =
(b(0) - a(0)) * (c(1) - a(1)) - (b(1) - a(1)) * (c(0) - a(0));
if (det < 0) {
std::swap(cloud[1], cloud[2]);
}
- for (const Vec2f& point : cloud) {
+ for (const Vec2s& point : cloud) {
cvx_hull.emplace_back(point);
}
return;
@@ -994,9 +995,9 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
// in the direction (0, -1) (take the element of the set which has the lowest
// y coordinate).
size_t support_idx = 0;
- FCL_REAL support_val = cloud[0](1);
+ CoalScalar support_val = cloud[0](1);
for (size_t i = 1; i < cloud.size(); ++i) {
- const FCL_REAL val = cloud[i](1);
+ const CoalScalar val = cloud[i](1);
if (val < support_val) {
support_val = val;
support_idx = i;
@@ -1005,17 +1006,17 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
std::swap(cloud[0], cloud[support_idx]);
cvx_hull.clear();
cvx_hull.emplace_back(cloud[0]);
- const Vec2f& v = cvx_hull[0];
+ const Vec2s& v = cvx_hull[0];
// Step 2 - Sort the rest of the point cloud according to the angle made with
// v. Note: we use stable_sort instead of sort because sort can fail if two
// values are identical.
std::stable_sort(
- cloud.begin() + 1, cloud.end(), [&v](const Vec2f& p1, const Vec2f& p2) {
+ cloud.begin() + 1, cloud.end(), [&v](const Vec2s& p1, const Vec2s& p2) {
// p1 is "smaller" than p2 if det(p1 - v, p2 - v) >= 0
- const FCL_REAL det =
+ const CoalScalar det =
(p1(0) - v(0)) * (p2(1) - v(1)) - (p1(1) - v(1)) * (p2(0) - v(0));
- if (std::abs(det) <= Eigen::NumTraits<FCL_REAL>::dummy_precision()) {
+ if (std::abs(det) <= Eigen::NumTraits<CoalScalar>::dummy_precision()) {
// If two points are identical or (v, p1, p2) are colinear, p1 is
// "smaller" if it is closer to v.
return ((p1 - v).squaredNorm() <= (p2 - v).squaredNorm());
@@ -1027,14 +1028,14 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
// to the cvx-hull if they successively form "left turns" only. A left turn
// is: considering the last three points of the cvx-hull, if they form a
// right-hand basis (determinant > 0) then they make a left turn.
- auto isRightSided = [](const Vec2f& p1, const Vec2f& p2, const Vec2f& p3) {
+ auto isRightSided = [](const Vec2s& p1, const Vec2s& p2, const Vec2s& p3) {
// Checks if (p2 - p1, p3 - p1) forms a right-sided base based on
// det(p2 - p1, p3 - p1)
- const FCL_REAL det =
+ const CoalScalar det =
(p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(1)) * (p3(0) - p1(0));
// Note: we set a dummy precision threshold so that identical points or
// colinear pionts are not added to the cvx-hull.
- return det > Eigen::NumTraits<FCL_REAL>::dummy_precision();
+ return det > Eigen::NumTraits<CoalScalar>::dummy_precision();
};
// We initialize the cvx-hull algo by adding the first three
@@ -1043,9 +1044,9 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
// to form a right sided basis, hence to form a left turn.
size_t cloud_beginning_idx = 1;
while (cvx_hull.size() < 3) {
- const Vec2f& vec = cloud[cloud_beginning_idx];
+ const Vec2s& vec = cloud[cloud_beginning_idx];
if ((cvx_hull.back() - vec).squaredNorm() >
- Eigen::NumTraits<FCL_REAL>::epsilon()) {
+ Eigen::NumTraits<CoalScalar>::epsilon()) {
cvx_hull.emplace_back(vec);
}
++cloud_beginning_idx;
@@ -1056,7 +1057,7 @@ void computeSupportSetConvexHull(SupportSet::Polygon& cloud,
// When we do a turn in the correct direction, we add a point to the
// convex-hull.
for (size_t i = cloud_beginning_idx; i < cloud.size(); ++i) {
- const Vec2f& vec = cloud[i];
+ const Vec2s& vec = cloud[i];
while (cvx_hull.size() > 1 &&
!isRightSided(cvx_hull[cvx_hull.size() - 2],
cvx_hull[cvx_hull.size() - 1], vec)) {
diff --git a/src/octree.cpp b/src/octree.cpp
index 6ca93eff..186ad51a 100644
--- a/src/octree.cpp
+++ b/src/octree.cpp
@@ -55,13 +55,13 @@ struct Neighbors {
void hasNeighboordPlusZ() { value |= 0x20; }
}; // struct neighbors
-void computeNeighbors(const std::vector<Vec6f>& boxes,
+void computeNeighbors(const std::vector<Vec6s>& boxes,
std::vector<Neighbors>& neighbors) {
- typedef std::vector<Vec6f> VectorVec6f;
+ typedef std::vector<Vec6s> VectorVec6s;
CoalScalar fixedSize = -1;
CoalScalar e(1e-8);
for (std::size_t i = 0; i < boxes.size(); ++i) {
- const Vec6f& box(boxes[i]);
+ const Vec6s& box(boxes[i]);
Neighbors& n(neighbors[i]);
CoalScalar x(box[0]);
CoalScalar y(box[1]);
@@ -72,9 +72,9 @@ void computeNeighbors(const std::vector<Vec6f>& boxes,
else
assert(s == fixedSize);
- for (VectorVec6f::const_iterator it = boxes.begin(); it != boxes.end();
+ for (VectorVec6s::const_iterator it = boxes.begin(); it != boxes.end();
++it) {
- const Vec6f& otherBox = *it;
+ const Vec6s& otherBox = *it;
CoalScalar xo(otherBox[0]);
CoalScalar yo(otherBox[1]);
CoalScalar zo(otherBox[2]);
@@ -106,20 +106,20 @@ void computeNeighbors(const std::vector<Vec6f>& boxes,
} // namespace internal
void OcTree::exportAsObjFile(const std::string& filename) const {
- std::vector<Vec6f> boxes(this->toBoxes());
+ std::vector<Vec6s> boxes(this->toBoxes());
std::vector<internal::Neighbors> neighbors(boxes.size());
internal::computeNeighbors(boxes, neighbors);
// compute list of vertices and faces
- typedef std::vector<Vec3f> VectorVec3f;
- std::vector<Vec3f> vertices;
+ typedef std::vector<Vec3s> VectorVec3s;
+ std::vector<Vec3s> vertices;
typedef std::array<std::size_t, 4> Array4;
typedef std::vector<Array4> VectorArray4;
std::vector<Array4> faces;
for (std::size_t i = 0; i < boxes.size(); ++i) {
- const Vec6f& box(boxes[i]);
+ const Vec6s& box(boxes[i]);
internal::Neighbors& n(neighbors[i]);
CoalScalar x(box[0]);
@@ -127,14 +127,14 @@ void OcTree::exportAsObjFile(const std::string& filename) const {
CoalScalar z(box[2]);
CoalScalar size(box[3]);
- vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z - .5 * size));
- vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z - .5 * size));
- vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z - .5 * size));
- vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z - .5 * size));
- vertices.push_back(Vec3f(x - .5 * size, y - .5 * size, z + .5 * size));
- vertices.push_back(Vec3f(x + .5 * size, y - .5 * size, z + .5 * size));
- vertices.push_back(Vec3f(x - .5 * size, y + .5 * size, z + .5 * size));
- vertices.push_back(Vec3f(x + .5 * size, y + .5 * size, z + .5 * size));
+ vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z - .5 * size));
+ vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z - .5 * size));
+ vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z - .5 * size));
+ vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z - .5 * size));
+ vertices.push_back(Vec3s(x - .5 * size, y - .5 * size, z + .5 * size));
+ vertices.push_back(Vec3s(x + .5 * size, y - .5 * size, z + .5 * size));
+ vertices.push_back(Vec3s(x - .5 * size, y + .5 * size, z + .5 * size));
+ vertices.push_back(Vec3s(x + .5 * size, y + .5 * size, z + .5 * size));
// Add face only if box has no neighbor with the same face
if (!n.minusX()) {
@@ -172,9 +172,9 @@ void OcTree::exportAsObjFile(const std::string& filename) const {
std::runtime_error);
// write vertices
os << "# list of vertices\n";
- for (VectorVec3f::const_iterator it = vertices.begin(); it != vertices.end();
+ for (VectorVec3s::const_iterator it = vertices.begin(); it != vertices.end();
++it) {
- const Vec3f& v = *it;
+ const Vec3s& v = *it;
os << "v " << v[0] << " " << v[1] << " " << v[2] << '\n';
}
os << "\n# list of faces\n";
diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp
index 98beb82a..e32677a0 100644
--- a/src/shape/convex.cpp
+++ b/src/shape/convex.cpp
@@ -22,16 +22,16 @@ namespace coal {
// Reorders `tri` such that the dot product between the normal of triangle and
// the vector `triangle barycentre - convex_tri.center` is positive.
void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) {
- Vec3f p0, p1, p2;
+ Vec3s p0, p1, p2;
p0 = (*(convex_tri->points))[tri[0]];
p1 = (*(convex_tri->points))[tri[1]];
p2 = (*(convex_tri->points))[tri[2]];
- Vec3f barycentre_tri, center_barycenter;
+ Vec3s barycentre_tri, center_barycenter;
barycentre_tri = (p0 + p1 + p2) / 3;
center_barycenter = barycentre_tri - convex_tri->center;
- Vec3f edge_tri1, edge_tri2, n_tri;
+ Vec3s edge_tri1, edge_tri2, n_tri;
edge_tri1 = p1 - p0;
edge_tri2 = p2 - p1;
n_tri = edge_tri1.cross(edge_tri2);
@@ -41,7 +41,7 @@ void reorderTriangle(const Convex<Triangle>* convex_tri, Triangle& tri) {
}
}
-ConvexBase* ConvexBase::convexHull(std::shared_ptr<std::vector<Vec3f>>& pts,
+ConvexBase* ConvexBase::convexHull(std::shared_ptr<std::vector<Vec3s>>& pts,
unsigned int num_points, bool keepTriangles,
const char* qhullCommand) {
COAL_COMPILER_DIAGNOSTIC_PUSH
@@ -51,7 +51,7 @@ ConvexBase* ConvexBase::convexHull(std::shared_ptr<std::vector<Vec3f>>& pts,
COAL_COMPILER_DIAGNOSTIC_POP
}
-ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
+ConvexBase* ConvexBase::convexHull(const Vec3s* pts, unsigned int num_points,
bool keepTriangles,
const char* qhullCommand) {
#ifdef COAL_HAS_QHULL
@@ -81,15 +81,15 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points,
// Initialize the vertices
size_t nvertex = static_cast<size_t>(qh.vertexCount());
- std::shared_ptr<std::vector<Vec3f>> vertices(
- new std::vector<Vec3f>(size_t(nvertex)));
+ std::shared_ptr<std::vector<Vec3s>> vertices(
+ new std::vector<Vec3s>(size_t(nvertex)));
QhullVertexList vertexList(qh.vertexList());
size_t i_vertex = 0;
for (QhullVertexList::const_iterator v = vertexList.begin();
v != vertexList.end(); ++v) {
QhullPoint pt((*v).point());
pts_to_vertices[(size_t)pt.id()] = (int)i_vertex;
- (*vertices)[i_vertex] = Vec3f(pt[0], pt[1], pt[2]);
+ (*vertices)[i_vertex] = Vec3s(pt[0], pt[1], pt[2]);
++i_vertex;
}
assert(i_vertex == nvertex);
@@ -230,15 +230,15 @@ void ConvexBase::buildDoubleDescription() {
void ConvexBase::buildDoubleDescriptionFromQHullResult(const Qhull& qh) {
num_normals_and_offsets = static_cast<unsigned int>(qh.facetCount());
- normals.reset(new std::vector<Vec3f>(num_normals_and_offsets));
- std::vector<Vec3f>& normals_ = *normals;
+ normals.reset(new std::vector<Vec3s>(num_normals_and_offsets));
+ std::vector<Vec3s>& normals_ = *normals;
offsets.reset(new std::vector<double>(num_normals_and_offsets));
std::vector<double>& offsets_ = *offsets;
unsigned int i_normal = 0;
for (QhullFacet facet = qh.beginFacet(); facet != qh.endFacet();
facet = facet.next()) {
const orgQhull::QhullHyperplane& plane = facet.hyperplane();
- normals_[i_normal] = Vec3f(plane.coordinates()[0], plane.coordinates()[1],
+ normals_[i_normal] = Vec3s(plane.coordinates()[0], plane.coordinates()[1],
plane.coordinates()[2]);
offsets_[i_normal] = plane.offset();
i_normal++;
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index ba7a2078..ab6c04a7 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -40,7 +40,7 @@
namespace coal {
-void ConvexBase::initialize(std::shared_ptr<std::vector<Vec3f>> points_,
+void ConvexBase::initialize(std::shared_ptr<std::vector<Vec3s>> points_,
unsigned int num_points_) {
this->points = points_;
this->num_points = num_points_;
@@ -54,7 +54,7 @@ void ConvexBase::initialize(std::shared_ptr<std::vector<Vec3f>> points_,
this->computeCenter();
}
-void ConvexBase::set(std::shared_ptr<std::vector<Vec3f>> points_,
+void ConvexBase::set(std::shared_ptr<std::vector<Vec3s>> points_,
unsigned int num_points_) {
initialize(points_, num_points_);
}
@@ -66,7 +66,7 @@ ConvexBase::ConvexBase(const ConvexBase& other)
center(other.center) {
if (other.points.get() && other.points->size() > 0) {
// Deep copy of other points
- points.reset(new std::vector<Vec3f>(*other.points));
+ points.reset(new std::vector<Vec3s>(*other.points));
} else
points.reset();
@@ -93,7 +93,7 @@ ConvexBase::ConvexBase(const ConvexBase& other)
nneighbors_.reset();
if (other.normals.get() && other.normals->size() > 0) {
- normals.reset(new std::vector<Vec3f>(*(other.normals)));
+ normals.reset(new std::vector<Vec3s>(*(other.normals)));
} else
normals.reset();
@@ -111,7 +111,7 @@ ConvexBase::~ConvexBase() {}
void ConvexBase::computeCenter() {
center.setZero();
- const std::vector<Vec3f>& points_ = *points;
+ const std::vector<Vec3s>& points_ = *points;
for (std::size_t i = 0; i < num_points; ++i)
center += points_[i]; // TODO(jcarpent): vectorization
center /= num_points;
@@ -145,8 +145,8 @@ void Box::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -156,8 +156,8 @@ void Sphere::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = radius;
@@ -167,8 +167,8 @@ void Ellipsoid::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -178,8 +178,8 @@ void Capsule::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -189,8 +189,8 @@ void Cone::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -200,8 +200,8 @@ void Cylinder::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -211,8 +211,8 @@ void ConvexBase::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -222,8 +222,8 @@ void Halfspace::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -233,8 +233,8 @@ void Plane::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
@@ -244,8 +244,8 @@ void TriangleP::computeLocalAABB() {
computeBV<AABB>(*this, Transform3f(), aabb_local);
const CoalScalar ssr = this->getSweptSphereRadius();
if (ssr > 0) {
- aabb_local.min_ -= Vec3f::Constant(ssr);
- aabb_local.max_ += Vec3f::Constant(ssr);
+ aabb_local.min_ -= Vec3s::Constant(ssr);
+ aabb_local.max_ += Vec3s::Constant(ssr);
}
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).norm();
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index b5d4bf2f..11960729 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -43,52 +43,52 @@ namespace coal {
namespace details {
-std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf) {
- std::vector<Vec3f> result(8);
+std::vector<Vec3s> getBoundVertices(const Box& box, const Transform3f& tf) {
+ std::vector<Vec3s> result(8);
CoalScalar a = box.halfSide[0];
CoalScalar b = box.halfSide[1];
CoalScalar c = box.halfSide[2];
- result[0] = tf.transform(Vec3f(a, b, c));
- result[1] = tf.transform(Vec3f(a, b, -c));
- result[2] = tf.transform(Vec3f(a, -b, c));
- result[3] = tf.transform(Vec3f(a, -b, -c));
- result[4] = tf.transform(Vec3f(-a, b, c));
- result[5] = tf.transform(Vec3f(-a, b, -c));
- result[6] = tf.transform(Vec3f(-a, -b, c));
- result[7] = tf.transform(Vec3f(-a, -b, -c));
+ result[0] = tf.transform(Vec3s(a, b, c));
+ result[1] = tf.transform(Vec3s(a, b, -c));
+ result[2] = tf.transform(Vec3s(a, -b, c));
+ result[3] = tf.transform(Vec3s(a, -b, -c));
+ result[4] = tf.transform(Vec3s(-a, b, c));
+ result[5] = tf.transform(Vec3s(-a, b, -c));
+ result[6] = tf.transform(Vec3s(-a, -b, c));
+ result[7] = tf.transform(Vec3s(-a, -b, -c));
return result;
}
// we use icosahedron to bound the sphere
-std::vector<Vec3f> getBoundVertices(const Sphere& sphere,
+std::vector<Vec3s> getBoundVertices(const Sphere& sphere,
const Transform3f& tf) {
- std::vector<Vec3f> result(12);
+ std::vector<Vec3s> result(12);
const CoalScalar m = (1 + sqrt(5.0)) / 2.0;
CoalScalar edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
CoalScalar a = edge_size;
CoalScalar b = m * edge_size;
- result[0] = tf.transform(Vec3f(0, a, b));
- result[1] = tf.transform(Vec3f(0, -a, b));
- result[2] = tf.transform(Vec3f(0, a, -b));
- result[3] = tf.transform(Vec3f(0, -a, -b));
- result[4] = tf.transform(Vec3f(a, b, 0));
- result[5] = tf.transform(Vec3f(-a, b, 0));
- result[6] = tf.transform(Vec3f(a, -b, 0));
- result[7] = tf.transform(Vec3f(-a, -b, 0));
- result[8] = tf.transform(Vec3f(b, 0, a));
- result[9] = tf.transform(Vec3f(b, 0, -a));
- result[10] = tf.transform(Vec3f(-b, 0, a));
- result[11] = tf.transform(Vec3f(-b, 0, -a));
+ result[0] = tf.transform(Vec3s(0, a, b));
+ result[1] = tf.transform(Vec3s(0, -a, b));
+ result[2] = tf.transform(Vec3s(0, a, -b));
+ result[3] = tf.transform(Vec3s(0, -a, -b));
+ result[4] = tf.transform(Vec3s(a, b, 0));
+ result[5] = tf.transform(Vec3s(-a, b, 0));
+ result[6] = tf.transform(Vec3s(a, -b, 0));
+ result[7] = tf.transform(Vec3s(-a, -b, 0));
+ result[8] = tf.transform(Vec3s(b, 0, a));
+ result[9] = tf.transform(Vec3s(b, 0, -a));
+ result[10] = tf.transform(Vec3s(-b, 0, a));
+ result[11] = tf.transform(Vec3s(-b, 0, -a));
return result;
}
// we use scaled icosahedron to bound the ellipsoid
-std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
+std::vector<Vec3s> getBoundVertices(const Ellipsoid& ellipsoid,
const Transform3f& tf) {
- std::vector<Vec3f> result(12);
+ std::vector<Vec3s> result(12);
const CoalScalar phi = (1 + sqrt(5.0)) / 2.0;
const CoalScalar a = sqrt(3.0) / (phi * phi);
@@ -104,25 +104,25 @@ std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid,
CoalScalar Bb = B * b;
CoalScalar Ca = C * a;
CoalScalar Cb = C * b;
- result[0] = tf.transform(Vec3f(0, Ba, Cb));
- result[1] = tf.transform(Vec3f(0, -Ba, Cb));
- result[2] = tf.transform(Vec3f(0, Ba, -Cb));
- result[3] = tf.transform(Vec3f(0, -Ba, -Cb));
- result[4] = tf.transform(Vec3f(Aa, Bb, 0));
- result[5] = tf.transform(Vec3f(-Aa, Bb, 0));
- result[6] = tf.transform(Vec3f(Aa, -Bb, 0));
- result[7] = tf.transform(Vec3f(-Aa, -Bb, 0));
- result[8] = tf.transform(Vec3f(Ab, 0, Ca));
- result[9] = tf.transform(Vec3f(Ab, 0, -Ca));
- result[10] = tf.transform(Vec3f(-Ab, 0, Ca));
- result[11] = tf.transform(Vec3f(-Ab, 0, -Ca));
+ result[0] = tf.transform(Vec3s(0, Ba, Cb));
+ result[1] = tf.transform(Vec3s(0, -Ba, Cb));
+ result[2] = tf.transform(Vec3s(0, Ba, -Cb));
+ result[3] = tf.transform(Vec3s(0, -Ba, -Cb));
+ result[4] = tf.transform(Vec3s(Aa, Bb, 0));
+ result[5] = tf.transform(Vec3s(-Aa, Bb, 0));
+ result[6] = tf.transform(Vec3s(Aa, -Bb, 0));
+ result[7] = tf.transform(Vec3s(-Aa, -Bb, 0));
+ result[8] = tf.transform(Vec3s(Ab, 0, Ca));
+ result[9] = tf.transform(Vec3s(Ab, 0, -Ca));
+ result[10] = tf.transform(Vec3s(-Ab, 0, Ca));
+ result[11] = tf.transform(Vec3s(-Ab, 0, -Ca));
return result;
}
-std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
+std::vector<Vec3s> getBoundVertices(const Capsule& capsule,
const Transform3f& tf) {
- std::vector<Vec3f> result(36);
+ std::vector<Vec3s> result(36);
const CoalScalar m = (1 + sqrt(5.0)) / 2.0;
CoalScalar hl = capsule.halfLength;
@@ -131,101 +131,101 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule,
CoalScalar b = m * edge_size;
CoalScalar r2 = capsule.radius * 2 / sqrt(3.0);
- result[0] = tf.transform(Vec3f(0, a, b + hl));
- result[1] = tf.transform(Vec3f(0, -a, b + hl));
- result[2] = tf.transform(Vec3f(0, a, -b + hl));
- result[3] = tf.transform(Vec3f(0, -a, -b + hl));
- result[4] = tf.transform(Vec3f(a, b, hl));
- result[5] = tf.transform(Vec3f(-a, b, hl));
- result[6] = tf.transform(Vec3f(a, -b, hl));
- result[7] = tf.transform(Vec3f(-a, -b, hl));
- result[8] = tf.transform(Vec3f(b, 0, a + hl));
- result[9] = tf.transform(Vec3f(b, 0, -a + hl));
- result[10] = tf.transform(Vec3f(-b, 0, a + hl));
- result[11] = tf.transform(Vec3f(-b, 0, -a + hl));
-
- result[12] = tf.transform(Vec3f(0, a, b - hl));
- result[13] = tf.transform(Vec3f(0, -a, b - hl));
- result[14] = tf.transform(Vec3f(0, a, -b - hl));
- result[15] = tf.transform(Vec3f(0, -a, -b - hl));
- result[16] = tf.transform(Vec3f(a, b, -hl));
- result[17] = tf.transform(Vec3f(-a, b, -hl));
- result[18] = tf.transform(Vec3f(a, -b, -hl));
- result[19] = tf.transform(Vec3f(-a, -b, -hl));
- result[20] = tf.transform(Vec3f(b, 0, a - hl));
- result[21] = tf.transform(Vec3f(b, 0, -a - hl));
- result[22] = tf.transform(Vec3f(-b, 0, a - hl));
- result[23] = tf.transform(Vec3f(-b, 0, -a - hl));
+ result[0] = tf.transform(Vec3s(0, a, b + hl));
+ result[1] = tf.transform(Vec3s(0, -a, b + hl));
+ result[2] = tf.transform(Vec3s(0, a, -b + hl));
+ result[3] = tf.transform(Vec3s(0, -a, -b + hl));
+ result[4] = tf.transform(Vec3s(a, b, hl));
+ result[5] = tf.transform(Vec3s(-a, b, hl));
+ result[6] = tf.transform(Vec3s(a, -b, hl));
+ result[7] = tf.transform(Vec3s(-a, -b, hl));
+ result[8] = tf.transform(Vec3s(b, 0, a + hl));
+ result[9] = tf.transform(Vec3s(b, 0, -a + hl));
+ result[10] = tf.transform(Vec3s(-b, 0, a + hl));
+ result[11] = tf.transform(Vec3s(-b, 0, -a + hl));
+
+ result[12] = tf.transform(Vec3s(0, a, b - hl));
+ result[13] = tf.transform(Vec3s(0, -a, b - hl));
+ result[14] = tf.transform(Vec3s(0, a, -b - hl));
+ result[15] = tf.transform(Vec3s(0, -a, -b - hl));
+ result[16] = tf.transform(Vec3s(a, b, -hl));
+ result[17] = tf.transform(Vec3s(-a, b, -hl));
+ result[18] = tf.transform(Vec3s(a, -b, -hl));
+ result[19] = tf.transform(Vec3s(-a, -b, -hl));
+ result[20] = tf.transform(Vec3s(b, 0, a - hl));
+ result[21] = tf.transform(Vec3s(b, 0, -a - hl));
+ result[22] = tf.transform(Vec3s(-b, 0, a - hl));
+ result[23] = tf.transform(Vec3s(-b, 0, -a - hl));
CoalScalar c = 0.5 * r2;
CoalScalar d = capsule.radius;
- result[24] = tf.transform(Vec3f(r2, 0, hl));
- result[25] = tf.transform(Vec3f(c, d, hl));
- result[26] = tf.transform(Vec3f(-c, d, hl));
- result[27] = tf.transform(Vec3f(-r2, 0, hl));
- result[28] = tf.transform(Vec3f(-c, -d, hl));
- result[29] = tf.transform(Vec3f(c, -d, hl));
-
- result[30] = tf.transform(Vec3f(r2, 0, -hl));
- result[31] = tf.transform(Vec3f(c, d, -hl));
- result[32] = tf.transform(Vec3f(-c, d, -hl));
- result[33] = tf.transform(Vec3f(-r2, 0, -hl));
- result[34] = tf.transform(Vec3f(-c, -d, -hl));
- result[35] = tf.transform(Vec3f(c, -d, -hl));
+ result[24] = tf.transform(Vec3s(r2, 0, hl));
+ result[25] = tf.transform(Vec3s(c, d, hl));
+ result[26] = tf.transform(Vec3s(-c, d, hl));
+ result[27] = tf.transform(Vec3s(-r2, 0, hl));
+ result[28] = tf.transform(Vec3s(-c, -d, hl));
+ result[29] = tf.transform(Vec3s(c, -d, hl));
+
+ result[30] = tf.transform(Vec3s(r2, 0, -hl));
+ result[31] = tf.transform(Vec3s(c, d, -hl));
+ result[32] = tf.transform(Vec3s(-c, d, -hl));
+ result[33] = tf.transform(Vec3s(-r2, 0, -hl));
+ result[34] = tf.transform(Vec3s(-c, -d, -hl));
+ result[35] = tf.transform(Vec3s(c, -d, -hl));
return result;
}
-std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf) {
- std::vector<Vec3f> result(7);
+std::vector<Vec3s> getBoundVertices(const Cone& cone, const Transform3f& tf) {
+ std::vector<Vec3s> result(7);
CoalScalar hl = cone.halfLength;
CoalScalar r2 = cone.radius * 2 / sqrt(3.0);
CoalScalar a = 0.5 * r2;
CoalScalar b = cone.radius;
- result[0] = tf.transform(Vec3f(r2, 0, -hl));
- result[1] = tf.transform(Vec3f(a, b, -hl));
- result[2] = tf.transform(Vec3f(-a, b, -hl));
- result[3] = tf.transform(Vec3f(-r2, 0, -hl));
- result[4] = tf.transform(Vec3f(-a, -b, -hl));
- result[5] = tf.transform(Vec3f(a, -b, -hl));
+ result[0] = tf.transform(Vec3s(r2, 0, -hl));
+ result[1] = tf.transform(Vec3s(a, b, -hl));
+ result[2] = tf.transform(Vec3s(-a, b, -hl));
+ result[3] = tf.transform(Vec3s(-r2, 0, -hl));
+ result[4] = tf.transform(Vec3s(-a, -b, -hl));
+ result[5] = tf.transform(Vec3s(a, -b, -hl));
- result[6] = tf.transform(Vec3f(0, 0, hl));
+ result[6] = tf.transform(Vec3s(0, 0, hl));
return result;
}
-std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder,
+std::vector<Vec3s> getBoundVertices(const Cylinder& cylinder,
const Transform3f& tf) {
- std::vector<Vec3f> result(12);
+ std::vector<Vec3s> result(12);
CoalScalar hl = cylinder.halfLength;
CoalScalar r2 = cylinder.radius * 2 / sqrt(3.0);
CoalScalar a = 0.5 * r2;
CoalScalar b = cylinder.radius;
- result[0] = tf.transform(Vec3f(r2, 0, -hl));
- result[1] = tf.transform(Vec3f(a, b, -hl));
- result[2] = tf.transform(Vec3f(-a, b, -hl));
- result[3] = tf.transform(Vec3f(-r2, 0, -hl));
- result[4] = tf.transform(Vec3f(-a, -b, -hl));
- result[5] = tf.transform(Vec3f(a, -b, -hl));
+ result[0] = tf.transform(Vec3s(r2, 0, -hl));
+ result[1] = tf.transform(Vec3s(a, b, -hl));
+ result[2] = tf.transform(Vec3s(-a, b, -hl));
+ result[3] = tf.transform(Vec3s(-r2, 0, -hl));
+ result[4] = tf.transform(Vec3s(-a, -b, -hl));
+ result[5] = tf.transform(Vec3s(a, -b, -hl));
- result[6] = tf.transform(Vec3f(r2, 0, hl));
- result[7] = tf.transform(Vec3f(a, b, hl));
- result[8] = tf.transform(Vec3f(-a, b, hl));
- result[9] = tf.transform(Vec3f(-r2, 0, hl));
- result[10] = tf.transform(Vec3f(-a, -b, hl));
- result[11] = tf.transform(Vec3f(a, -b, hl));
+ result[6] = tf.transform(Vec3s(r2, 0, hl));
+ result[7] = tf.transform(Vec3s(a, b, hl));
+ result[8] = tf.transform(Vec3s(-a, b, hl));
+ result[9] = tf.transform(Vec3s(-r2, 0, hl));
+ result[10] = tf.transform(Vec3s(-a, -b, hl));
+ result[11] = tf.transform(Vec3s(a, -b, hl));
return result;
}
-std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
+std::vector<Vec3s> getBoundVertices(const ConvexBase& convex,
const Transform3f& tf) {
- std::vector<Vec3f> result(convex.num_points);
- const std::vector<Vec3f>& points_ = *(convex.points);
+ std::vector<Vec3s> result(convex.num_points);
+ const std::vector<Vec3s>& points_ = *(convex.points);
for (std::size_t i = 0; i < convex.num_points; ++i) {
result[i] = tf.transform(points_[i]);
}
@@ -233,9 +233,9 @@ std::vector<Vec3f> getBoundVertices(const ConvexBase& convex,
return result;
}
-std::vector<Vec3f> getBoundVertices(const TriangleP& triangle,
+std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
const Transform3f& tf) {
- std::vector<Vec3f> result(3);
+ std::vector<Vec3s> result(3);
result[0] = tf.transform(triangle.a);
result[1] = tf.transform(triangle.b);
result[2] = tf.transform(triangle.c);
@@ -252,7 +252,7 @@ Halfspace transform(const Halfspace& a, const Transform3f& tf) {
/// where n' = R * n
/// and d' = d + n' * T
- Vec3f n = tf.getRotation() * a.n;
+ Vec3s n = tf.getRotation() * a.n;
CoalScalar d = a.d + n.dot(tf.getTranslation());
Halfspace result(n, d);
result.setSweptSphereRadius(a.getSweptSphereRadius());
@@ -267,7 +267,7 @@ Plane transform(const Plane& a, const Transform3f& tf) {
/// where n' = R * n
/// and d' = d + n' * T
- Vec3f n = tf.getRotation() * a.n;
+ Vec3s n = tf.getRotation() * a.n;
CoalScalar d = a.d + n.dot(tf.getTranslation());
Plane result(n, d);
result.setSweptSphereRadius(a.getSweptSphereRadius());
@@ -279,7 +279,7 @@ std::array<Halfspace, 2> transformToHalfspaces(const Plane& a,
const Transform3f& tf) {
// A plane can be represented by two halfspaces
- Vec3f n = tf.getRotation() * a.n;
+ Vec3s n = tf.getRotation() * a.n;
CoalScalar d = a.d + n.dot(tf.getTranslation());
std::array<Halfspace, 2> result = {Halfspace(n, d), Halfspace(-n, -d)};
result[0].setSweptSphereRadius(a.getSweptSphereRadius());
@@ -290,19 +290,19 @@ std::array<Halfspace, 2> transformToHalfspaces(const Plane& a,
template <>
void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv) {
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
- Vec3f v_delta(R.cwiseAbs() * s.halfSide);
+ Vec3s v_delta(R.cwiseAbs() * s.halfSide);
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
template <>
void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv) {
- const Vec3f& T = tf.getTranslation();
+ const Vec3s& T = tf.getTranslation();
- Vec3f v_delta(Vec3f::Constant(s.radius));
+ Vec3s v_delta(Vec3s::Constant(s.radius));
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
@@ -310,10 +310,10 @@ void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv) {
template <>
void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3f& tf,
AABB& bv) {
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
- Vec3f v_delta = R * e.radii;
+ Vec3s v_delta = R * e.radii;
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
@@ -321,18 +321,18 @@ void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3f& tf,
template <>
void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf,
AABB& bv) {
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
- Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius));
+ Vec3s v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3s::Constant(s.radius));
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
template <>
void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) {
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
fabs(R(0, 2) * s.halfLength);
@@ -341,7 +341,7 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) {
CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
fabs(R(2, 2) * s.halfLength);
- Vec3f v_delta(x_range, y_range, z_range);
+ Vec3s v_delta(x_range, y_range, z_range);
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
@@ -349,8 +349,8 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) {
template <>
void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf,
AABB& bv) {
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
CoalScalar x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) +
fabs(R(0, 2) * s.halfLength);
@@ -359,7 +359,7 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf,
CoalScalar z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) +
fabs(R(2, 2) * s.halfLength);
- Vec3f v_delta(x_range, y_range, z_range);
+ Vec3s v_delta(x_range, y_range, z_range);
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
@@ -367,13 +367,13 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf,
template <>
void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
AABB& bv) {
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
AABB bv_;
- const std::vector<Vec3f>& points_ = *(s.points);
+ const std::vector<Vec3s>& points_ = *(s.points);
for (std::size_t i = 0; i < s.num_points; ++i) {
- Vec3f new_p = R * points_[i] + T;
+ Vec3s new_p = R * points_[i] + T;
bv_ += new_p;
}
@@ -390,12 +390,12 @@ template <>
void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf,
AABB& bv) {
Halfspace new_s = transform(s, tf);
- const Vec3f& n = new_s.n;
+ const Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
AABB bv_;
- bv_.min_ = Vec3f::Constant(-(std::numeric_limits<CoalScalar>::max)());
- bv_.max_ = Vec3f::Constant((std::numeric_limits<CoalScalar>::max)());
+ bv_.min_ = Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)());
+ bv_.max_ = Vec3s::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)
@@ -422,12 +422,12 @@ void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf,
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 Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
AABB bv_;
- bv_.min_ = Vec3f::Constant(-(std::numeric_limits<CoalScalar>::max)());
- bv_.max_ = Vec3f::Constant((std::numeric_limits<CoalScalar>::max)());
+ bv_.min_ = Vec3s::Constant(-(std::numeric_limits<CoalScalar>::max)());
+ bv_.max_ = Vec3s::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) {
@@ -460,8 +460,8 @@ void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv) {
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
bv.To = T;
bv.axes = R;
@@ -474,7 +474,7 @@ void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv) {
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- const Vec3f& T = tf.getTranslation();
+ const Vec3s& T = tf.getTranslation();
bv.To.noalias() = T;
bv.axes.setIdentity();
@@ -487,8 +487,8 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv) {
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
bv.To.noalias() = T;
bv.axes.noalias() = R;
@@ -501,8 +501,8 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv) {
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
bv.To.noalias() = T;
bv.axes.noalias() = R;
@@ -516,8 +516,8 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf,
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
bv.To.noalias() = T;
bv.axes.noalias() = R;
@@ -531,8 +531,8 @@ void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf,
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ const Matrix3s& R = tf.getRotation();
+ const Vec3s& T = tf.getTranslation();
fit(s.points->data(), s.num_points, bv);
@@ -588,7 +588,7 @@ void computeBV<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].o = Vec3s();
bv.spheres[0].r = (std::numeric_limits<CoalScalar>::max)();
}
@@ -600,7 +600,7 @@ void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf,
std::runtime_error);
}
Halfspace new_s = transform(s, tf);
- const Vec3f& n = new_s.n;
+ const Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
const short D = 8;
@@ -660,7 +660,7 @@ void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf,
std::runtime_error);
}
Halfspace new_s = transform(s, tf);
- const Vec3f& n = new_s.n;
+ const Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
const short D = 9;
@@ -726,7 +726,7 @@ void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf,
std::runtime_error);
}
Halfspace new_s = transform(s, tf);
- const Vec3f& n = new_s.n;
+ const Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
const short D = 12;
@@ -805,14 +805,14 @@ void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv) {
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- Vec3f n = tf.getRotation() * s.n;
+ Vec3s n = tf.getRotation() * s.n;
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
bv.axes.col(0).noalias() = n;
bv.extent << 0, (std::numeric_limits<CoalScalar>::max)(),
(std::numeric_limits<CoalScalar>::max)();
- Vec3f p = s.n * s.d;
+ Vec3s p = s.n * s.d;
bv.To =
tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T
}
@@ -823,7 +823,7 @@ void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv) {
COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
std::runtime_error);
}
- Vec3f n = tf.getRotation() * s.n;
+ Vec3s n = tf.getRotation() * s.n;
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
bv.axes.col(0).noalias() = n;
@@ -833,7 +833,7 @@ void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv) {
bv.radius = 0;
- Vec3f p = s.n * s.d;
+ Vec3s p = s.n * s.d;
bv.Tr = tf.transform(p);
}
@@ -856,7 +856,7 @@ void computeBV<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].o = Vec3s();
bv.spheres[0].r = (std::numeric_limits<CoalScalar>::max)();
}
@@ -868,7 +868,7 @@ void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf,
std::runtime_error);
}
Plane new_s = transform(s, tf);
- const Vec3f& n = new_s.n;
+ const Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
const short D = 8;
@@ -914,7 +914,7 @@ void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf,
std::runtime_error);
}
Plane new_s = transform(s, tf);
- const Vec3f& n = new_s.n;
+ const Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
const short D = 9;
@@ -962,7 +962,7 @@ void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf,
std::runtime_error);
}
Plane new_s = transform(s, tf);
- const Vec3f& n = new_s.n;
+ const Vec3s& n = new_s.n;
const CoalScalar& d = new_s.d;
const short D = 12;
diff --git a/test/accelerated_gjk.cpp b/test/accelerated_gjk.cpp
index 0526e499..624c422a 100644
--- a/test/accelerated_gjk.cpp
+++ b/test/accelerated_gjk.cpp
@@ -56,7 +56,7 @@ using coal::ShapeBase;
using coal::support_func_guess_t;
using coal::Transform3f;
using coal::Triangle;
-using coal::Vec3f;
+using coal::Vec3s;
using coal::details::GJK;
using coal::details::MinkowskiDiff;
using coal::details::SupportOptions;
@@ -125,7 +125,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
Transform3f identity = Transform3f::Identity();
// Same init for both solvers
- Vec3f init_guess = Vec3f(1, 0, 0);
+ Vec3s init_guess = Vec3s(1, 0, 0);
support_func_guess_t init_support_guess;
init_support_guess.setZero();
@@ -139,7 +139,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
// Evaluate both solvers twice, make sure they give the same solution
GJK::Status res_gjk_1 =
gjk.evaluate(mink_diff, init_guess, init_support_guess);
- Vec3f ray_gjk = gjk.ray;
+ Vec3s ray_gjk = gjk.ray;
GJK::Status res_gjk_2 =
gjk.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res_gjk_1 == res_gjk_2);
@@ -150,7 +150,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
// --------------
GJK::Status res_nesterov_gjk_1 =
gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess);
- Vec3f ray_nesterov = gjk_nesterov.ray;
+ Vec3s ray_nesterov = gjk_nesterov.ray;
GJK::Status res_nesterov_gjk_2 =
gjk_nesterov.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res_nesterov_gjk_1 == res_nesterov_gjk_2);
@@ -171,7 +171,7 @@ void test_accelerated_gjk(const ShapeBase& shape0, const ShapeBase& shape1) {
// ------------
GJK::Status res_polyak_gjk_1 =
gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess);
- Vec3f ray_polyak = gjk_polyak.ray;
+ Vec3s ray_polyak = gjk_polyak.ray;
GJK::Status res_polyak_gjk_2 =
gjk_polyak.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res_polyak_gjk_1 == res_polyak_gjk_2);
diff --git a/test/benchmark.cpp b/test/benchmark.cpp
index 584ceb64..2f7b3742 100644
--- a/test/benchmark.cpp
+++ b/test/benchmark.cpp
@@ -33,7 +33,7 @@ bool verbose = false;
CoalScalar DELTA = 0.001;
template <typename BV>
-void makeModel(const std::vector<Vec3f>& vertices,
+void makeModel(const std::vector<Vec3s>& vertices,
const std::vector<Triangle>& triangles,
SplitMethodType split_method, BVHModel<BV>& model);
@@ -78,7 +78,7 @@ struct traits<OBBRSS> {
};
template <typename BV>
-void makeModel(const std::vector<Vec3f>& vertices,
+void makeModel(const std::vector<Vec3s>& vertices,
const std::vector<Triangle>& triangles,
SplitMethodType split_method, BVHModel<BV>& model) {
model.bv_splitter.reset(new BVSplitter<BV>(split_method));
@@ -166,7 +166,7 @@ double run<OBB>(const std::vector<Transform3f>& tf,
}
int main(int, char*[]) {
- std::vector<Vec3f> p1, p2;
+ std::vector<Vec3s> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
diff --git a/test/box_box_collision.cpp b/test/box_box_collision.cpp
index 2633febd..8a99e3b1 100644
--- a/test/box_box_collision.cpp
+++ b/test/box_box_collision.cpp
@@ -15,7 +15,7 @@ using coal::CollisionRequest;
using coal::CollisionResult;
using coal::ComputeCollision;
using coal::Transform3f;
-using coal::Vec3f;
+using coal::Vec3s;
BOOST_AUTO_TEST_CASE(box_box_collision) {
// Define boxes
@@ -33,13 +33,13 @@ BOOST_AUTO_TEST_CASE(box_box_collision) {
CollisionResult res;
ComputeCollision collide_functor(&shape1, &shape2);
- T1.setTranslation(Vec3f(0, 0, 0));
+ T1.setTranslation(Vec3s(0, 0, 0));
res.clear();
BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == true);
res.clear();
BOOST_CHECK(collide_functor(T1, T2, req, res) == true);
- T1.setTranslation(Vec3f(2, 0, 0));
+ T1.setTranslation(Vec3s(2, 0, 0));
res.clear();
BOOST_CHECK(collide(&shape1, T1, &shape2, T2, req, res) == false);
res.clear();
diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp
index e559ab2c..dc5189a1 100644
--- a/test/box_box_distance.cpp
+++ b/test/box_box_distance.cpp
@@ -57,14 +57,14 @@ using coal::CollisionObject;
using coal::DistanceRequest;
using coal::DistanceResult;
using coal::Transform3f;
-using coal::Vec3f;
+using coal::Vec3s;
BOOST_AUTO_TEST_CASE(distance_box_box_1) {
CollisionGeometryPtr_t s1(new coal::Box(6, 10, 2));
CollisionGeometryPtr_t s2(new coal::Box(2, 2, 2));
Transform3f tf1;
- Transform3f tf2(Vec3f(25, 20, 5));
+ Transform3f tf2(Vec3s(25, 20, 5));
CollisionObject o1(s1, tf1);
CollisionObject o2(s2, tf2);
@@ -87,8 +87,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1) {
double dy = 20 - 5 - 1;
double dz = 5 - 1 - 1;
- const Vec3f& p1 = distanceResult.nearest_points[0];
- const Vec3f& p2 = distanceResult.nearest_points[1];
+ const Vec3s& p1 = distanceResult.nearest_points[0];
+ const Vec3s& p2 = distanceResult.nearest_points[1];
BOOST_CHECK_CLOSE(distanceResult.min_distance,
sqrt(dx * dx + dy * dy + dz * dz), 1e-4);
@@ -107,7 +107,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) {
Transform3f tf1;
Transform3f tf2(coal::makeQuat(cos(pi / 8), sin(pi / 8) / sqrt(3),
sin(pi / 8) / sqrt(3), sin(pi / 8) / sqrt(3)),
- Vec3f(0, 0, 10));
+ Vec3s(0, 0, 10));
CollisionObject o1(s1, tf1);
CollisionObject o2(s2, tf2);
@@ -127,8 +127,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2) {
<< ", p2 = " << distanceResult.nearest_points[1]
<< ", distance = " << distanceResult.min_distance << std::endl;
- const Vec3f& p1 = distanceResult.nearest_points[0];
- const Vec3f& p2 = distanceResult.nearest_points[1];
+ const Vec3s& p1 = distanceResult.nearest_points[0];
+ const Vec3s& p2 = distanceResult.nearest_points[1];
double distance = -1.62123444 + 10 - 1;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
@@ -145,9 +145,9 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
CollisionGeometryPtr_t s2(new coal::Box(1, 1, 1));
static double pi = M_PI;
Transform3f tf1(coal::makeQuat(cos(pi / 8), 0, 0, sin(pi / 8)),
- Vec3f(-2, 1, .5));
+ Vec3s(-2, 1, .5));
Transform3f tf2(coal::makeQuat(cos(pi / 8), 0, sin(pi / 8), 0),
- Vec3f(2, .5, .5));
+ Vec3s(2, .5, .5));
CollisionObject o1(s1, tf1);
CollisionObject o2(s2, tf2);
@@ -167,13 +167,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
<< ", p2 = " << distanceResult.nearest_points[1]
<< ", distance = " << distanceResult.min_distance << std::endl;
- const Vec3f& p1 = distanceResult.nearest_points[0];
- const Vec3f& p2 = distanceResult.nearest_points[1];
+ const Vec3s& p1 = distanceResult.nearest_points[0];
+ const Vec3s& p2 = distanceResult.nearest_points[1];
double distance = 4 - sqrt(2);
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
- const Vec3f p1Ref(sqrt(2) / 2 - 2, 1, .5);
- const Vec3f p2Ref(2 - sqrt(2) / 2, 1, .5);
+ const Vec3s p1Ref(sqrt(2) / 2 - 2, 1, .5);
+ const Vec3s p2Ref(2 - sqrt(2) / 2, 1, .5);
BOOST_CHECK_CLOSE(p1[0], p1Ref[0], 1e-4);
BOOST_CHECK_CLOSE(p1[1], p1Ref[1], 1e-4);
BOOST_CHECK_CLOSE(p1[2], p1Ref[2], 1e-4);
@@ -184,7 +184,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
// Apply the same global transform to both objects and recompute
Transform3f tf3(coal::makeQuat(0.435952844074, -0.718287018243,
0.310622451066, 0.444435113443),
- Vec3f(4, 5, 6));
+ Vec3s(4, 5, 6));
tf1 = tf3 * tf1;
tf2 = tf3 * tf2;
o1 = CollisionObject(s1, tf1);
@@ -204,8 +204,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3) {
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
- const Vec3f p1Moved = tf3.transform(p1Ref);
- const Vec3f p2Moved = tf3.transform(p2Ref);
+ const Vec3s p1Moved = tf3.transform(p1Ref);
+ const Vec3s p2Moved = tf3.transform(p2Ref);
BOOST_CHECK_CLOSE(p1[0], p1Moved[0], 1e-4);
BOOST_CHECK_CLOSE(p1[1], p1Moved[1], 1e-4);
BOOST_CHECK_CLOSE(p1[2], p1Moved[2], 1e-4);
@@ -223,28 +223,28 @@ BOOST_AUTO_TEST_CASE(distance_box_box_4) {
DistanceResult distanceResult;
double distance;
- Transform3f tf1(Vec3f(2, 0, 0));
+ Transform3f tf1(Vec3s(2, 0, 0));
Transform3f tf2;
coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
distance = 1.;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
- tf1.setTranslation(Vec3f(1.01, 0, 0));
+ tf1.setTranslation(Vec3s(1.01, 0, 0));
distanceResult.clear();
coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
distance = 0.01;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
- tf1.setTranslation(Vec3f(0.99, 0, 0));
+ tf1.setTranslation(Vec3s(0.99, 0, 0));
distanceResult.clear();
coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
distance = -0.01;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
- tf1.setTranslation(Vec3f(0, 0, 0));
+ tf1.setTranslation(Vec3s(0, 0, 0));
distanceResult.clear();
coal::distance(&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
diff --git a/test/broadphase.cpp b/test/broadphase.cpp
index 4bb9b642..8750e55d 100644
--- a/test/broadphase.cpp
+++ b/test/broadphase.cpp
@@ -159,7 +159,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
Box* box = new Box(single_size, single_size, single_size);
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(box),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -174,7 +174,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
Sphere* sphere = new Sphere(single_size / 2);
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(sphere),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -189,7 +189,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
Cylinder* cylinder = new Cylinder(single_size / 2, single_size);
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(cylinder),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -204,7 +204,7 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env,
Cone* cone = new Cone(single_size / 2, single_size);
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(cone),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -231,7 +231,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
generateBVHModel(*model, box, Transform3f());
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(model),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -248,7 +248,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
generateBVHModel(*model, sphere, Transform3f(), 16, 16);
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(model),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -265,7 +265,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(model),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -282,7 +282,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env,
generateBVHModel(*model, cone, Transform3f(), 16, 16);
env.push_back(new CollisionObject(
shared_ptr<CollisionGeometry>(model),
- Transform3f(Vec3f(
+ Transform3f(Vec3s(
x * step_size + delta_size + 0.5 * single_size - env_scale,
y * step_size + delta_size + 0.5 * single_size - env_scale,
z * step_size + delta_size + 0.5 * single_size - env_scale))));
@@ -308,7 +308,7 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size,
managers.push_back(new SaPCollisionManager());
managers.push_back(new IntervalTreeCollisionManager());
- Vec3f lower_limit, upper_limit;
+ Vec3s lower_limit, upper_limit;
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
CoalScalar cell_size =
std::min(std::min((upper_limit[0] - lower_limit[0]) / 5,
@@ -458,7 +458,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size,
managers.push_back(new SaPCollisionManager());
managers.push_back(new IntervalTreeCollisionManager());
- Vec3f lower_limit, upper_limit;
+ Vec3s lower_limit, upper_limit;
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
CoalScalar cell_size =
std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
diff --git a/test/broadphase_collision_1.cpp b/test/broadphase_collision_1.cpp
index 2655390f..4afeda0c 100644
--- a/test/broadphase_collision_1.cpp
+++ b/test/broadphase_collision_1.cpp
@@ -212,7 +212,7 @@ void broad_phase_duplicate_check_test(CoalScalar env_scale,
managers.push_back(new SSaPCollisionManager());
managers.push_back(new SaPCollisionManager());
managers.push_back(new IntervalTreeCollisionManager());
- Vec3f lower_limit, upper_limit;
+ Vec3s lower_limit, upper_limit;
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
CoalScalar cell_size =
std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
@@ -283,13 +283,13 @@ void broad_phase_duplicate_check_test(CoalScalar env_scale,
CoalScalar rand_trans_z =
2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
- Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) *
- Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) *
- Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ()));
- Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
+ Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) *
+ Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) *
+ Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ()));
+ Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z);
- Matrix3f R = env[i]->getRotation();
- Vec3f T = env[i]->getTranslation();
+ Matrix3s R = env[i]->getRotation();
+ Vec3s T = env[i]->getTranslation();
env[i]->setTransform(dR * R, dR * T + dT);
env[i]->computeAABB();
}
@@ -383,7 +383,7 @@ void broad_phase_update_collision_test(CoalScalar env_scale,
managers.push_back(new SaPCollisionManager());
managers.push_back(new IntervalTreeCollisionManager());
- Vec3f lower_limit, upper_limit;
+ Vec3s lower_limit, upper_limit;
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
CoalScalar cell_size =
std::min(std::min((upper_limit[0] - lower_limit[0]) / 20,
@@ -456,13 +456,13 @@ void broad_phase_update_collision_test(CoalScalar env_scale,
CoalScalar rand_trans_z =
2 * (rand() / (CoalScalar)RAND_MAX - 0.5) * delta_trans_max;
- Matrix3f dR(Eigen::AngleAxisd(rand_angle_x, Vec3f::UnitX()) *
- Eigen::AngleAxisd(rand_angle_y, Vec3f::UnitY()) *
- Eigen::AngleAxisd(rand_angle_z, Vec3f::UnitZ()));
- Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
+ Matrix3s dR(Eigen::AngleAxisd(rand_angle_x, Vec3s::UnitX()) *
+ Eigen::AngleAxisd(rand_angle_y, Vec3s::UnitY()) *
+ Eigen::AngleAxisd(rand_angle_z, Vec3s::UnitZ()));
+ Vec3s dT(rand_trans_x, rand_trans_y, rand_trans_z);
- Matrix3f R = env[i]->getRotation();
- Vec3f T = env[i]->getTranslation();
+ Matrix3s R = env[i]->getRotation();
+ Vec3s T = env[i]->getTranslation();
env[i]->setTransform(dR * R, dR * T + dT);
env[i]->computeAABB();
}
diff --git a/test/broadphase_collision_2.cpp b/test/broadphase_collision_2.cpp
index 329ebbed..fe546582 100644
--- a/test/broadphase_collision_2.cpp
+++ b/test/broadphase_collision_2.cpp
@@ -208,7 +208,7 @@ void broad_phase_collision_test(CoalScalar env_scale, std::size_t env_size,
managers.push_back(new SaPCollisionManager());
managers.push_back(new IntervalTreeCollisionManager());
- Vec3f lower_limit, upper_limit;
+ Vec3s lower_limit, upper_limit;
SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
// CoalScalar ncell_per_axis = std::pow((S)env_size / 10, 1 / 3.0);
CoalScalar ncell_per_axis = 20;
diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp
index 4b1f8aed..b4abb861 100644
--- a/test/bvh_models.cpp
+++ b/test/bvh_models.cpp
@@ -56,11 +56,11 @@ using namespace coal;
template <typename BV>
void testBVHModelPointCloud() {
- Box box(Vec3f::Ones());
+ Box box(Vec3s::Ones());
double a = box.halfSide[0];
double b = box.halfSide[1];
double c = box.halfSide[2];
- std::vector<Vec3f> points(8);
+ std::vector<Vec3s> points(8);
points[0] << a, -b, c;
points[1] << a, b, c;
points[2] << -a, b, c;
@@ -114,7 +114,7 @@ void testBVHModelPointCloud() {
return;
}
- Matrixx3f all_points((Eigen::DenseIndex)points.size(), 3);
+ MatrixX3s all_points((Eigen::DenseIndex)points.size(), 3);
for (size_t k = 0; k < points.size(); ++k)
all_points.row((Eigen::DenseIndex)k) = points[k].transpose();
@@ -139,13 +139,13 @@ void testBVHModelPointCloud() {
template <typename BV>
void testBVHModelTriangles() {
shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
- Box box(Vec3f::Ones());
- AABB aabb(Vec3f(-1, 0, -1), Vec3f(1, 1, 1));
+ Box box(Vec3s::Ones());
+ AABB aabb(Vec3s(-1, 0, -1), Vec3s(1, 1, 1));
double a = box.halfSide[0];
double b = box.halfSide[1];
double c = box.halfSide[2];
- std::vector<Vec3f> points(8);
+ std::vector<Vec3s> points(8);
std::vector<Triangle> tri_indices(12);
points[0] << a, -b, c;
points[1] << a, b, c;
@@ -197,14 +197,14 @@ void testBVHModelTriangles() {
BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
- pose.setTranslation(Vec3f(0, 1, 0));
+ pose.setTranslation(Vec3s(0, 1, 0));
cropped.reset(BVHExtract(*model, pose, aabb));
BOOST_REQUIRE(cropped);
BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
- pose.setTranslation(Vec3f(0, 0, 0));
+ pose.setTranslation(Vec3s(0, 0, 0));
CoalScalar sqrt2_2 = std::sqrt(2) / 2;
pose.setQuatRotation(Quatf(sqrt2_2, sqrt2_2, 0, 0));
cropped.reset(BVHExtract(*model, pose, aabb));
@@ -213,13 +213,13 @@ void testBVHModelTriangles() {
BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
- pose.setTranslation(-Vec3f(1, 1, 1));
+ pose.setTranslation(-Vec3s(1, 1, 1));
pose.setQuatRotation(Quatf::Identity());
cropped.reset(BVHExtract(*model, pose, aabb));
BOOST_CHECK(!cropped);
- aabb = AABB(Vec3f(-0.1, -0.1, -0.1), Vec3f(0.1, 0.1, 0.1));
- pose.setTranslation(Vec3f(-0.5, -0.5, 0));
+ aabb = AABB(Vec3s(-0.1, -0.1, -0.1), Vec3s(0.1, 0.1, 0.1));
+ pose.setTranslation(Vec3s(-0.5, -0.5, 0));
cropped.reset(BVHExtract(*model, pose, aabb));
BOOST_REQUIRE(cropped);
BOOST_CHECK_EQUAL(cropped->num_tris, 2);
@@ -229,12 +229,12 @@ void testBVHModelTriangles() {
template <typename BV>
void testBVHModelSubModel() {
shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
- Box box(Vec3f::Ones());
+ Box box(Vec3s::Ones());
double a = box.halfSide[0];
double b = box.halfSide[1];
double c = box.halfSide[2];
- std::vector<Vec3f> points(8);
+ std::vector<Vec3s> points(8);
std::vector<Triangle> tri_indices(12);
points[0] << a, -b, c;
points[1] << a, b, c;
@@ -304,7 +304,7 @@ void testLoadPolyhedron() {
typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
PolyhedronPtr_t P1(new Polyhedron_t), P2;
- Vec3f scale;
+ Vec3s scale;
scale.setConstant(1);
loadPolyhedronFromResource(env, scale, P1);
@@ -331,11 +331,11 @@ void testLoadGerardBauzil() {
typedef shared_ptr<Polyhedron_t> PolyhedronPtr_t;
PolyhedronPtr_t P1(new Polyhedron_t), P2;
- Vec3f scale;
+ Vec3s scale;
scale.setConstant(1);
loadPolyhedronFromResource(env, scale, P1);
CollisionGeometryPtr_t cylinder(new Cylinder(.27, .27));
- Transform3f pos(Vec3f(-1.33, 1.36, .14));
+ Transform3f pos(Vec3s(-1.33, 1.36, .14));
CollisionObject obj(cylinder, pos);
CollisionObject stairs(P1);
diff --git a/test/capsule_box_1.cpp b/test/capsule_box_1.cpp
index 568bfe3e..386aecf4 100644
--- a/test/capsule_box_1.cpp
+++ b/test/capsule_box_1.cpp
@@ -59,7 +59,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
coal::DistanceRequest distanceRequest(true, 0, 0);
coal::DistanceResult distanceResult;
- coal::Transform3f tf1(coal::Vec3f(3., 0, 0));
+ coal::Transform3f tf1(coal::Vec3s(3., 0, 0));
coal::Transform3f tf2;
coal::CollisionObject capsule(capsuleGeometry, tf1);
coal::CollisionObject box(boxGeometry, tf2);
@@ -67,9 +67,9 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
// test distance
coal::distance(&capsule, &box, distanceRequest, distanceResult);
// Nearest point on capsule
- coal::Vec3f o1(distanceResult.nearest_points[0]);
+ coal::Vec3s o1(distanceResult.nearest_points[0]);
// Nearest point on box
- coal::Vec3f o2(distanceResult.nearest_points[1]);
+ coal::Vec3s o2(distanceResult.nearest_points[1]);
BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.5, 1e-1);
BOOST_CHECK_CLOSE(o1[0], 1.0, 1e-1);
CHECK_CLOSE_TO_0(o1[1], 1e-1);
@@ -77,7 +77,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
CHECK_CLOSE_TO_0(o2[1], 1e-1);
// Move capsule above box
- tf1 = coal::Transform3f(coal::Vec3f(0., 0., 8.));
+ tf1 = coal::Transform3f(coal::Vec3s(0., 0., 8.));
capsule.setTransform(tf1);
// test distance
@@ -96,7 +96,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
BOOST_CHECK_CLOSE(o2[2], 2.0, 1e-1);
// Rotate capsule around y axis by pi/2 and move it behind box
- tf1.setTranslation(coal::Vec3f(-10., 0., 0.));
+ tf1.setTranslation(coal::Vec3s(-10., 0., 0.));
tf1.setQuatRotation(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0));
capsule.setTransform(tf1);
diff --git a/test/capsule_box_2.cpp b/test/capsule_box_2.cpp
index b16d523c..8e3bb3ba 100644
--- a/test/capsule_box_2.cpp
+++ b/test/capsule_box_2.cpp
@@ -61,7 +61,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
// Rotate capsule around y axis by pi/2 and move it behind box
coal::Transform3f tf1(coal::makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0),
- coal::Vec3f(-10., 0.8, 1.5));
+ coal::Vec3s(-10., 0.8, 1.5));
coal::Transform3f tf2;
coal::CollisionObject capsule(capsuleGeometry, tf1);
coal::CollisionObject box(boxGeometry, tf2);
@@ -69,8 +69,8 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) {
// test distance
distanceResult.clear();
coal::distance(&capsule, &box, distanceRequest, distanceResult);
- coal::Vec3f o1 = distanceResult.nearest_points[0];
- coal::Vec3f o2 = distanceResult.nearest_points[1];
+ coal::Vec3s o1 = distanceResult.nearest_points[0];
+ coal::Vec3s o2 = distanceResult.nearest_points[1];
BOOST_CHECK_CLOSE(distanceResult.min_distance, 5.5, 1e-2);
BOOST_CHECK_CLOSE(o1[0], -6, 1e-2);
diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp
index a20f0fe8..4534af24 100644
--- a/test/capsule_capsule.cpp
+++ b/test/capsule_capsule.cpp
@@ -241,7 +241,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) {
CollisionGeometryPtr_t s2(new Capsule(5, 10));
Transform3f tf1;
- Transform3f tf2(Vec3f(20.1, 0, 0));
+ Transform3f tf2(Vec3s(20.1, 0, 0));
CollisionObject o1(s1, tf1);
CollisionObject o2(s2, tf2);
@@ -267,7 +267,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) {
CollisionGeometryPtr_t s2(new Capsule(5, 10));
Transform3f tf1;
- Transform3f tf2(Vec3f(20, 20, 0));
+ Transform3f tf2(Vec3s(20, 20, 0));
CollisionObject o1(s1, tf1);
CollisionObject o2(s2, tf2);
@@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) {
CollisionGeometryPtr_t s2(new Capsule(5, 10));
Transform3f tf1;
- Transform3f tf2(Vec3f(0, 0, 20.1));
+ Transform3f tf2(Vec3s(0, 0, 20.1));
CollisionObject o1(s1, tf1);
CollisionObject o2(s2, tf2);
@@ -320,7 +320,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) {
CollisionGeometryPtr_t s2(new Capsule(5, 10));
Transform3f tf1;
- Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3f(0, 0, 25.1));
+ Transform3f tf2(makeQuat(sqrt(2) / 2, 0, sqrt(2) / 2, 0), Vec3s(0, 0, 25.1));
CollisionObject o1(s1, tf1);
CollisionObject o2(s2, tf2);
@@ -343,8 +343,8 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) {
<< std::endl
<< "distance = " << distanceResult.min_distance << std::endl;
- const Vec3f& p1 = distanceResult.nearest_points[0];
- const Vec3f& p2 = distanceResult.nearest_points[1];
+ const Vec3s& p1 = distanceResult.nearest_points[0];
+ const Vec3s& p2 = distanceResult.nearest_points[1];
BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
CHECK_CLOSE_TO_0(p1[0], 1e-4);
diff --git a/test/collision.cpp b/test/collision.cpp
index 76bd8824..d0295994 100644
--- a/test/collision.cpp
+++ b/test/collision.cpp
@@ -78,8 +78,8 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test) {
generateRandomTransforms(r_extents, rotate_transform, 1);
AABB aabb1;
- aabb1.min_ = Vec3f(-600, -600, -600);
- aabb1.max_ = Vec3f(600, 600, 600);
+ aabb1.min_ = Vec3s(-600, -600, -600);
+ aabb1.max_ = Vec3s(600, 600, 600);
OBB obb1;
convertBV(aabb1, rotate_transform[0], obb1);
@@ -128,8 +128,8 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test) {
generateRandomTransforms(r_extents, rotate_transform, 1);
AABB aabb1;
- aabb1.min_ = Vec3f(-600, -600, -600);
- aabb1.max_ = Vec3f(600, 600, 600);
+ aabb1.min_ = Vec3s(-600, -600, -600);
+ aabb1.max_ = Vec3s(600, 600, 600);
OBB obb1;
convertBV(aabb1, rotate_transform[0], obb1);
@@ -213,8 +213,8 @@ BOOST_AUTO_TEST_CASE(OBB_AABB_test) {
generateRandomTransforms(extents, transforms, n);
AABB aabb1;
- aabb1.min_ = Vec3f(-600, -600, -600);
- aabb1.max_ = Vec3f(600, 600, 600);
+ aabb1.min_ = Vec3s(-600, -600, -600);
+ aabb1.max_ = Vec3s(600, 600, 600);
OBB obb1;
convertBV(aabb1, Transform3f(), obb1);
@@ -369,9 +369,9 @@ struct mesh_mesh_run_test {
model1->bv_splitter.reset(new BVSplitter<BV>(splitMethod));
model2->bv_splitter.reset(new BVSplitter<BV>(splitMethod));
- loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3f::Ones(),
+ loadPolyhedronFromResource(TEST_RESOURCES_DIR "/env.obj", Vec3s::Ones(),
model1);
- loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3f::Ones(),
+ loadPolyhedronFromResource(TEST_RESOURCES_DIR "/rob.obj", Vec3s::Ones(),
model2);
Timer timer(false);
diff --git a/test/collision_node_asserts.cpp b/test/collision_node_asserts.cpp
index d1dbc9e3..3214a96c 100644
--- a/test/collision_node_asserts.cpp
+++ b/test/collision_node_asserts.cpp
@@ -13,12 +13,12 @@ double DegToRad(const double& deg) {
static double degToRad = pi / 180.;
return deg * degToRad;
}
-std::vector<Vec3f> dirs{Vec3f::UnitZ(), -Vec3f::UnitZ(), Vec3f::UnitY(),
- -Vec3f::UnitY(), Vec3f::UnitX(), -Vec3f::UnitX()};
+std::vector<Vec3s> dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(),
+ -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()};
BOOST_AUTO_TEST_CASE(TestTriangles) {
- std::vector<Vec3f> triVertices{Vec3f(1, 0, 0), Vec3f(1, 1, 0),
- Vec3f(0, 1, 0)};
+ std::vector<Vec3s> triVertices{Vec3s(1, 0, 0), Vec3s(1, 1, 0),
+ Vec3s(0, 1, 0)};
std::vector<Triangle> triangle{{0, 1, 2}};
BVHModel<OBBRSS> tri1{};
@@ -44,11 +44,11 @@ BOOST_AUTO_TEST_CASE(TestTriangles) {
for (int j = 0; j < 180; j += 30) {
for (int k = 0; k < 180; k += 30) {
tri1Tf.setQuatRotation(
- Eigen::AngleAxis<double>(0., Vec3f::UnitZ()) *
- Eigen::AngleAxis<double>(DegToRad(k), Vec3f::UnitY()));
+ Eigen::AngleAxis<double>(0., Vec3s::UnitZ()) *
+ Eigen::AngleAxis<double>(DegToRad(k), Vec3s::UnitY()));
tri2Tf.setQuatRotation(
- Eigen::AngleAxis<double>(DegToRad(i), Vec3f::UnitZ()) *
- Eigen::AngleAxis<double>(DegToRad(j), Vec3f::UnitY()));
+ Eigen::AngleAxis<double>(DegToRad(i), Vec3s::UnitZ()) *
+ Eigen::AngleAxis<double>(DegToRad(j), Vec3s::UnitY()));
CollisionResult result;
/// assertion: src/collision_node.cpp:58
diff --git a/test/contact_patch.cpp b/test/contact_patch.cpp
index 0a02bbe9..6db7f106 100644
--- a/test/contact_patch.cpp
+++ b/test/contact_patch.cpp
@@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE(box_box_no_collision) {
Transform3f tf2;
// set translation to separate the shapes
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, 2 * halfside + offset));
+ tf2.setTranslation(Vec3s(0, 0, 2 * halfside + offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -79,7 +79,7 @@ BOOST_AUTO_TEST_CASE(box_sphere) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset));
+ tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -117,7 +117,7 @@ BOOST_AUTO_TEST_CASE(box_box) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset));
+ tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -142,7 +142,7 @@ BOOST_AUTO_TEST_CASE(box_box) {
patch_res2.numContactPatches() > 0 && col_res.isCollision()) {
const Contact& contact = col_res.getContact(0);
const CoalScalar tol = 1e-6;
- EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol);
+ EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol);
const size_t expected_size = 4;
ContactPatch expected(expected_size);
@@ -150,11 +150,11 @@ BOOST_AUTO_TEST_CASE(box_box) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const std::array<Vec3f, 4> corners = {
- Vec3f(halfside, halfside, halfside),
- Vec3f(halfside, -halfside, halfside),
- Vec3f(-halfside, -halfside, halfside),
- Vec3f(-halfside, halfside, halfside),
+ const std::array<Vec3s, 4> corners = {
+ Vec3s(halfside, halfside, halfside),
+ Vec3s(halfside, -halfside, halfside),
+ Vec3s(-halfside, -halfside, halfside),
+ Vec3s(-halfside, halfside, halfside),
};
for (size_t i = 0; i < expected_size; ++i) {
expected.addPoint(corners[i] +
@@ -175,7 +175,7 @@ BOOST_AUTO_TEST_CASE(halfspace_box) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, halfside - offset));
+ tf2.setTranslation(Vec3s(0, 0, halfside - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -201,7 +201,7 @@ BOOST_AUTO_TEST_CASE(halfspace_box) {
const Contact& contact = col_res.getContact(0);
const CoalScalar tol = 1e-6;
EIGEN_VECTOR_IS_APPROX(contact.normal, hspace.n, tol);
- EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3f(0, 0, 1), tol);
+ EIGEN_VECTOR_IS_APPROX(hspace.n, Vec3s(0, 0, 1), tol);
const size_t expected_size = 4;
ContactPatch expected(expected_size);
@@ -209,11 +209,11 @@ BOOST_AUTO_TEST_CASE(halfspace_box) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const std::array<Vec3f, 4> corners = {
- tf2.transform(Vec3f(halfside, halfside, -halfside)),
- tf2.transform(Vec3f(halfside, -halfside, -halfside)),
- tf2.transform(Vec3f(-halfside, -halfside, -halfside)),
- tf2.transform(Vec3f(-halfside, halfside, -halfside)),
+ const std::array<Vec3s, 4> corners = {
+ tf2.transform(Vec3s(halfside, halfside, -halfside)),
+ tf2.transform(Vec3s(halfside, -halfside, -halfside)),
+ tf2.transform(Vec3s(-halfside, -halfside, -halfside)),
+ tf2.transform(Vec3s(-halfside, halfside, -halfside)),
};
for (size_t i = 0; i < expected_size; ++i) {
expected.addPoint(corners[i] -
@@ -235,7 +235,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+ tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -263,7 +263,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const Vec3f capsule_end(0, 0, -capsule.halfLength);
+ const Vec3s capsule_end(0, 0, -capsule.halfLength);
expected.addPoint(tf2.transform(capsule_end));
const ContactPatch& contact_patch = patch_res.getContactPatch(0);
@@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const Vec3f capsule_end(0, 0, capsule.halfLength);
+ const Vec3s capsule_end(0, 0, capsule.halfLength);
expected.addPoint(tf2.transform(capsule_end));
const ContactPatch& contact_patch = patch_res.getContactPatch(0);
@@ -326,8 +326,8 @@ BOOST_AUTO_TEST_CASE(halfspace_capsule) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const Vec3f p1(-capsule.radius, 0, capsule.halfLength);
- const Vec3f p2(-capsule.radius, 0, -capsule.halfLength);
+ const Vec3s p1(-capsule.radius, 0, capsule.halfLength);
+ const Vec3s p2(-capsule.radius, 0, -capsule.halfLength);
expected.addPoint(tf2.transform(p1));
expected.addPoint(tf2.transform(p2));
@@ -347,7 +347,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+ tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -375,12 +375,12 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- std::array<Vec3f, ContactPatch::default_preallocated_size> points;
+ std::array<Vec3s, ContactPatch::default_preallocated_size> points;
const CoalScalar angle_increment =
2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6));
for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) {
const CoalScalar theta = (CoalScalar)(i)*angle_increment;
- Vec3f point_on_cone_base(std::cos(theta) * cone.radius,
+ Vec3s point_on_cone_base(std::cos(theta) * cone.radius,
std::sin(theta) * cone.radius, -cone.halfLength);
expected.addPoint(tf2.transform(point_on_cone_base));
}
@@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const Vec3f cone_tip(0, 0, cone.halfLength);
+ const Vec3s cone_tip(0, 0, cone.halfLength);
expected.addPoint(tf2.transform(cone_tip));
BOOST_CHECK(contact_patch.isSame(expected, tol));
@@ -455,7 +455,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cone) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const Vec3f point_on_circle_basis(-cone.radius, 0, -cone.halfLength);
+ const Vec3s point_on_circle_basis(-cone.radius, 0, -cone.halfLength);
expected.addPoint(tf2.transform(point_on_circle_basis));
BOOST_CHECK(contact_patch.isSame(expected, tol));
@@ -472,7 +472,7 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+ tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -490,12 +490,12 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- std::array<Vec3f, ContactPatch::default_preallocated_size> points;
+ std::array<Vec3s, ContactPatch::default_preallocated_size> points;
const CoalScalar angle_increment =
2.0 * (CoalScalar)(EIGEN_PI) / ((CoalScalar)(6));
for (size_t i = 0; i < ContactPatch::default_preallocated_size; ++i) {
const CoalScalar theta = (CoalScalar)(i)*angle_increment;
- Vec3f point_on_cone_base(std::cos(theta) * cylinder.radius,
+ Vec3s point_on_cone_base(std::cos(theta) * cylinder.radius,
std::sin(theta) * cylinder.radius,
-cylinder.halfLength);
expected.addPoint(tf2.transform(point_on_cone_base));
@@ -563,9 +563,9 @@ BOOST_AUTO_TEST_CASE(halfspace_cylinder) {
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
expected.addPoint(
- tf2.transform(Vec3f(cylinder.radius, 0, cylinder.halfLength)));
+ tf2.transform(Vec3s(cylinder.radius, 0, cylinder.halfLength)));
expected.addPoint(
- tf2.transform(Vec3f(cylinder.radius, 0, -cylinder.halfLength)));
+ tf2.transform(Vec3s(cylinder.radius, 0, -cylinder.halfLength)));
const ContactPatch& contact_patch = patch_res.getContactPatch(0);
BOOST_CHECK(expected.isSame(contact_patch, tol));
@@ -581,7 +581,7 @@ BOOST_AUTO_TEST_CASE(convex_convex) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, 2 * halfside - offset));
+ tf2.setTranslation(Vec3s(0, 0, 2 * halfside - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -606,7 +606,7 @@ BOOST_AUTO_TEST_CASE(convex_convex) {
patch_res2.numContactPatches() > 0 && col_res.isCollision()) {
const Contact& contact = col_res.getContact(0);
const CoalScalar tol = 1e-6;
- EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3f(0, 0, 1), tol);
+ EIGEN_VECTOR_IS_APPROX(contact.normal, Vec3s(0, 0, 1), tol);
const size_t expected_size = 4;
ContactPatch expected(expected_size);
@@ -614,11 +614,11 @@ BOOST_AUTO_TEST_CASE(convex_convex) {
constructOrthonormalBasisFromVector(contact.normal);
expected.tf.translation() = contact.pos;
expected.penetration_depth = contact.penetration_depth;
- const std::array<Vec3f, 4> corners = {
- Vec3f(halfside, halfside, halfside),
- Vec3f(halfside, -halfside, halfside),
- Vec3f(-halfside, -halfside, halfside),
- Vec3f(-halfside, halfside, halfside),
+ const std::array<Vec3s, 4> corners = {
+ Vec3s(halfside, halfside, halfside),
+ Vec3s(halfside, -halfside, halfside),
+ Vec3s(-halfside, -halfside, halfside),
+ Vec3s(-halfside, halfside, halfside),
};
for (size_t i = 0; i < expected_size; ++i) {
expected.addPoint(corners[i] +
@@ -635,8 +635,8 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
// Two tetrahedrons make contact on one of their edge.
const size_t expected_size = 2;
- const Vec3f expected_cp1(0, 0.5, 0);
- const Vec3f expected_cp2(0, 1, 0);
+ const Vec3s expected_cp1(0, 0.5, 0);
+ const Vec3s expected_cp2(0, 1, 0);
const Transform3f tf1; // identity
const Transform3f tf2; // identity
@@ -650,22 +650,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
{
// Case 1 - Face-Face contact
- std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
- Vec3f(-1, 0, 0),
- Vec3f(0, 0, 0),
- Vec3f(0, 1, 0),
- Vec3f(-1, -1, -1),
+ std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+ Vec3s(-1, 0, 0),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 1, 0),
+ Vec3s(-1, -1, -1),
}));
std::shared_ptr<std::vector<Triangle>> tris1(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
Triangle(0, 3, 1), Triangle(2, 1, 3)}));
Convex<Triangle> tetra1(pts1, 4, tris1, 4);
- std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
- Vec3f(0, 0.5, 0),
- Vec3f(0, 1.5, 0),
- Vec3f(1, 0.5, 0),
- Vec3f(1, 1, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+ Vec3s(0, 0.5, 0),
+ Vec3s(0, 1.5, 0),
+ Vec3s(1, 0.5, 0),
+ Vec3s(1, 1, 1),
}));
std::shared_ptr<std::vector<Triangle>> tris2(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -701,22 +701,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
{
// Case 2 - Face-Segment contact
- std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
- Vec3f(-1, 0, -0.2),
- Vec3f(0, 0, 0),
- Vec3f(0, 1, 0),
- Vec3f(-1, -1, -1),
+ std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+ Vec3s(-1, 0, -0.2),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 1, 0),
+ Vec3s(-1, -1, -1),
}));
std::shared_ptr<std::vector<Triangle>> tris1(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
Triangle(0, 3, 1), Triangle(2, 1, 3)}));
Convex<Triangle> tetra1(pts1, 4, tris1, 4);
- std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
- Vec3f(0, 0.5, 0),
- Vec3f(0, 1.5, 0),
- Vec3f(1, 0.5, 0),
- Vec3f(1, 1, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+ Vec3s(0, 0.5, 0),
+ Vec3s(0, 1.5, 0),
+ Vec3s(1, 0.5, 0),
+ Vec3s(1, 1, 1),
}));
std::shared_ptr<std::vector<Triangle>> tris2(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -750,22 +750,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_segment) {
{
// Case 3 - Segment-Segment contact
- std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
- Vec3f(-1, 0, -0.2),
- Vec3f(0, 0, 0),
- Vec3f(0, 1, 0),
- Vec3f(-1, -1, -1),
+ std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+ Vec3s(-1, 0, -0.2),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 1, 0),
+ Vec3s(-1, -1, -1),
}));
std::shared_ptr<std::vector<Triangle>> tris1(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
Triangle(0, 3, 1), Triangle(2, 1, 3)}));
Convex<Triangle> tetra1(pts1, 4, tris1, 4);
- std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
- Vec3f(0, 0.5, 0),
- Vec3f(0, 1.5, 0),
- Vec3f(1, 0.5, 0.5),
- Vec3f(1, 1, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+ Vec3s(0, 0.5, 0),
+ Vec3s(0, 1.5, 0),
+ Vec3s(1, 0.5, 0.5),
+ Vec3s(1, 1, 1),
}));
std::shared_ptr<std::vector<Triangle>> tris2(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -802,7 +802,7 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
// This case covers the vertex-vertex edge case of contact patches.
// Two tetrahedrons make contact on one of their vertex.
const size_t expected_size = 1;
- const Vec3f expected_cp(0, 0, 0);
+ const Vec3s expected_cp(0, 0, 0);
const Transform3f tf1; // identity
const Transform3f tf2; // identity
@@ -816,22 +816,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
{
// Case 1 - Face-Face contact
- std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
- Vec3f(-1, 0, 0),
- Vec3f(0, 0, 0),
- Vec3f(0, 1, 0),
- Vec3f(-1, -1, -1),
+ std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+ Vec3s(-1, 0, 0),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 1, 0),
+ Vec3s(-1, -1, -1),
}));
std::shared_ptr<std::vector<Triangle>> tris1(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
Triangle(0, 3, 1), Triangle(2, 1, 3)}));
Convex<Triangle> tetra1(pts1, 4, tris1, 4);
- std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
- Vec3f(1, 0, 0),
- Vec3f(0, 0, 0),
- Vec3f(0, -1, 0),
- Vec3f(1, 1, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+ Vec3s(1, 0, 0),
+ Vec3s(0, 0, 0),
+ Vec3s(0, -1, 0),
+ Vec3s(1, 1, 1),
}));
std::shared_ptr<std::vector<Triangle>> tris2(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -864,22 +864,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
{
// Case 2 - Segment-Face contact
- std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
- Vec3f(-1, 0, -0.5),
- Vec3f(0, 0, 0),
- Vec3f(0, 1, 0),
- Vec3f(-1, -1, -1),
+ std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+ Vec3s(-1, 0, -0.5),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 1, 0),
+ Vec3s(-1, -1, -1),
}));
std::shared_ptr<std::vector<Triangle>> tris1(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
Triangle(0, 3, 1), Triangle(2, 1, 3)}));
Convex<Triangle> tetra1(pts1, 4, tris1, 4);
- std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
- Vec3f(1, 0, 0),
- Vec3f(0, 0, 0),
- Vec3f(0, -1, 0),
- Vec3f(1, 1, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+ Vec3s(1, 0, 0),
+ Vec3s(0, 0, 0),
+ Vec3s(0, -1, 0),
+ Vec3s(1, 1, 1),
}));
std::shared_ptr<std::vector<Triangle>> tris2(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -912,22 +912,22 @@ BOOST_AUTO_TEST_CASE(edge_case_vertex_vertex) {
{
// Case 2 - Segment-Segment contact
- std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
- Vec3f(-1, 0, -0.2),
- Vec3f(0, 0, 0),
- Vec3f(0, 1, 0),
- Vec3f(-1, -1, -1),
+ std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+ Vec3s(-1, 0, -0.2),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 1, 0),
+ Vec3s(-1, -1, -1),
}));
std::shared_ptr<std::vector<Triangle>> tris1(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
Triangle(0, 3, 1), Triangle(2, 1, 3)}));
Convex<Triangle> tetra1(pts1, 4, tris1, 4);
- std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
- Vec3f(1, 0, 0),
- Vec3f(0, 0, 0),
- Vec3f(0, -1, 0.5),
- Vec3f(1, 1, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+ Vec3s(1, 0, 0),
+ Vec3s(0, 0, 0),
+ Vec3s(0, -1, 0.5),
+ Vec3s(1, 1, 1),
}));
std::shared_ptr<std::vector<Triangle>> tris2(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
@@ -963,8 +963,8 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) {
// This case covers the segment-face edge case of contact patches.
// Two tetrahedrons make contact on one of their segment/face respectively.
const size_t expected_size = 2;
- const Vec3f expected_cp1(0, 0, 0);
- const Vec3f expected_cp2(-0.5, 0.5, 0);
+ const Vec3s expected_cp1(0, 0, 0);
+ const Vec3s expected_cp2(-0.5, 0.5, 0);
const Transform3f tf1; // identity
const Transform3f tf2; // identity
@@ -977,22 +977,22 @@ BOOST_AUTO_TEST_CASE(edge_case_segment_face) {
ContactPatchResult patch_res(patch_req);
{
- std::shared_ptr<std::vector<Vec3f>> pts1(new std::vector<Vec3f>({
- Vec3f(-1, 0, -0),
- Vec3f(0, 0, 0),
- Vec3f(0, 1, 0),
- Vec3f(-1, -1, -1),
+ std::shared_ptr<std::vector<Vec3s>> pts1(new std::vector<Vec3s>({
+ Vec3s(-1, 0, -0),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 1, 0),
+ Vec3s(-1, -1, -1),
}));
std::shared_ptr<std::vector<Triangle>> tris1(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
Triangle(0, 3, 1), Triangle(2, 1, 3)}));
Convex<Triangle> tetra1(pts1, 4, tris1, 4);
- std::shared_ptr<std::vector<Vec3f>> pts2(new std::vector<Vec3f>({
- Vec3f(-0.5, 0.5, 0),
- Vec3f(0.5, -0.5, 0),
- Vec3f(1, 0.5, 0.5),
- Vec3f(1, 1, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts2(new std::vector<Vec3s>({
+ Vec3s(-0.5, 0.5, 0),
+ Vec3s(0.5, -0.5, 0),
+ Vec3s(1, 0.5, 0.5),
+ Vec3s(1, 1, 1),
}));
std::shared_ptr<std::vector<Triangle>> tris2(
new std::vector<Triangle>({Triangle(0, 1, 2), Triangle(0, 2, 3),
diff --git a/test/convex.cpp b/test/convex.cpp
index bc48ca8d..02507b26 100644
--- a/test/convex.cpp
+++ b/test/convex.cpp
@@ -157,11 +157,11 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) {
Transform3f tf1;
Transform3f tf2;
- tf2.setTranslation(Vec3f(3, 0, 0));
+ tf2.setTranslation(Vec3s(3, 0, 0));
compareShapeIntersection(box, convex_box, tf1, tf2, eps);
compareShapeDistance(box, convex_box, tf1, tf2, eps);
- tf2.setTranslation(Vec3f(0, 0, 0));
+ tf2.setTranslation(Vec3s(0, 0, 0));
compareShapeIntersection(box, convex_box, tf1, tf2, eps);
compareShapeDistance(box, convex_box, tf1, tf2, eps);
@@ -174,8 +174,8 @@ BOOST_AUTO_TEST_CASE(compare_convex_box) {
#ifdef COAL_HAS_QHULL
BOOST_AUTO_TEST_CASE(convex_hull_throw) {
- std::shared_ptr<std::vector<Vec3f>> points(
- new std::vector<Vec3f>({Vec3f(1, 1, 1), Vec3f(0, 0, 0), Vec3f(1, 0, 0)}));
+ std::shared_ptr<std::vector<Vec3s>> points(
+ new std::vector<Vec3s>({Vec3s(1, 1, 1), Vec3s(0, 0, 0), Vec3s(1, 0, 0)}));
BOOST_CHECK_THROW(ConvexBase::convexHull(points, 0, false, NULL),
std::invalid_argument);
@@ -188,11 +188,11 @@ BOOST_AUTO_TEST_CASE(convex_hull_throw) {
}
BOOST_AUTO_TEST_CASE(convex_hull_quad) {
- std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
- Vec3f(1, 1, 1),
- Vec3f(0, 0, 0),
- Vec3f(1, 0, 0),
- Vec3f(0, 0, 1),
+ std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+ Vec3s(1, 1, 1),
+ Vec3s(0, 0, 0),
+ Vec3s(1, 0, 0),
+ Vec3s(0, 0, 1),
}));
ConvexBase* convexHull = ConvexBase::convexHull(points, 4, false, NULL);
@@ -205,26 +205,26 @@ BOOST_AUTO_TEST_CASE(convex_hull_quad) {
}
BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
- std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
- Vec3f(1, 1, 1),
- Vec3f(1, 1, -1),
- Vec3f(1, -1, 1),
- Vec3f(1, -1, -1),
- Vec3f(-1, 1, 1),
- Vec3f(-1, 1, -1),
- Vec3f(-1, -1, 1),
- Vec3f(-1, -1, -1),
- Vec3f(0, 0, 0),
- Vec3f(0, 0, 0.99),
+ std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+ Vec3s(1, 1, 1),
+ Vec3s(1, 1, -1),
+ Vec3s(1, -1, 1),
+ Vec3s(1, -1, -1),
+ Vec3s(-1, 1, 1),
+ Vec3s(-1, 1, -1),
+ Vec3s(-1, -1, 1),
+ Vec3s(-1, -1, -1),
+ Vec3s(0, 0, 0),
+ Vec3s(0, 0, 0.99),
}));
ConvexBase* convexHull = ConvexBase::convexHull(points, 9, false, NULL);
BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
{
- const std::vector<Vec3f>& cvxhull_points = *(convexHull->points);
+ const std::vector<Vec3s>& cvxhull_points = *(convexHull->points);
for (size_t i = 0; i < 8; ++i) {
- BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1));
+ BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1));
BOOST_CHECK_EQUAL((*(convexHull->neighbors))[i].count(), 3);
}
}
@@ -236,9 +236,9 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
BOOST_REQUIRE_EQUAL(8, convexHull->num_points);
{
- const std::vector<Vec3f>& cvxhull_points = *(convexHull->points);
+ const std::vector<Vec3s>& cvxhull_points = *(convexHull->points);
for (size_t i = 0; i < 8; ++i) {
- BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3f(1, 1, 1));
+ BOOST_CHECK(cvxhull_points[i].cwiseAbs() == Vec3s(1, 1, 1));
BOOST_CHECK((*(convexHull->neighbors))[i].count() >= 3);
BOOST_CHECK((*(convexHull->neighbors))[i].count() <= 6);
}
@@ -249,16 +249,16 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) {
BOOST_AUTO_TEST_CASE(convex_copy_constructor) {
Convex<Triangle>* convexHullTriCopy;
{
- std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
- Vec3f(1, 1, 1),
- Vec3f(1, 1, -1),
- Vec3f(1, -1, 1),
- Vec3f(1, -1, -1),
- Vec3f(-1, 1, 1),
- Vec3f(-1, 1, -1),
- Vec3f(-1, -1, 1),
- Vec3f(-1, -1, -1),
- Vec3f(0, 0, 0),
+ std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+ Vec3s(1, 1, 1),
+ Vec3s(1, 1, -1),
+ Vec3s(1, -1, 1),
+ Vec3s(1, -1, -1),
+ Vec3s(-1, 1, 1),
+ Vec3s(-1, 1, -1),
+ Vec3s(-1, -1, 1),
+ Vec3s(-1, -1, -1),
+ Vec3s(0, 0, 0),
}));
Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
@@ -272,16 +272,16 @@ BOOST_AUTO_TEST_CASE(convex_copy_constructor) {
}
BOOST_AUTO_TEST_CASE(convex_clone) {
- std::shared_ptr<std::vector<Vec3f>> points(new std::vector<Vec3f>({
- Vec3f(1, 1, 1),
- Vec3f(1, 1, -1),
- Vec3f(1, -1, 1),
- Vec3f(1, -1, -1),
- Vec3f(-1, 1, 1),
- Vec3f(-1, 1, -1),
- Vec3f(-1, -1, 1),
- Vec3f(-1, -1, -1),
- Vec3f(0, 0, 0),
+ std::shared_ptr<std::vector<Vec3s>> points(new std::vector<Vec3s>({
+ Vec3s(1, 1, 1),
+ Vec3s(1, 1, -1),
+ Vec3s(1, -1, 1),
+ Vec3s(1, -1, -1),
+ Vec3s(-1, 1, 1),
+ Vec3s(-1, 1, -1),
+ Vec3s(-1, -1, 1),
+ Vec3s(-1, -1, -1),
+ Vec3s(0, 0, 0),
}));
Convex<Triangle>* convexHullTri = dynamic_cast<Convex<Triangle>*>(
diff --git a/test/distance.cpp b/test/distance.cpp
index 603d57ba..77a35f60 100644
--- a/test/distance.cpp
+++ b/test/distance.cpp
@@ -56,30 +56,30 @@ bool verbose = false;
CoalScalar DELTA = 0.001;
template <typename BV>
-void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
+void distance_Test(const Transform3f& tf, const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, unsigned int qsize,
DistanceRes& distance_result, bool verbose = true);
bool collide_Test_OBB(const Transform3f& tf,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, bool verbose);
template <typename BV, typename TraversalNode>
void distance_Test_Oriented(const Transform3f& tf,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, unsigned int qsize,
DistanceRes& distance_result, bool verbose = true);
-inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) {
+inline bool nearlyEqual(const Vec3s& a, const Vec3s& b) {
if (fabs(a[0] - b[0]) > DELTA) return false;
if (fabs(a[1] - b[1]) > DELTA) return false;
if (fabs(a[2] - b[2]) > DELTA) return false;
@@ -87,7 +87,7 @@ inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) {
}
BOOST_AUTO_TEST_CASE(mesh_distance) {
- std::vector<Vec3f> p1, p2;
+ std::vector<Vec3s> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
@@ -469,9 +469,9 @@ BOOST_AUTO_TEST_CASE(mesh_distance) {
template <typename BV, typename TraversalNode>
void distance_Test_Oriented(const Transform3f& tf,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, unsigned int qsize,
DistanceRes& distance_result, bool verbose) {
@@ -499,8 +499,8 @@ void distance_Test_Oriented(const Transform3f& tf,
distance(&node, NULL, qsize);
// points are in local coordinate, to global coordinate
- Vec3f p1 = local_result.nearest_points[0];
- Vec3f p2 = local_result.nearest_points[1];
+ Vec3s p1 = local_result.nearest_points[0];
+ Vec3s p2 = local_result.nearest_points[1];
distance_result.distance = local_result.min_distance;
distance_result.p1 = p1;
@@ -516,9 +516,9 @@ void distance_Test_Oriented(const Transform3f& tf,
}
template <typename BV>
-void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
+void distance_Test(const Transform3f& tf, const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, unsigned int qsize,
DistanceRes& distance_result, bool verbose) {
@@ -566,9 +566,9 @@ void distance_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
}
bool collide_Test_OBB(const Transform3f& tf,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, bool verbose) {
BVHModel<OBB> m1;
diff --git a/test/distance_lower_bound.cpp b/test/distance_lower_bound.cpp
index 7c034f69..21c2f3ee 100644
--- a/test/distance_lower_bound.cpp
+++ b/test/distance_lower_bound.cpp
@@ -58,7 +58,7 @@ using coal::OBBRSS;
using coal::shared_ptr;
using coal::Transform3f;
using coal::Triangle;
-using coal::Vec3f;
+using coal::Vec3s;
bool testDistanceLowerBound(const Transform3f& tf,
const CollisionGeometryPtr_t& m1,
@@ -113,7 +113,7 @@ bool testDistance(const Transform3f& tf, const CollisionGeometryPtr_t& m1,
}
BOOST_AUTO_TEST_CASE(mesh_mesh) {
- std::vector<Vec3f> p1, p2;
+ std::vector<Vec3s> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
@@ -232,7 +232,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
}
BOOST_AUTO_TEST_CASE(box_mesh) {
- std::vector<Vec3f> p1;
+ std::vector<Vec3s> p1;
std::vector<Triangle> t1;
boost::filesystem::path path(TEST_RESOURCES_DIR);
diff --git a/test/frontlist.cpp b/test/frontlist.cpp
index 2ed4dd76..df1e6e6d 100644
--- a/test/frontlist.cpp
+++ b/test/frontlist.cpp
@@ -52,9 +52,9 @@ namespace utf = boost::unit_test::framework;
template <typename BV>
bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, bool refit_bottomup,
bool verbose);
@@ -62,23 +62,23 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
template <typename BV, typename TraversalNode>
bool collide_front_list_Test_Oriented(const Transform3f& tf1,
const Transform3f& tf2,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method,
bool verbose);
template <typename BV>
-bool collide_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
+bool collide_Test(const Transform3f& tf, const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, bool verbose);
// TODO: randomly still have some runtime error
BOOST_AUTO_TEST_CASE(front_list) {
- std::vector<Vec3f> p1, p2;
+ std::vector<Vec3s> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
@@ -271,9 +271,9 @@ BOOST_AUTO_TEST_CASE(front_list) {
template <typename BV>
bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, bool refit_bottomup,
bool verbose) {
@@ -284,7 +284,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
BVHFrontList front_list;
- std::vector<Vec3f> vertices1_new(vertices1.size());
+ std::vector<Vec3s> vertices1_new(vertices1.size());
for (std::size_t i = 0; i < vertices1_new.size(); ++i) {
vertices1_new[i] = tf1.transform(vertices1[i]);
}
@@ -338,9 +338,9 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
template <typename BV, typename TraversalNode>
bool collide_front_list_Test_Oriented(const Transform3f& tf1,
const Transform3f& tf2,
- const std::vector<Vec3f>& vertices1,
+ const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method,
bool verbose) {
@@ -392,9 +392,9 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1,
}
template <typename BV>
-bool collide_Test(const Transform3f& tf, const std::vector<Vec3f>& vertices1,
+bool collide_Test(const Transform3f& tf, const std::vector<Vec3s>& vertices1,
const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2,
+ const std::vector<Vec3s>& vertices2,
const std::vector<Triangle>& triangles2,
SplitMethodType split_method, bool verbose) {
BVHModel<BV> m1;
diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp
index c391e6a2..ad89819c 100644
--- a/test/geometric_shapes.cpp
+++ b/test/geometric_shapes.cpp
@@ -80,8 +80,8 @@ template <typename S1, typename S2>
void printComparisonError(const std::string& comparison_type, const S1& s1,
const Transform3f& tf1, const S2& s2,
const Transform3f& tf2,
- const Vec3f& contact_or_normal,
- const Vec3f& expected_contact_or_normal,
+ const Vec3s& contact_or_normal,
+ const Vec3s& expected_contact_or_normal,
bool check_opposite_normal, CoalScalar tol) {
std::cout << "Disagreement between " << comparison_type << " and expected_"
<< comparison_type << " for " << getNodeTypeName(s1.getNodeType())
@@ -127,10 +127,10 @@ void printComparisonError(const std::string& comparison_type, const S1& s1,
template <typename S1, typename S2>
void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2,
- const Transform3f& tf2, const Vec3f& contact,
- Vec3f* expected_point, CoalScalar depth,
- CoalScalar* expected_depth, const Vec3f& normal,
- Vec3f* expected_normal, bool check_opposite_normal,
+ const Transform3f& tf2, const Vec3s& contact,
+ Vec3s* expected_point, CoalScalar depth,
+ CoalScalar* expected_depth, const Vec3s& normal,
+ Vec3s* expected_normal, bool check_opposite_normal,
CoalScalar tol) {
if (expected_point) {
bool contact_equal = isEqual(contact, *expected_point, tol);
@@ -164,16 +164,16 @@ void compareContact(const S1& s1, const Transform3f& tf1, const S2& s2,
template <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,
+ Vec3s* expected_point = NULL,
CoalScalar* expected_depth = NULL,
- Vec3f* expected_normal = NULL,
+ Vec3s* expected_normal = NULL,
bool check_opposite_normal = false,
CoalScalar tol = 1e-9) {
CollisionRequest request;
CollisionResult result;
- Vec3f contact;
- Vec3f normal; // normal direction should be from object 1 to object 2
+ Vec3s contact;
+ Vec3s normal; // normal direction should be from object 1 to object 2
bool collision;
bool check_failed = false;
@@ -244,46 +244,46 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) {
Transform3f transform;
generateRandomTransform(extents, transform);
- // Vec3f point;
+ // Vec3s point;
// CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(40, 0, 0));
+ tf2 = Transform3f(Vec3s(40, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(40, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(30, 0, 0));
+ tf2 = Transform3f(Vec3s(30, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(30.01, 0, 0));
+ tf2 = Transform3f(Vec3s(30.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(30.01, 0, 0));
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(29.9, 0, 0));
+ tf2 = Transform3f(Vec3s(29.9, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(29.9, 0, 0));
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
@@ -302,45 +302,45 @@ BOOST_AUTO_TEST_CASE(collide_spheresphere) {
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-29.9, 0, 0));
+ tf2 = Transform3f(Vec3s(-29.9, 0, 0));
normal << -1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0));
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(-29.9, 0, 0));
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-30.0, 0, 0));
+ tf2 = Transform3f(Vec3s(-30.0, 0, 0));
normal << -1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-30.01, 0, 0));
+ tf2 = Transform3f(Vec3s(-30.01, 0, 0));
normal << -1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-30.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
}
-bool compareContactPoints(const Vec3f& c1, const Vec3f& c2) {
+bool compareContactPoints(const Vec3s& c1, const Vec3s& c2) {
return c1[2] < c2[2];
} // Ascending order
-void testBoxBoxContactPoints(const Matrix3f& R) {
+void testBoxBoxContactPoints(const Matrix3s& R) {
Box s1(100, 100, 100);
Box s2(10, 20, 30);
// Vertices of s2
- std::vector<Vec3f> vertices(8);
+ std::vector<Vec3s> vertices(8);
vertices[0] << 1, 1, 1;
vertices[1] << 1, 1, -1;
vertices[2] << 1, -1, 1;
@@ -354,11 +354,11 @@ void testBoxBoxContactPoints(const Matrix3f& R) {
vertices[i].array() *= s2.halfSide.array();
}
- Transform3f tf1 = Transform3f(Vec3f(0, 0, -50));
+ Transform3f tf1 = Transform3f(Vec3s(0, 0, -50));
Transform3f tf2 = Transform3f(R);
- Vec3f normal;
- Vec3f p1, p2;
+ Vec3s normal;
+ Vec3s p1, p2;
// Make sure the two boxes are colliding
solver1.gjk_tolerance = 1e-5;
@@ -375,8 +375,8 @@ void testBoxBoxContactPoints(const Matrix3f& R) {
std::sort(vertices.begin(), vertices.end(), compareContactPoints);
// The lowest vertex along z-axis should be the contact point
- FCL_CHECK(normal.isApprox(Vec3f(0, 0, 1), 1e-6));
- Vec3f point = 0.5 * (p1 + p2);
+ FCL_CHECK(normal.isApprox(Vec3s(0, 0, 1), 1e-6));
+ Vec3s point = 0.5 * (p1 + p2);
FCL_CHECK(vertices[0].head<2>().isApprox(point.head<2>(), 1e-6));
FCL_CHECK(vertices[0][2] <= point[2] && point[2] < 0);
}
@@ -391,9 +391,9 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) {
Transform3f transform;
generateRandomTransform(extents, transform);
- // Vec3f point;
+ // Vec3s point;
// CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
Quatf q;
q = AngleAxis((CoalScalar)3.140 / 6, UnitZ);
@@ -410,23 +410,23 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) {
tf2 = transform;
// TODO: Need convention for normal when the centers of two objects are at
// same position. The current result is (1, 0, 0).
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(15, 0, 0));
+ tf2 = Transform3f(Vec3s(15, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 1e-8);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(15.01, 0, 0));
+ tf2 = Transform3f(Vec3s(15.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(15.01, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(15.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
@@ -438,7 +438,7 @@ BOOST_AUTO_TEST_CASE(collide_boxbox) {
tf1 = transform;
tf2 = transform * Transform3f(q);
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, 0x0);
@@ -461,9 +461,9 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) {
Transform3f transform;
generateRandomTransform(extents, transform);
- // Vec3f point;
+ // Vec3s point;
// CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -481,36 +481,36 @@ BOOST_AUTO_TEST_CASE(collide_spherebox) {
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(22.50001, 0, 0));
+ tf2 = Transform3f(Vec3s(22.50001, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(22.501, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(22.501, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(22.4, 0, 0));
+ tf2 = Transform3f(Vec3s(22.4, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(22.4, 0, 0));
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(22.4, 0, 0));
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
}
BOOST_AUTO_TEST_CASE(distance_spherebox) {
- coal::Matrix3f rotSphere;
+ coal::Matrix3s rotSphere;
rotSphere << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
- coal::Vec3f trSphere(0.0, 0.0, 0.0);
+ coal::Vec3s trSphere(0.0, 0.0, 0.0);
- coal::Matrix3f rotBox;
+ coal::Matrix3s rotBox;
rotBox << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
- coal::Vec3f trBox(0.0, 5.0, 3.0);
+ coal::Vec3s trBox(0.0, 5.0, 3.0);
coal::Sphere sphere(1);
coal::Box box(10, 2, 10);
@@ -521,9 +521,9 @@ BOOST_AUTO_TEST_CASE(distance_spherebox) {
CoalScalar eps = Eigen::NumTraits<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);
- EIGEN_VECTOR_IS_APPROX(result.normal, Vec3f(0, 1, 0), eps);
+ EIGEN_VECTOR_IS_APPROX(result.nearest_points[0], Vec3s(0, 1, 0), eps);
+ EIGEN_VECTOR_IS_APPROX(result.nearest_points[1], Vec3s(0, 4, 0), eps);
+ EIGEN_VECTOR_IS_APPROX(result.normal, Vec3s(0, 1, 0), eps);
}
BOOST_AUTO_TEST_CASE(collide_spherecapsule) {
@@ -536,9 +536,9 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) {
Transform3f transform;
generateRandomTransform(extents, transform);
- // Vec3f point;
+ // Vec3s point;
// CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -555,38 +555,38 @@ BOOST_AUTO_TEST_CASE(collide_spherecapsule) {
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(24.9, 0, 0));
+ tf2 = Transform3f(Vec3s(24.9, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(24.9, 0, 0));
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(24.9, 0, 0));
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(25, 0, 0));
+ tf2 = Transform3f(Vec3s(25, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(24.999999, 0, 0));
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(24.999999, 0, 0));
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(25.1, 0, 0));
+ tf2 = Transform3f(Vec3s(25.1, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(25.1, 0, 0));
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(25.1, 0, 0));
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
}
@@ -601,9 +601,9 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) {
Transform3f transform;
generateRandomTransform(extents, transform);
- // Vec3f point;
+ // Vec3s point;
// CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -620,36 +620,36 @@ BOOST_AUTO_TEST_CASE(collide_cylindercylinder) {
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ tf2 = Transform3f(Vec3s(9.9, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 9.9, 0));
+ tf2 = Transform3f(Vec3s(0, 9.9, 0));
normal << 0, 1, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ tf2 = Transform3f(Vec3s(9.9, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ tf2 = transform * Transform3f(Vec3s(9.9, 0, 0));
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(10.01, 0, 0));
+ tf2 = Transform3f(Vec3s(10.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(10.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
}
@@ -664,9 +664,9 @@ BOOST_AUTO_TEST_CASE(collide_conecone) {
Transform3f transform;
generateRandomTransform(extents, transform);
- // Vec3f point;
+ // Vec3s point;
// CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -684,8 +684,8 @@ BOOST_AUTO_TEST_CASE(collide_conecone) {
tf1 = Transform3f();
// z=0 is a singular points. Two normals could be returned.
- tf2 = Transform3f(Vec3f(9.9, 0, 0.00001));
- normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
+ tf2 = Transform3f(Vec3s(9.9, 0, 0.00001));
+ normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
.normalized();
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
@@ -697,37 +697,37 @@ BOOST_AUTO_TEST_CASE(collide_conecone) {
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.00001));
- normal = Vec3f(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
+ tf2 = transform * Transform3f(Vec3s(9.9, 0, 0.00001));
+ normal = Vec3s(2 * (s1.halfLength + s2.halfLength), 0, s1.radius + s2.radius)
.normalized();
normal = transform.getRotation() * normal;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, true, tol_gjk);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(10.1, 0, 0));
+ tf2 = Transform3f(Vec3s(10.1, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(10.001, 0, 0));
+ tf2 = Transform3f(Vec3s(10.001, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(10.001, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(10.001, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 9.9));
+ tf2 = Transform3f(Vec3s(0, 0, 9.9));
normal << 0, 0, 1;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
- normal = transform.getRotation() * Vec3f(0, 0, 1);
+ tf2 = transform * Transform3f(Vec3s(0, 0, 9.9));
+ normal = transform.getRotation() * Vec3s(0, 0, 1);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
}
@@ -742,9 +742,9 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) {
Transform3f transform;
generateRandomTransform(extents, transform);
- // Vec3f point;
+ // Vec3s point;
// CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -761,81 +761,81 @@ BOOST_AUTO_TEST_CASE(collide_conecylinder) {
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(9.9, 0, 0));
+ tf2 = Transform3f(Vec3s(9.9, 0, 0));
normal =
- Vec3f(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius))
+ Vec3s(2 * (s1.halfLength + s2.halfLength), 0, -(s1.radius + s2.radius))
.normalized();
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(9.9, 0, 0));
normal = transform.getRotation() * normal;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(9.9, 0, 0.1));
+ tf2 = Transform3f(Vec3s(9.9, 0, 0.1));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(9.9, 0, 0.1));
+ tf2 = transform * Transform3f(Vec3s(9.9, 0, 0.1));
normal = transform.getRotation() * normal;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(10.01, 0, 0));
+ tf2 = Transform3f(Vec3s(10.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(10.01, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(10, 0, 0));
+ tf2 = Transform3f(Vec3s(10, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(10, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(10, 0, 0));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, NULL);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 9.9));
+ tf2 = Transform3f(Vec3s(0, 0, 9.9));
normal << 0, 0, 1;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 9.9));
- normal = transform.getRotation() * Vec3f(0, 0, 1);
+ tf2 = transform * Transform3f(Vec3s(0, 0, 9.9));
+ normal = transform.getRotation() * Vec3s(0, 0, 1);
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 10.01));
+ tf2 = Transform3f(Vec3s(0, 0, 10.01));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 10.01));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 10.01));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 10));
+ tf2 = Transform3f(Vec3s(0, 0, 10));
normal << 0, 0, 1;
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s1, tf1, s2, tf2, false);
}
@@ -846,20 +846,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f normal;
+ Vec3s normal;
//
// Testing collision x, y, z
//
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 20, 0, 0;
t[1] << -20, 0, 0;
t[2] << 0, 20, 0;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f(); // identity
- tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, 0.001));
normal << 0, 0, 1;
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -868,7 +868,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, -0.001));
normal << 0, 0, -1;
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -879,14 +879,14 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 30, 0, 0;
t[1] << 9.9, -20, 0;
t[2] << 9.9, 20, 0;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, 0.001));
normal << 9.9, 0, 0.001;
normal.normalize();
SET_LINE;
@@ -896,7 +896,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, -0.001));
normal << 9.9, 0, -0.001;
normal.normalize();
SET_LINE;
@@ -908,14 +908,14 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 30, 0, 0;
t[1] << -20, 0, 0;
t[2] << 0, 0, 20;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(0, 0.001, 0));
+ tf_tri.setTranslation(Vec3s(0, 0.001, 0));
normal << 0, 1, 0;
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -924,7 +924,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, -0.001, 0));
+ tf_tri.setTranslation(Vec3s(0, -0.001, 0));
normal << 0, -1, 0;
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -935,14 +935,14 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 0, 30, 0;
t[1] << 0, -10, 0;
t[2] << 0, 0, 20;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(0.001, 0, 0));
+ tf_tri.setTranslation(Vec3s(0.001, 0, 0));
normal << 1, 0, 0;
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -951,7 +951,7 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
testShapeCollide(s, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(-0.001, 0, 0));
+ tf_tri.setTranslation(Vec3s(-0.001, 0, 0));
normal << -1, 0, 0;
testShapeCollide(s, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
SET_LINE;
@@ -965,20 +965,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
// Testing no collision x, y, z
//
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 20, 0, 0;
t[1] << -20, 0, 0;
t[2] << 0, 20, 0;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(0, 0, 10.1));
+ tf_tri.setTranslation(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(s, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(0, 0, -10.1));
+ tf_tri.setTranslation(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -986,20 +986,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 20, 0, 0;
t[1] << -20, 0, 0;
t[2] << 0, 0, 20;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(0, 10.1, 0));
+ tf_tri.setTranslation(Vec3s(0, 10.1, 0));
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(s, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(0, -10.1, 0));
+ tf_tri.setTranslation(Vec3s(0, -10.1, 0));
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1007,20 +1007,20 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 0, 20, 0;
t[1] << 0, -20, 0;
t[2] << 0, 0, 20;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(10.1, 0, 0));
+ tf_tri.setTranslation(Vec3s(10.1, 0, 0));
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(s, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(-10.1, 0, 0));
+ tf_tri.setTranslation(Vec3s(-10.1, 0, 0));
SET_LINE;
testShapeCollide(s, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1029,24 +1029,24 @@ BOOST_AUTO_TEST_CASE(collide_spheretriangle) {
}
BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) {
- Halfspace hs(Vec3f(0, 0, 1), 0);
+ Halfspace hs(Vec3s(0, 0, 1), 0);
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f normal;
+ Vec3s normal;
normal = hs.n; // with halfspaces, it's simple: normal is always
// the normal of the halfspace.
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 20, 0, 0;
t[1] << -20, 0, 0;
t[2] << 0, 20, 0;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f(); // identity
- tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, -0.001));
normal = hs.n;
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -1055,19 +1055,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) {
testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(hs, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(1, 1, 0.001));
+ tf_tri.setTranslation(Vec3s(1, 1, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(hs, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(-1, -1, 0.001));
+ tf_tri.setTranslation(Vec3s(-1, -1, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1075,14 +1075,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 30, 0, 0;
t[1] << -20, 0, 0;
t[2] << 0, 0, 20;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, -0.001));
normal = hs.n;
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -1091,19 +1091,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) {
testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(hs, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(1, 1, 0.001));
+ tf_tri.setTranslation(Vec3s(1, 1, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(hs, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(-1, -1, 0.001));
+ tf_tri.setTranslation(Vec3s(-1, -1, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1111,14 +1111,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 0, 30, 0;
t[1] << 0, -10, 0;
t[2] << 0, 0, 20;
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f();
- tf_tri.setTranslation(Vec3f(0, 0, -0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, -0.001));
normal = hs.n;
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -1127,19 +1127,19 @@ BOOST_AUTO_TEST_CASE(collide_halfspacetriangle) {
testShapeCollide(hs, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0, 0.001));
+ tf_tri.setTranslation(Vec3s(0, 0, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(hs, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(1, 1, 0.001));
+ tf_tri.setTranslation(Vec3s(1, 1, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(hs, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(-1, -1, 0.001));
+ tf_tri.setTranslation(Vec3s(-1, -1, 0.001));
SET_LINE;
testShapeCollide(hs, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1151,14 +1151,14 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f normal;
+ Vec3s normal;
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 20, 0, 0.05;
t[1] << -20, 0, 0.05;
t[2] << 0, 20, -0.1;
- Plane pl(Vec3f(0, 0, 1), 0);
+ Plane pl(Vec3s(0, 0, 1), 0);
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f(); // identity
@@ -1170,7 +1170,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0, 0.05));
+ tf_tri.setTranslation(Vec3s(0, 0, 0.05));
normal = pl.n;
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -1179,13 +1179,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0, -0.06));
+ tf_tri.setTranslation(Vec3s(0, 0, -0.06));
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(pl, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(0, 0, 0.11));
+ tf_tri.setTranslation(Vec3s(0, 0, 0.11));
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1193,11 +1193,11 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 30, 0.05, 0;
t[1] << -20, 0.05, 0;
t[2] << 0, -0.1, 20;
- Plane pl(Vec3f(0, 1, 0), 0);
+ Plane pl(Vec3s(0, 1, 0), 0);
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f(); // identity
@@ -1209,7 +1209,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, 0.05, 0));
+ tf_tri.setTranslation(Vec3s(0, 0.05, 0));
normal = pl.n;
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -1218,13 +1218,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0, -0.06, 0));
+ tf_tri.setTranslation(Vec3s(0, -0.06, 0));
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(pl, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(0, 0.11, 0));
+ tf_tri.setTranslation(Vec3s(0, 0.11, 0));
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1232,11 +1232,11 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
}
{
- Vec3f t[3];
+ Vec3s t[3];
t[0] << 0.05, 30, 0;
t[1] << 0.05, -10, 0;
t[2] << -0.1, 0, 20;
- Plane pl(Vec3f(1, 0, 0), 0);
+ Plane pl(Vec3s(1, 0, 0), 0);
TriangleP tri(t[0], t[1], t[2]);
Transform3f tf_tri = Transform3f(); // identity
@@ -1248,7 +1248,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(0.05, 0, 0));
+ tf_tri.setTranslation(Vec3s(0.05, 0, 0));
normal = pl.n;
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, true, NULL, NULL, &normal);
@@ -1257,13 +1257,13 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
testShapeCollide(pl, transform, tri, transform * tf_tri, true, NULL, NULL,
&normal);
- tf_tri.setTranslation(Vec3f(-0.06, 0, 0));
+ tf_tri.setTranslation(Vec3s(-0.06, 0, 0));
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
SET_LINE;
testShapeCollide(pl, transform, tri, transform * tf_tri, false);
- tf_tri.setTranslation(Vec3f(0.11, 0, 0));
+ tf_tri.setTranslation(Vec3s(0.11, 0, 0));
SET_LINE;
testShapeCollide(pl, Transform3f(), tri, tf_tri, false);
SET_LINE;
@@ -1273,7 +1273,7 @@ BOOST_AUTO_TEST_CASE(collide_planetriangle) {
BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
Sphere s(10);
- Halfspace hs(Vec3f(1, 0, 0), 0);
+ Halfspace hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -1281,9 +1281,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -1295,14 +1295,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(-5, 0, 0));
+ contact = transform.transform(Vec3s(-5, 0, 0));
depth = -10;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5, 0, 0));
+ tf2 = Transform3f(Vec3s(5, 0, 0));
contact << -2.5, 0, 0;
depth = -15;
normal << -1, 0, 0;
@@ -1310,15 +1310,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5, 0, 0));
- contact = transform.transform(Vec3f(-2.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(5, 0, 0));
+ contact = transform.transform(Vec3s(-2.5, 0, 0));
depth = -15;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5, 0, 0));
+ tf2 = Transform3f(Vec3s(-5, 0, 0));
contact << -7.5, 0, 0;
depth = -5;
normal << -1, 0, 0;
@@ -1326,25 +1326,25 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
- contact = transform.transform(Vec3f(-7.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5, 0, 0));
+ contact = transform.transform(Vec3s(-7.5, 0, 0));
depth = -5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-10.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-10.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(10.1, 0, 0));
+ tf2 = Transform3f(Vec3s(10.1, 0, 0));
contact << 0.05, 0, 0;
depth = -20.1;
normal << -1, 0, 0;
@@ -1352,17 +1352,17 @@ BOOST_AUTO_TEST_CASE(collide_halfspacesphere) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
- contact = transform.transform(Vec3f(0.05, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(10.1, 0, 0));
+ contact = transform.transform(Vec3s(0.05, 0, 0));
depth = -20.1;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
}
BOOST_AUTO_TEST_CASE(collide_planesphere) {
Sphere s(10);
- Plane hs(Vec3f(1, 0, 0), 0);
+ Plane hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -1370,13 +1370,13 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
- Vec3f p1, p2;
+ Vec3s normal;
+ Vec3s p1, p2;
CoalScalar eps = 1e-6;
- tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf1 = Transform3f(Vec3s(eps, 0, 0));
tf2 = Transform3f();
depth = -10 + eps;
p1 << -10 + eps, 0, 0;
@@ -1390,12 +1390,12 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
normal =
- transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
eps = -1e-6;
- tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf1 = Transform3f(Vec3s(eps, 0, 0));
tf2 = Transform3f();
depth = -10 - eps;
p1 << 10 + eps, 0, 0;
@@ -1408,12 +1408,12 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
tf1 = transform * tf1;
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
- normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5, 0, 0));
+ tf2 = Transform3f(Vec3s(5, 0, 0));
p1 << 10, 0, 0;
p2 << 5, 0, 0;
contact << (p1 + p2) / 2;
@@ -1423,15 +1423,15 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(5, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -5;
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5, 0, 0));
+ tf2 = Transform3f(Vec3s(-5, 0, 0));
p1 << -10, 0, 0;
p2 << -5, 0, 0;
contact << (p1 + p2) / 2;
@@ -1441,37 +1441,37 @@ BOOST_AUTO_TEST_CASE(collide_planesphere) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-10.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-10.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-10.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(10.1, 0, 0));
+ tf2 = Transform3f(Vec3s(10.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(10.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(10.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
}
BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
Box s(5, 10, 20);
- Halfspace hs(Vec3f(1, 0, 0), 0);
+ Halfspace hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -1479,9 +1479,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -1493,14 +1493,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(-1.25, 0, 0));
+ contact = transform.transform(Vec3s(-1.25, 0, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(1.25, 0, 0));
+ tf2 = Transform3f(Vec3s(1.25, 0, 0));
contact << -0.625, 0, 0;
depth = -3.75;
normal << -1, 0, 0;
@@ -1508,15 +1508,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
- contact = transform.transform(Vec3f(-0.625, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(1.25, 0, 0));
+ contact = transform.transform(Vec3s(-0.625, 0, 0));
depth = -3.75;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+ tf2 = Transform3f(Vec3s(-1.25, 0, 0));
contact << -1.875, 0, 0;
depth = -1.25;
normal << -1, 0, 0;
@@ -1524,15 +1524,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
- contact = transform.transform(Vec3f(-1.875, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-1.25, 0, 0));
+ contact = transform.transform(Vec3s(-1.875, 0, 0));
depth = -1.25;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.51, 0, 0));
+ tf2 = Transform3f(Vec3s(2.51, 0, 0));
contact << 0.005, 0, 0;
depth = -5.01;
normal << -1, 0, 0;
@@ -1540,20 +1540,20 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
- contact = transform.transform(Vec3f(0.005, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(2.51, 0, 0));
+ contact = transform.transform(Vec3s(0.005, 0, 0));
depth = -5.01;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.51, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-2.51, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
@@ -1565,7 +1565,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacebox) {
BOOST_AUTO_TEST_CASE(collide_planebox) {
Box s(5, 10, 20);
- Plane hs(Vec3f(1, 0, 0), 0);
+ Plane hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -1573,14 +1573,14 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
- Vec3f p1(2.5, 0, 0);
- Vec3f p2(0, 0, 0);
+ Vec3s p1(2.5, 0, 0);
+ Vec3s p2(0, 0, 0);
contact << (p1 + p2) / 2;
depth = -2.5;
normal << 1, 0, 0; // (1, 0, 0) or (-1, 0, 0)
@@ -1591,12 +1591,12 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(1.25, 0, 0));
+ tf2 = Transform3f(Vec3s(1.25, 0, 0));
p2 << 1.25, 0, 0;
contact << (p1 + p2) / 2;
depth = -1.25;
@@ -1605,15 +1605,15 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(1.25, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -1.25;
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+ tf2 = Transform3f(Vec3s(-1.25, 0, 0));
p1 << -2.5, 0, 0;
p2 << -1.25, 0, 0;
contact << (p1 + p2) / 2;
@@ -1623,30 +1623,30 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-1.25, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -1.25;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.51, 0, 0));
+ tf2 = Transform3f(Vec3s(2.51, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(2.51, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.51, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-2.51, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
@@ -1658,7 +1658,7 @@ BOOST_AUTO_TEST_CASE(collide_planebox) {
BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
Capsule s(5, 10);
- Halfspace hs(Vec3f(1, 0, 0), 0);
+ Halfspace hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -1666,9 +1666,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -1680,14 +1680,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(-2.5, 0, 0));
+ contact = transform.transform(Vec3s(-2.5, 0, 0));
depth = -5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(2.5, 0, 0));
contact << -1.25, 0, 0;
depth = -7.5;
normal << -1, 0, 0;
@@ -1695,15 +1695,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
- contact = transform.transform(Vec3f(-1.25, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(2.5, 0, 0));
+ contact = transform.transform(Vec3s(-1.25, 0, 0));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.5, 0, 0));
contact << -3.75, 0, 0;
depth = -2.5;
normal << -1, 0, 0;
@@ -1711,15 +1711,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
- contact = transform.transform(Vec3f(-3.75, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0));
+ contact = transform.transform(Vec3s(-3.75, 0, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(5.1, 0, 0));
contact << 0.05, 0, 0;
depth = -10.1;
normal << -1, 0, 0;
@@ -1727,24 +1727,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
- contact = transform.transform(Vec3f(0.05, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(5.1, 0, 0));
+ contact = transform.transform(Vec3s(0.05, 0, 0));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Halfspace(Vec3f(0, 1, 0), 0);
+ hs = Halfspace(Vec3s(0, 1, 0), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -1756,14 +1756,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, -2.5, 0));
+ contact = transform.transform(Vec3s(0, -2.5, 0));
depth = -5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = Transform3f(Vec3s(0, 2.5, 0));
contact << 0, -1.25, 0;
depth = -7.5;
normal << 0, -1, 0;
@@ -1771,15 +1771,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
- contact = transform.transform(Vec3f(0, -1.25, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 2.5, 0));
+ contact = transform.transform(Vec3s(0, -1.25, 0));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = Transform3f(Vec3s(0, -2.5, 0));
contact << 0, -3.75, 0;
depth = -2.5;
normal << 0, -1, 0;
@@ -1787,15 +1787,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
- contact = transform.transform(Vec3f(0, -3.75, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -2.5, 0));
+ contact = transform.transform(Vec3s(0, -3.75, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = Transform3f(Vec3s(0, 5.1, 0));
contact << 0, 0.05, 0;
depth = -10.1;
normal << 0, -1, 0;
@@ -1803,24 +1803,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
- contact = transform.transform(Vec3f(0, 0.05, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 5.1, 0));
+ contact = transform.transform(Vec3s(0, 0.05, 0));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Halfspace(Vec3f(0, 0, 1), 0);
+ hs = Halfspace(Vec3s(0, 0, 1), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -1832,14 +1832,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, -5));
+ contact = transform.transform(Vec3s(0, 0, -5));
depth = -10;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = Transform3f(Vec3s(0, 0, 2.5));
contact << 0, 0, -3.75;
depth = -12.5;
normal << 0, 0, -1;
@@ -1847,15 +1847,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
- contact = transform.transform(Vec3f(0, 0, -3.75));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 2.5));
+ contact = transform.transform(Vec3s(0, 0, -3.75));
depth = -12.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = Transform3f(Vec3s(0, 0, -2.5));
contact << 0, 0, -6.25;
depth = -7.5;
normal << 0, 0, -1;
@@ -1863,15 +1863,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
- contact = transform.transform(Vec3f(0, 0, -6.25));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -2.5));
+ contact = transform.transform(Vec3s(0, 0, -6.25));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = Transform3f(Vec3s(0, 0, 10.1));
contact << 0, 0, 0.05;
depth = -20.1;
normal << 0, 0, -1;
@@ -1879,27 +1879,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
- contact = transform.transform(Vec3f(0, 0, 0.05));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 10.1));
+ contact = transform.transform(Vec3s(0, 0, 0.05));
depth = -20.1;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
}
BOOST_AUTO_TEST_CASE(collide_planecapsule) {
Capsule s(5, 10);
- Plane hs(Vec3f(1, 0, 0), 0);
+ Plane hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -1907,9 +1907,9 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -1921,14 +1921,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
+ contact = transform.transform(Vec3s(0, 0, 0));
depth = -5;
- normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, 0x0, 0x0, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(2.5, 0, 0));
contact << 2.5, 0, 0;
depth = -2.5;
normal << 1, 0, 0;
@@ -1936,15 +1936,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
- contact = transform.transform(Vec3f(2.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(2.5, 0, 0));
+ contact = transform.transform(Vec3s(2.5, 0, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.5, 0, 0));
contact << -2.5, 0, 0;
depth = -2.5;
normal << -1, 0, 0;
@@ -1952,34 +1952,34 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
- contact = transform.transform(Vec3f(-2.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0));
+ contact = transform.transform(Vec3s(-2.5, 0, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Plane(Vec3f(0, 1, 0), 0);
+ hs = Plane(Vec3s(0, 1, 0), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -1991,14 +1991,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
+ contact = transform.transform(Vec3s(0, 0, 0));
depth = -5;
- normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
+ normal = transform.getRotation() * Vec3s(0, 1, 0); // (0, 1, 0) or (0, -1, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = Transform3f(Vec3s(0, 2.5, 0));
contact << 0, 2.5, 0;
depth = -2.5;
normal << 0, 1, 0;
@@ -2006,15 +2006,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
- contact = transform.transform(Vec3f(0, 2.5, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 2.5, 0));
+ contact = transform.transform(Vec3s(0, 2.5, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 1, 0);
+ normal = transform.getRotation() * Vec3s(0, 1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = Transform3f(Vec3s(0, -2.5, 0));
contact << 0, -2.5, 0;
depth = -2.5;
normal << 0, -1, 0;
@@ -2022,34 +2022,34 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
- contact = transform.transform(Vec3f(0, -2.5, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -2.5, 0));
+ contact = transform.transform(Vec3s(0, -2.5, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = Transform3f(Vec3s(0, 5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Plane(Vec3f(0, 0, 1), 0);
+ hs = Plane(Vec3s(0, 0, 1), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -2061,14 +2061,14 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, 0));
+ contact = transform.transform(Vec3s(0, 0, 0));
depth = -10;
- normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
+ normal = transform.getRotation() * Vec3s(0, 0, 1); // (0, 0, 1) or (0, 0, -1)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = Transform3f(Vec3s(0, 0, 2.5));
contact << 0, 0, 2.5;
depth = -7.5;
normal << 0, 0, 1;
@@ -2076,15 +2076,15 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
- contact = transform.transform(Vec3f(0, 0, 2.5));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 2.5));
+ contact = transform.transform(Vec3s(0, 0, 2.5));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, 0, 1);
+ normal = transform.getRotation() * Vec3s(0, 0, 1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = Transform3f(Vec3s(0, 0, -2.5));
contact << 0, 0, -2.5;
depth = -7.5;
normal << 0, 0, -1;
@@ -2092,37 +2092,37 @@ BOOST_AUTO_TEST_CASE(collide_planecapsule) {
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
- contact = transform.transform(Vec3f(0, 0, -2.5));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -2.5));
+ contact = transform.transform(Vec3s(0, 0, -2.5));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, 0x0, &depth, 0x0);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = Transform3f(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
}
BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
Cylinder s(5, 10);
- Halfspace hs(Vec3f(1, 0, 0), 0);
+ Halfspace hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -2130,9 +2130,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -2144,14 +2144,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(-2.5, 0, 0));
+ contact = transform.transform(Vec3s(-2.5, 0, 0));
depth = -5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(2.5, 0, 0));
contact << -1.25, 0, 0;
depth = -7.5;
normal << -1, 0, 0;
@@ -2159,15 +2159,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
- contact = transform.transform(Vec3f(-1.25, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(2.5, 0, 0));
+ contact = transform.transform(Vec3s(-1.25, 0, 0));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.5, 0, 0));
contact << -3.75, 0, 0;
depth = -2.5;
normal << -1, 0, 0;
@@ -2175,15 +2175,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
- contact = transform.transform(Vec3f(-3.75, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0));
+ contact = transform.transform(Vec3s(-3.75, 0, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(5.1, 0, 0));
contact << 0.05, 0, 0;
depth = -10.1;
normal << -1, 0, 0;
@@ -2191,24 +2191,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
- contact = transform.transform(Vec3f(0.05, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(5.1, 0, 0));
+ contact = transform.transform(Vec3s(0.05, 0, 0));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Halfspace(Vec3f(0, 1, 0), 0);
+ hs = Halfspace(Vec3s(0, 1, 0), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -2220,14 +2220,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, -2.5, 0));
+ contact = transform.transform(Vec3s(0, -2.5, 0));
depth = -5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = Transform3f(Vec3s(0, 2.5, 0));
contact << 0, -1.25, 0;
depth = -7.5;
normal << 0, -1, 0;
@@ -2235,15 +2235,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
- contact = transform.transform(Vec3f(0, -1.25, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 2.5, 0));
+ contact = transform.transform(Vec3s(0, -1.25, 0));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = Transform3f(Vec3s(0, -2.5, 0));
contact << 0, -3.75, 0;
depth = -2.5;
normal << 0, -1, 0;
@@ -2251,15 +2251,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
- contact = transform.transform(Vec3f(0, -3.75, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -2.5, 0));
+ contact = transform.transform(Vec3s(0, -3.75, 0));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = Transform3f(Vec3s(0, 5.1, 0));
contact << 0, 0.05, 0;
depth = -10.1;
normal << 0, -1, 0;
@@ -2267,24 +2267,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
- contact = transform.transform(Vec3f(0, 0.05, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 5.1, 0));
+ contact = transform.transform(Vec3s(0, 0.05, 0));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Halfspace(Vec3f(0, 0, 1), 0);
+ hs = Halfspace(Vec3s(0, 0, 1), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -2296,14 +2296,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, -2.5));
+ contact = transform.transform(Vec3s(0, 0, -2.5));
depth = -5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = Transform3f(Vec3s(0, 0, 2.5));
contact << 0, 0, -1.25;
depth = -7.5;
normal << 0, 0, -1;
@@ -2311,15 +2311,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
- contact = transform.transform(Vec3f(0, 0, -1.25));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 2.5));
+ contact = transform.transform(Vec3s(0, 0, -1.25));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = Transform3f(Vec3s(0, 0, -2.5));
contact << 0, 0, -3.75;
depth = -2.5;
normal << 0, 0, -1;
@@ -2327,15 +2327,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
- contact = transform.transform(Vec3f(0, 0, -3.75));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -2.5));
+ contact = transform.transform(Vec3s(0, 0, -3.75));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 5.1));
+ tf2 = Transform3f(Vec3s(0, 0, 5.1));
contact << 0, 0, 0.05;
depth = -10.1;
normal << 0, 0, -1;
@@ -2343,27 +2343,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
- contact = transform.transform(Vec3f(0, 0, 0.05));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 5.1));
+ contact = transform.transform(Vec3s(0, 0, 0.05));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -5.1));
+ tf2 = Transform3f(Vec3s(0, 0, -5.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -5.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
}
BOOST_AUTO_TEST_CASE(collide_planecylinder) {
Cylinder s(5, 10);
- Plane hs(Vec3f(1, 0, 0), 0);
+ Plane hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -2371,13 +2371,13 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
- Vec3f p1, p2;
+ Vec3s normal;
+ Vec3s p1, p2;
CoalScalar eps = 1e-6;
- tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf1 = Transform3f(Vec3s(eps, 0, 0));
tf2 = Transform3f();
p1 << -5 + eps, 0, 0;
p2 << 0, 0, 0;
@@ -2392,12 +2392,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
contact = transform.transform((p1 + p2) / 2);
depth = -5 + eps;
normal =
- transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
eps = -1e-6;
- tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf1 = Transform3f(Vec3s(eps, 0, 0));
tf2 = Transform3f();
p1 << 5 + eps, 0, 0;
p2 << 0, 0, 0;
@@ -2412,12 +2412,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
contact = transform.transform((p1 + p2) / 2);
depth = -5 - eps;
normal =
- transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(2.5, 0, 0));
p1 << 5, 0, 0;
p2 << 2.5, 0, 0;
contact << (p1 + p2) / 2;
@@ -2427,15 +2427,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(2.5, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.5, 0, 0));
p1 << -5, 0, 0;
p2 << -2.5, 0, 0;
contact << (p1 + p2) / 2;
@@ -2445,37 +2445,37 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Plane(Vec3f(0, 1, 0), 0);
+ hs = Plane(Vec3s(0, 1, 0), 0);
eps = 1e-6;
- tf1 = Transform3f(Vec3f(0, eps, 0));
+ tf1 = Transform3f(Vec3s(0, eps, 0));
tf2 = Transform3f();
p1 << 0, -5 + eps, 0;
p2 << 0, 0, 0;
@@ -2489,12 +2489,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 + eps;
- normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
eps = -1e-6;
- tf1 = Transform3f(Vec3f(0, eps, 0));
+ tf1 = Transform3f(Vec3s(0, eps, 0));
tf2 = Transform3f();
p1 << 0, 5 + eps, 0;
p2 << 0, 0, 0;
@@ -2508,12 +2508,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 - eps;
- normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = Transform3f(Vec3s(0, 2.5, 0));
p1 << 0, 5, 0;
p2 << 0, 2.5, 0;
contact << (p1 + p2) / 2;
@@ -2523,15 +2523,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 2.5, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 1, 0);
+ normal = transform.getRotation() * Vec3s(0, 1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = Transform3f(Vec3s(0, -2.5, 0));
p1 << 0, -5, 0;
p2 << 0, -2.5, 0;
contact << (p1 + p2) / 2;
@@ -2541,37 +2541,37 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -2.5, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = Transform3f(Vec3s(0, 5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Plane(Vec3f(0, 0, 1), 0);
+ hs = Plane(Vec3s(0, 0, 1), 0);
eps = 1e-6;
- tf1 = Transform3f(Vec3f(0, 0, eps));
+ tf1 = Transform3f(Vec3s(0, 0, eps));
tf2 = Transform3f();
p1 << 0, 0, -5 + eps;
p2 << 0, 0, 0;
@@ -2585,12 +2585,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 + eps;
- normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
eps = -1e-6;
- tf1 = Transform3f(Vec3f(0, 0, eps));
+ tf1 = Transform3f(Vec3s(0, 0, eps));
tf2 = Transform3f();
p1 << 0, 0, 5 + eps;
p2 << 0, 0, 0;
@@ -2604,12 +2604,12 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 - eps;
- normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = Transform3f(Vec3s(0, 0, 2.5));
p1 << 0, 0, 5;
p2 << 0, 0, 2.5;
contact << (p1 + p2) / 2;
@@ -2619,15 +2619,15 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 2.5));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 0, 1);
+ normal = transform.getRotation() * Vec3s(0, 0, 1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = Transform3f(Vec3s(0, 0, -2.5));
p1 << 0, 0, -5.;
p2 << 0, 0, -2.5;
contact << (p1 + p2) / 2;
@@ -2637,37 +2637,37 @@ BOOST_AUTO_TEST_CASE(collide_planecylinder) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -2.5));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = Transform3f(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
}
BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
Cone s(5, 10);
- Halfspace hs(Vec3f(1, 0, 0), 0);
+ Halfspace hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -2675,9 +2675,9 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
+ Vec3s normal;
tf1 = Transform3f();
tf2 = Transform3f();
@@ -2689,14 +2689,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(-2.5, 0, -5));
+ contact = transform.transform(Vec3s(-2.5, 0, -5));
depth = -5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(2.5, 0, 0));
contact << -1.25, 0, -5;
depth = -7.5;
normal << -1, 0, 0;
@@ -2704,15 +2704,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
- contact = transform.transform(Vec3f(-1.25, 0, -5));
+ tf2 = transform * Transform3f(Vec3s(2.5, 0, 0));
+ contact = transform.transform(Vec3s(-1.25, 0, -5));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.5, 0, 0));
contact << -3.75, 0, -5;
depth = -2.5;
normal << -1, 0, 0;
@@ -2720,15 +2720,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
- contact = transform.transform(Vec3f(-3.75, 0, -5));
+ tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0));
+ contact = transform.transform(Vec3s(-3.75, 0, -5));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(5.1, 0, 0));
contact << 0.05, 0, -5;
depth = -10.1;
normal << -1, 0, 0;
@@ -2736,24 +2736,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
- contact = transform.transform(Vec3f(0.05, 0, -5));
+ tf2 = transform * Transform3f(Vec3s(5.1, 0, 0));
+ contact = transform.transform(Vec3s(0.05, 0, -5));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Halfspace(Vec3f(0, 1, 0), 0);
+ hs = Halfspace(Vec3s(0, 1, 0), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -2765,14 +2765,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, -2.5, -5));
+ contact = transform.transform(Vec3s(0, -2.5, -5));
depth = -5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = Transform3f(Vec3s(0, 2.5, 0));
contact << 0, -1.25, -5;
depth = -7.5;
normal << 0, -1, 0;
@@ -2780,15 +2780,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
- contact = transform.transform(Vec3f(0, -1.25, -5));
+ tf2 = transform * Transform3f(Vec3s(0, 2.5, 0));
+ contact = transform.transform(Vec3s(0, -1.25, -5));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = Transform3f(Vec3s(0, -2.5, 0));
contact << 0, -3.75, -5;
depth = -2.5;
normal << 0, -1, 0;
@@ -2796,15 +2796,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
- contact = transform.transform(Vec3f(0, -3.75, -5));
+ tf2 = transform * Transform3f(Vec3s(0, -2.5, 0));
+ contact = transform.transform(Vec3s(0, -3.75, -5));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = Transform3f(Vec3s(0, 5.1, 0));
contact << 0, 0.05, -5;
depth = -10.1;
normal << 0, -1, 0;
@@ -2812,24 +2812,24 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
- contact = transform.transform(Vec3f(0, 0.05, -5));
+ tf2 = transform * Transform3f(Vec3s(0, 5.1, 0));
+ contact = transform.transform(Vec3s(0, 0.05, -5));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Halfspace(Vec3f(0, 0, 1), 0);
+ hs = Halfspace(Vec3s(0, 0, 1), 0);
tf1 = Transform3f();
tf2 = Transform3f();
@@ -2841,14 +2841,14 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
tf1 = transform;
tf2 = transform;
- contact = transform.transform(Vec3f(0, 0, -2.5));
+ contact = transform.transform(Vec3s(0, 0, -2.5));
depth = -5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = Transform3f(Vec3s(0, 0, 2.5));
contact << 0, 0, -1.25;
depth = -7.5;
normal << 0, 0, -1;
@@ -2856,15 +2856,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
- contact = transform.transform(Vec3f(0, 0, -1.25));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 2.5));
+ contact = transform.transform(Vec3s(0, 0, -1.25));
depth = -7.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = Transform3f(Vec3s(0, 0, -2.5));
contact << 0, 0, -3.75;
depth = -2.5;
normal << 0, 0, -1;
@@ -2872,15 +2872,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
- contact = transform.transform(Vec3f(0, 0, -3.75));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -2.5));
+ contact = transform.transform(Vec3s(0, 0, -3.75));
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 5.1));
+ tf2 = Transform3f(Vec3s(0, 0, 5.1));
contact << 0, 0, 0.05;
depth = -10.1;
normal << 0, 0, -1;
@@ -2888,27 +2888,27 @@ BOOST_AUTO_TEST_CASE(collide_halfspacecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 5.1));
- contact = transform.transform(Vec3f(0, 0, 0.05));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 5.1));
+ contact = transform.transform(Vec3s(0, 0, 0.05));
depth = -10.1;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -5.1));
+ tf2 = Transform3f(Vec3s(0, 0, -5.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -5.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -5.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
}
BOOST_AUTO_TEST_CASE(collide_planecone) {
Cone s(5, 10);
- Plane hs(Vec3f(1, 0, 0), 0);
+ Plane hs(Vec3s(1, 0, 0), 0);
Transform3f tf1;
Transform3f tf2;
@@ -2916,13 +2916,13 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
Transform3f transform;
generateRandomTransform(extents, transform);
- Vec3f contact;
+ Vec3s contact;
CoalScalar depth;
- Vec3f normal;
- Vec3f p1, p2;
+ Vec3s normal;
+ Vec3s p1, p2;
CoalScalar eps = 1e-6;
- tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf1 = Transform3f(Vec3s(eps, 0, 0));
tf2 = Transform3f();
p1 << -5 + eps, 0, -5;
p2 << 0, 0, -5;
@@ -2937,12 +2937,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
contact = transform.transform((p1 + p2) / 2);
depth = -5 + eps;
normal =
- transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
eps = -1e-6;
- tf1 = Transform3f(Vec3f(eps, 0, 0));
+ tf1 = Transform3f(Vec3s(eps, 0, 0));
tf2 = Transform3f();
p1 << 5 + eps, 0, -5;
p2 << 0, 0, -5;
@@ -2957,12 +2957,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
contact = transform.transform((p1 + p2) / 2);
depth = -5 - eps;
normal =
- transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
+ transform.getRotation() * Vec3s(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(2.5, 0, 0));
p1 << 5, 0, -5;
p2 << 2.5, 0, -5;
contact << (p1 + p2) / 2;
@@ -2972,15 +2972,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(2.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(2.5, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(1, 0, 0);
+ normal = transform.getRotation() * Vec3s(1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = Transform3f(Vec3s(-2.5, 0, 0));
p1 << -5, 0, -5;
p2 << -2.5, 0, -5;
contact << (p1 + p2) / 2;
@@ -2990,37 +2990,37 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-2.5, 0, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(-1, 0, 0);
+ normal = transform.getRotation() * Vec3s(-1, 0, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0));
+ tf2 = transform * Transform3f(Vec3s(-5.1, 0, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Plane(Vec3f(0, 1, 0), 0);
+ hs = Plane(Vec3s(0, 1, 0), 0);
eps = 1e-6;
- tf1 = Transform3f(Vec3f(0, eps, 0));
+ tf1 = Transform3f(Vec3s(0, eps, 0));
tf2 = Transform3f();
p1 << 0, -5 + eps, -5;
p2 << 0, 0, -5;
@@ -3034,12 +3034,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 + eps;
- normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
eps = -1e-6;
- tf1 = Transform3f(Vec3f(0, eps, 0));
+ tf1 = Transform3f(Vec3s(0, eps, 0));
tf2 = Transform3f();
p1 << 0, 5 + eps, -5;
p2 << 0, 0, -5;
@@ -3053,12 +3053,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 - eps;
- normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 1, 0); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = Transform3f(Vec3s(0, 2.5, 0));
p1 << 0, 5, -5;
p2 << 0, 2.5, -5;
contact << (p1 + p2) / 2;
@@ -3068,15 +3068,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 2.5, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 2.5, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 1, 0);
+ normal = transform.getRotation() * Vec3s(0, 1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = Transform3f(Vec3s(0, -2.5, 0));
p1 << 0, -5, -5;
p2 << 0, -2.5, -5;
contact << (p1 + p2) / 2;
@@ -3086,37 +3086,37 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -2.5, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -2.5, 0));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, -1, 0);
+ normal = transform.getRotation() * Vec3s(0, -1, 0);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = Transform3f(Vec3s(0, 5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, 5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, -5.1, 0));
+ tf2 = transform * Transform3f(Vec3s(0, -5.1, 0));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
- hs = Plane(Vec3f(0, 0, 1), 0);
+ hs = Plane(Vec3s(0, 0, 1), 0);
eps = 1e-6;
- tf1 = Transform3f(Vec3f(0, 0, eps));
+ tf1 = Transform3f(Vec3s(0, 0, eps));
tf2 = Transform3f();
p1 << 0, 0, -5 + eps;
p2 << 0, 0, 0;
@@ -3130,12 +3130,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 + eps;
- normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
eps = -1e-6;
- tf1 = Transform3f(Vec3f(0, 0, eps));
+ tf1 = Transform3f(Vec3s(0, 0, eps));
tf2 = Transform3f();
p1 << 0, 0, 5 + eps;
p2 << 0, 0, 0;
@@ -3149,12 +3149,12 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
tf2 = transform;
contact = transform.transform((p1 + p2) / 2);
depth = -5 - eps;
- normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
+ normal = transform.getRotation() * Vec3s(0, 0, 1); // (1, 0, 0) or (-1, 0, 0)
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal, true);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = Transform3f(Vec3s(0, 0, 2.5));
p1 << 0, 0, 5;
p2 << 0, 0, 2.5;
contact << (p1 + p2) / 2;
@@ -3164,15 +3164,15 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 2.5));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 2.5));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 0, 1);
+ normal = transform.getRotation() * Vec3s(0, 0, 1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = Transform3f(Vec3s(0, 0, -2.5));
p1 << 0, 0, -5;
p2 << 0, 0, -2.5;
contact << (p1 + p2) / 2;
@@ -3182,30 +3182,30 @@ BOOST_AUTO_TEST_CASE(collide_planecone) {
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -2.5));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -2.5));
contact = transform.transform((p1 + p2) / 2);
depth = -2.5;
- normal = transform.getRotation() * Vec3f(0, 0, -1);
+ normal = transform.getRotation() * Vec3s(0, 0, -1);
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, true, &contact, &depth, &normal);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = Transform3f(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, 10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, 10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = Transform3f();
- tf2 = Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
tf1 = transform;
- tf2 = transform * Transform3f(Vec3f(0, 0, -10.1));
+ tf2 = transform * Transform3f(Vec3s(0, 0, -10.1));
SET_LINE;
testShapeCollide(s, tf1, hs, tf2, false);
}
@@ -3214,15 +3214,15 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
Transform3f tf1;
Transform3f tf2;
- Vec3f normal;
- Vec3f contact;
+ Vec3s normal;
+ Vec3s contact;
CoalScalar distance;
Transform3f transform;
generateRandomTransform(extents, transform);
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset = 3.14;
Plane plane1(n, offset);
Plane plane2(n, offset);
@@ -3249,7 +3249,7 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
}
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset1 = 3.14;
CoalScalar offset2 = offset1 + 1.19841;
Plane plane1(n, offset1);
@@ -3267,7 +3267,7 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
}
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset1 = 3.14;
CoalScalar offset2 = offset1 - 1.19841;
Plane plane1(n, offset1);
@@ -3285,10 +3285,10 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
}
{
- Vec3f n1(1, 0, 0);
+ Vec3s n1(1, 0, 0);
CoalScalar offset1 = 3.14;
Plane plane1(n1, offset1);
- Vec3f n2(0, 0, 1);
+ Vec3s n2(0, 0, 1);
CoalScalar offset2 = -2.13;
Plane plane2(n2, offset2);
@@ -3307,10 +3307,10 @@ BOOST_AUTO_TEST_CASE(collide_planeplane) {
}
{
- Vec3f n1(1, 0, 0);
+ Vec3s n1(1, 0, 0);
CoalScalar offset1 = 3.14;
Plane plane1(n1, offset1);
- Vec3f n2(1, 1, 1);
+ Vec3s n2(1, 1, 1);
CoalScalar offset2 = -2.13;
Plane plane2(n2, offset2);
@@ -3334,15 +3334,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
Transform3f tf1;
Transform3f tf2;
- Vec3f normal;
- Vec3f contact;
+ Vec3s normal;
+ Vec3s contact;
CoalScalar distance;
Transform3f transform;
generateRandomTransform(extents, transform);
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset = 3.14;
Halfspace hf1(n, offset);
Halfspace hf2(n, offset);
@@ -3361,7 +3361,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
}
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset1 = 3.14;
CoalScalar offset2 = offset1 + 1.19841;
Halfspace hf1(n, offset1);
@@ -3381,7 +3381,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
}
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset1 = 3.14;
CoalScalar offset2 = offset1 - 1.19841;
Halfspace hf1(n, offset1);
@@ -3402,10 +3402,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
}
{
- Vec3f n1(1, 0, 0);
+ Vec3s n1(1, 0, 0);
CoalScalar offset1 = 3.14;
Halfspace hf1(n1, offset1);
- Vec3f n2(0, 0, 1);
+ Vec3s n2(0, 0, 1);
CoalScalar offset2 = -2.13;
Halfspace hf2(n2, offset2);
@@ -3423,10 +3423,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspacehalfspace) {
}
{
- Vec3f n1(1, 0, 0);
+ Vec3s n1(1, 0, 0);
CoalScalar offset1 = 3.14;
Halfspace hf1(n1, offset1);
- Vec3f n2(1, 1, 1);
+ Vec3s n2(1, 1, 1);
CoalScalar offset2 = -2.13;
Halfspace hf2(n2, offset2);
@@ -3450,15 +3450,15 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
Transform3f tf1;
Transform3f tf2;
- Vec3f normal;
- Vec3f contact;
+ Vec3s normal;
+ Vec3s contact;
CoalScalar distance;
Transform3f transform;
generateRandomTransform(extents, transform);
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset = 3.14;
Halfspace hf(n, offset);
Plane plane(n, offset);
@@ -3478,7 +3478,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
}
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset1 = 3.14;
CoalScalar offset2 = offset1 + 1.19841;
Halfspace hf(n, offset1);
@@ -3499,7 +3499,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
}
{
- Vec3f n = Vec3f::Random().normalized();
+ Vec3s n = Vec3s::Random().normalized();
CoalScalar offset1 = 3.14;
CoalScalar offset2 = offset1 - 1.19841;
Halfspace hf(n, offset1);
@@ -3520,10 +3520,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
}
{
- Vec3f n1(1, 0, 0);
+ Vec3s n1(1, 0, 0);
CoalScalar offset1 = 3.14;
Halfspace hf(n1, offset1);
- Vec3f n2(0, 0, 1);
+ Vec3s n2(0, 0, 1);
CoalScalar offset2 = -2.13;
Plane plane(n2, offset2);
@@ -3541,10 +3541,10 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
}
{
- Vec3f n1(1, 0, 0);
+ Vec3s n1(1, 0, 0);
CoalScalar offset1 = 3.14;
Halfspace hf(n1, offset1);
- Vec3f n2(1, 1, 1);
+ Vec3s n2(1, 1, 1);
CoalScalar offset2 = -2.13;
Plane plane(n2, offset2);
@@ -3567,7 +3567,7 @@ BOOST_AUTO_TEST_CASE(collide_halfspaceplane) {
BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) {
Sphere s1(20);
Sphere s2(10);
- Vec3f closest_p1, closest_p2, normal;
+ Vec3s closest_p1, closest_p2, normal;
bool compute_penetration = true;
Transform3f transform;
@@ -3576,61 +3576,61 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) {
CoalScalar dist = -1;
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+ s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration,
closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 10) < 0.001);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)),
+ s1, Transform3f(), s2, Transform3f(Vec3s(30.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)),
+ s1, Transform3f(), s2, Transform3f(Vec3s(29.9, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(dist <= 0);
- dist = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2,
+ dist = solver1.shapeDistance(s1, Transform3f(Vec3s(40, 0, 0)), s2,
Transform3f(), compute_penetration, closest_p1,
closest_p2, normal);
BOOST_CHECK(fabs(dist - 10) < 0.001);
- dist = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2,
+ dist = solver1.shapeDistance(s1, Transform3f(Vec3s(30.1, 0, 0)), s2,
Transform3f(), compute_penetration, closest_p1,
closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
- dist = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2,
+ dist = solver1.shapeDistance(s1, Transform3f(Vec3s(29.9, 0, 0)), s2,
Transform3f(), compute_penetration, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist < 0);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 10) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(30.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(29.9, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(dist < 0);
- dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2,
+ dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(40, 0, 0)), s2,
transform, compute_penetration, closest_p1,
closest_p2, normal);
BOOST_CHECK(fabs(dist - 10) < 0.001);
- dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)),
+ dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(30.1, 0, 0)),
s2, transform, compute_penetration, closest_p1,
closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
- dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)),
+ dist = solver1.shapeDistance(s1, transform * Transform3f(Vec3s(29.9, 0, 0)),
s2, transform, compute_penetration, closest_p1,
closest_p2, normal);
BOOST_CHECK(dist < 0);
@@ -3639,7 +3639,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_spheresphere) {
BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) {
Box s1(20, 40, 50);
Box s2(10, 10, 10);
- Vec3f closest_p1, closest_p2, normal;
+ Vec3s closest_p1, closest_p2, normal;
bool compute_penetration = true;
Transform3f transform;
@@ -3658,57 +3658,57 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxbox) {
BOOST_CHECK(dist <= 0);
dist = solver1.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(20.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 10.1) < 0.001);
dist = solver1.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(0, 20.2, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 10.2) < 0.001);
dist = solver1.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 10.1, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
dist = solver2.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver2.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(20.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 10.1) < 0.001);
dist = solver2.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(0, 20.1, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 10.1) < 0.001);
dist = solver2.shapeDistance(
- s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)),
+ s2, Transform3f(), s2, Transform3f(Vec3s(10.1, 10.1, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(15.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), compute_penetration,
+ s1, Transform3f(), s2, Transform3f(Vec3s(20, 0, 0)), compute_penetration,
closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 5) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(20, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 5) < 0.001);
}
@@ -3720,22 +3720,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) {
Transform3f tf1(
Quatf(0.5279170511703305, -0.50981118132505521, -0.67596178682051911,
0.0668715876735793),
- Vec3f(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
+ Vec3s(0.041218354748013122, 1.2022554710435607, 0.77338855025700015));
Transform3f tf2(
Quatf(0.70738826916719977, 0, 0, 0.70682518110536596),
- Vec3f(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
+ Vec3s(-0.29936284351096382, 0.80023864435868775, 0.71750000000000003));
GJKSolver solver;
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
bool compute_penetration = true;
solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal);
// If objects are not colliding, p2 should be outside the cylinder and
// p1 should be outside the box
- Vec3f p2Loc(tf1.inverse().transform(p2));
+ Vec3s p2Loc(tf1.inverse().transform(p2));
bool p2_in_cylinder((fabs(p2Loc[2]) <= s1.halfLength) &&
(p2Loc[0] * p2Loc[0] + p2Loc[1] * p2Loc[1] <= s1.radius));
- Vec3f p1Loc(tf2.inverse().transform(p1));
+ Vec3s p1Loc(tf2.inverse().transform(p1));
bool p1_in_box = (p1Loc.array().abs() <= s2.halfSide.array()).all();
std::cout << "p2 in cylinder = (" << p2Loc.transpose() << ")" << std::endl;
std::cout << "p1 in box = (" << p1Loc.transpose() << ")" << std::endl;
@@ -3759,7 +3759,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) {
s1 = Cylinder(0.06, 0.1);
tf1.setTranslation(
- Vec3f(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293));
+ Vec3s(-0.66734052046473924, 0.22219183277457269, 0.76825248755616293));
tf1.setQuatRotation(Quatf(0.52613359459338371, 0.32189408354839893,
0.70415587451837913, -0.35175580165512249));
solver.shapeDistance(s1, tf1, s2, tf2, compute_penetration, p1, p2, normal);
@@ -3768,7 +3768,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylinderbox) {
BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) {
Sphere s1(20);
Box s2(5, 5, 5);
- Vec3f closest_p1, closest_p2, normal;
+ Vec3s closest_p1, closest_p2, normal;
bool compute_penetration = true;
Transform3f transform;
@@ -3779,17 +3779,17 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) {
int N = 10;
for (int i = 0; i < N + 1; ++i) {
CoalScalar dbox = 0.0001 + (s1.radius + s2.halfSide(0)) * i * 4 / (3 * N);
- dist = solver1.shapeDistance(s1, Transform3f(Vec3f(dbox, 0., 0.)), s2,
+ dist = solver1.shapeDistance(s1, Transform3f(Vec3s(dbox, 0., 0.)), s2,
Transform3f(), compute_penetration, closest_p1,
closest_p2, normal);
BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6);
- EIGEN_VECTOR_IS_APPROX(normal, -Vec3f(1, 0, 0), 1e-6);
+ EIGEN_VECTOR_IS_APPROX(normal, -Vec3s(1, 0, 0), 1e-6);
dist =
solver1.shapeDistance(s1, transform, s2, transform, compute_penetration,
closest_p1, closest_p2, normal);
dist = solver1.shapeDistance(
- s1, transform * Transform3f(Vec3f(dbox, 0., 0.)), s2, transform,
+ s1, transform * Transform3f(Vec3s(dbox, 0., 0.)), s2, transform,
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK_CLOSE(dist, (dbox - s1.radius - s2.halfSide(0)), 1e-6);
EIGEN_VECTOR_IS_APPROX(normal, -transform.getRotation().col(0), 1e-6);
@@ -3806,22 +3806,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) {
BOOST_CHECK(dist <= 0);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)),
+ s1, Transform3f(), s2, Transform3f(Vec3s(22.6, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(22.6, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.01);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+ s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration,
closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 17.5) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 17.5) < 0.001);
}
@@ -3829,7 +3829,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_boxsphere) {
BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) {
Cylinder s1(5, 10);
Cylinder s2(5, 10);
- Vec3f closest_p1, closest_p2, normal;
+ Vec3s closest_p1, closest_p2, normal;
bool compute_penetration = true;
Transform3f transform;
@@ -3874,22 +3874,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) {
}
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+ s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+ s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration,
closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 30) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 30) < 0.001);
}
@@ -3897,7 +3897,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_cylindercylinder) {
BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) {
Cone s1(5, 10);
Cone s2(5, 10);
- Vec3f closest_p1, closest_p2, normal;
+ Vec3s closest_p1, closest_p2, normal;
bool compute_penetration = true;
Transform3f transform;
@@ -3942,22 +3942,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) {
}
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+ s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), compute_penetration,
+ s1, Transform3f(), s2, Transform3f(Vec3s(0, 0, 40)), compute_penetration,
closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 30) < 1);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)),
+ s1, transform, s2, transform * Transform3f(Vec3s(0, 0, 40)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 30) < 1);
}
@@ -3965,7 +3965,7 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecone) {
BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) {
Cylinder s1(5, 10);
Cone s2(5, 10);
- Vec3f closest_p1, closest_p2, normal;
+ Vec3s closest_p1, closest_p2, normal;
bool compute_penetration = true;
Transform3f transform;
@@ -4010,22 +4010,22 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) {
}
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)),
+ s1, Transform3f(), s2, Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.01);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(10.1, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 0.1) < 0.02);
dist = solver1.shapeDistance(
- s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), compute_penetration,
+ s1, Transform3f(), s2, Transform3f(Vec3s(40, 0, 0)), compute_penetration,
closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 30) < 0.01);
dist = solver1.shapeDistance(
- s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)),
+ s1, transform, s2, transform * Transform3f(Vec3s(40, 0, 0)),
compute_penetration, closest_p1, closest_p2, normal);
BOOST_CHECK(fabs(dist - 30) < 0.1);
}
@@ -4033,16 +4033,16 @@ BOOST_AUTO_TEST_CASE(GJKSolver_shapeDistance_conecylinder) {
template <typename S1, typename S2>
void testReversibleShapeDistance(const S1& s1, const S2& s2,
CoalScalar distance) {
- Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
- Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
+ Transform3f tf1(Vec3s(-0.5 * distance, 0.0, 0.0));
+ Transform3f tf2(Vec3s(+0.5 * distance, 0.0, 0.0));
CoalScalar distA;
CoalScalar distB;
- Vec3f p1A;
- Vec3f p1B;
- Vec3f p2A;
- Vec3f p2B;
- Vec3f normalA, normalB;
+ Vec3s p1A;
+ Vec3s p1B;
+ Vec3s p2A;
+ Vec3s p2B;
+ Vec3s normalA, normalB;
bool compute_penetration = true;
const double tol = 1e-6;
@@ -4083,8 +4083,8 @@ BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) {
Capsule capsule(5, 10);
Cone cone(5, 10);
Cylinder cylinder(5, 10);
- Plane plane(Vec3f(0, 0, 0), 0.0);
- Halfspace halfspace(Vec3f(0, 0, 0), 0.0);
+ Plane plane(Vec3s(0, 0, 0), 0.0);
+ Halfspace halfspace(Vec3s(0, 0, 0), 0.0);
// Use sufficiently long distance so that all the primitive shapes CANNOT
// intersect
diff --git a/test/gjk.cpp b/test/gjk.cpp
index cdfb4d5e..bb4d8996 100644
--- a/test/gjk.cpp
+++ b/test/gjk.cpp
@@ -49,11 +49,11 @@
using coal::CoalScalar;
using coal::GJKSolver;
using coal::GJKVariant;
-using coal::Matrix3f;
+using coal::Matrix3s;
using coal::Quatf;
using coal::Transform3f;
using coal::TriangleP;
-using coal::Vec3f;
+using coal::Vec3s;
typedef Eigen::Matrix<CoalScalar, Eigen::Dynamic, 1> vector_t;
typedef Eigen::Matrix<CoalScalar, 6, 1> vector6_t;
@@ -79,8 +79,8 @@ void test_gjk_distance_triangle_triangle(
if (enable_gjk_nesterov_acceleration)
solver.gjk.gjk_variant = GJKVariant::NesterovAcceleration;
Transform3f tf1, tf2;
- Vec3f p1, p2, a1, a2;
- Matrix3f M;
+ Vec3s p1, p2, a1, a2;
+ Matrix3s M;
CoalScalar distance(sqrt(-1));
clock_t start, end;
@@ -88,41 +88,41 @@ void test_gjk_distance_triangle_triangle(
CoalScalar eps = 1e-7;
Results_t results(N);
for (std::size_t i = 0; i < N; ++i) {
- Vec3f P1_loc(Vec3f::Random()), P2_loc(Vec3f::Random()),
- P3_loc(Vec3f::Random());
- Vec3f Q1_loc(Vec3f::Random()), Q2_loc(Vec3f::Random()),
- Q3_loc(Vec3f::Random());
+ Vec3s P1_loc(Vec3s::Random()), P2_loc(Vec3s::Random()),
+ P3_loc(Vec3s::Random());
+ Vec3s Q1_loc(Vec3s::Random()), Q2_loc(Vec3s::Random()),
+ Q3_loc(Vec3s::Random());
if (i == 0) {
- P1_loc = Vec3f(0.063996093749999997, -0.15320971679687501,
+ P1_loc = Vec3s(0.063996093749999997, -0.15320971679687501,
-0.42799999999999999);
P2_loc =
- Vec3f(0.069105957031249998, -0.150722900390625, -0.42999999999999999);
- P3_loc = Vec3f(0.063996093749999997, -0.15320971679687501,
+ Vec3s(0.069105957031249998, -0.150722900390625, -0.42999999999999999);
+ P3_loc = Vec3s(0.063996093749999997, -0.15320971679687501,
-0.42999999999999999);
Q1_loc =
- Vec3f(-25.655000000000001, -1.2858199462890625, 3.7249809570312502);
- Q2_loc = Vec3f(-10.926, -1.284259033203125, 3.7281499023437501);
- Q3_loc = Vec3f(-10.926, -1.2866180419921875, 3.72335400390625);
+ Vec3s(-25.655000000000001, -1.2858199462890625, 3.7249809570312502);
+ Q2_loc = Vec3s(-10.926, -1.284259033203125, 3.7281499023437501);
+ Q3_loc = Vec3s(-10.926, -1.2866180419921875, 3.72335400390625);
Transform3f tf(
Quatf(-0.42437287410898855, -0.26862477561450587,
-0.46249645019513175, 0.73064726592483387),
- Vec3f(-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
+ Vec3s(-12.824601270753471, -1.6840516940066426, 3.8914453043793844));
tf1 = tf;
} else if (i == 1) {
P1_loc =
- Vec3f(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622);
+ Vec3s(-0.8027043342590332, -0.30276307463645935, -0.4372950792312622);
P2_loc =
- Vec3f(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
+ Vec3s(-0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
P3_loc =
- Vec3f(0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
+ Vec3s(0.8027043342590332, 0.30276307463645935, -0.4372950792312622);
Q1_loc =
- Vec3f(-0.224713996052742, -0.7417119741439819, 0.19999997317790985);
+ Vec3s(-0.224713996052742, -0.7417119741439819, 0.19999997317790985);
Q2_loc =
- Vec3f(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985);
+ Vec3s(-0.5247139930725098, -0.7417119741439819, 0.19999997317790985);
Q3_loc =
- Vec3f(-0.224713996052742, -0.7417119741439819, 0.09999997168779373);
- Matrix3f R;
- Vec3f T;
+ Vec3s(-0.224713996052742, -0.7417119741439819, 0.09999997168779373);
+ Matrix3s R;
+ Vec3s T;
R << 0.9657787025454787, 0.09400415350535746, 0.24173273843919627,
-0.06713698817647556, 0.9908494114820345, -0.11709000206805695,
-0.25052768814676646, 0.09685382227587608, 0.9632524147814993;
@@ -136,7 +136,7 @@ void test_gjk_distance_triangle_triangle(
TriangleP tri1(P1_loc, P2_loc, P3_loc);
TriangleP tri2(Q1_loc, Q2_loc, Q3_loc);
- Vec3f normal;
+ Vec3s normal;
const bool compute_penetration = true;
coal::DistanceRequest request(compute_penetration, compute_penetration);
coal::DistanceResult result;
@@ -155,7 +155,7 @@ void test_gjk_distance_triangle_triangle(
results[i].timeGjk = end - start;
results[i].collision = res;
if (res) {
- Vec3f c1, c2, normal2;
+ Vec3s c1, c2, normal2;
++nCol;
// check that moving triangle 2 by the penetration depth in the
// direction of the normal makes the triangles collision free.
@@ -189,15 +189,15 @@ void test_gjk_distance_triangle_triangle(
tf2.setIdentity();
}
// Compute vectors between vertices
- Vec3f P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)),
+ Vec3s P1(tf1.transform(P1_loc)), P2(tf1.transform(P2_loc)),
P3(tf1.transform(P3_loc)), Q1(tf2.transform(Q1_loc)),
Q2(tf2.transform(Q2_loc)), Q3(tf2.transform(Q3_loc));
- Vec3f u1(P2 - P1);
- Vec3f v1(P3 - P1);
- Vec3f w1(u1.cross(v1));
- Vec3f u2(Q2 - Q1);
- Vec3f v2(Q3 - Q1);
- Vec3f w2(u2.cross(v2));
+ Vec3s u1(P2 - P1);
+ Vec3s v1(P3 - P1);
+ Vec3s w1(u1.cross(v1));
+ Vec3s u2(Q2 - Q1);
+ Vec3s v2(Q3 - Q1);
+ Vec3s w2(u2.cross(v2));
BOOST_CHECK(w1.squaredNorm() > eps * eps);
M.col(0) = u1;
M.col(1) = v1;
@@ -334,7 +334,7 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_nesterov) {
test_gjk_distance_triangle_triangle(true);
}
-void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray,
+void test_gjk_unit_sphere(CoalScalar center_distance, Vec3s ray,
double swept_sphere_radius,
bool use_gjk_nesterov_acceleration) {
using namespace coal;
@@ -343,7 +343,7 @@ void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray,
sphere.setSweptSphereRadius(swept_sphere_radius);
typedef Eigen::Matrix<CoalScalar, 4, 1> Vec4f;
- Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3f::Zero());
+ Transform3f tf0(Quatf(Vec4f::Random().normalized()), Vec3s::Zero());
Transform3f tf1(Quatf(Vec4f::Random().normalized()), center_distance * ray);
bool expect_collision = center_distance <= 2 * (r + swept_sphere_radius);
@@ -359,7 +359,7 @@ void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray,
details::GJK gjk(2, 1e-6);
if (use_gjk_nesterov_acceleration)
gjk.gjk_variant = GJKVariant::NesterovAcceleration;
- details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
+ details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0));
if (expect_collision) {
BOOST_CHECK((status == details::GJK::Collision) ||
@@ -372,12 +372,12 @@ void test_gjk_unit_sphere(CoalScalar center_distance, Vec3f ray,
BOOST_CHECK_EQUAL(status, details::GJK::NoCollision);
}
- Vec3f w0, w1, normal;
+ Vec3s w0, w1, normal;
gjk.getWitnessPointsAndNormal(shape, w0, w1, normal);
- Vec3f w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) +
+ Vec3s w0_expected(tf0.inverse().transform(tf0.getTranslation() + ray) +
swept_sphere_radius * normal);
- Vec3f w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) -
+ Vec3s w1_expected(tf0.inverse().transform(tf1.getTranslation() - ray) -
swept_sphere_radius * normal);
EIGEN_VECTOR_IS_APPROX(w0, w0_expected, 1e-10);
@@ -389,36 +389,36 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
std::array<double, 5> swept_sphere_radius = {0., 0.1, 1., 10., 100.};
for (bool nesterov_acceleration : use_nesterov_acceleration) {
for (double ssr : swept_sphere_radius) {
- test_gjk_unit_sphere(3, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+ test_gjk_unit_sphere(3, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
- test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+ test_gjk_unit_sphere(2.01, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
- test_gjk_unit_sphere(2.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+ test_gjk_unit_sphere(2.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
- test_gjk_unit_sphere(1.0, Vec3f(1, 0, 0), ssr, nesterov_acceleration);
+ test_gjk_unit_sphere(1.0, Vec3s(1, 0, 0), ssr, nesterov_acceleration);
// Random rotation
- test_gjk_unit_sphere(3, Vec3f::Random().normalized(), ssr,
+ test_gjk_unit_sphere(3, Vec3s::Random().normalized(), ssr,
nesterov_acceleration);
- test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), ssr,
+ test_gjk_unit_sphere(2.01, Vec3s::Random().normalized(), ssr,
nesterov_acceleration);
- test_gjk_unit_sphere(2.0, Vec3f::Random().normalized(), ssr,
+ test_gjk_unit_sphere(2.0, Vec3s::Random().normalized(), ssr,
nesterov_acceleration);
- test_gjk_unit_sphere(1.0, Vec3f::Random().normalized(), ssr,
+ test_gjk_unit_sphere(1.0, Vec3s::Random().normalized(), ssr,
nesterov_acceleration);
}
}
}
-void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
+void test_gjk_triangle_capsule(Vec3s T, bool expect_collision,
bool use_gjk_nesterov_acceleration,
- Vec3f w0_expected, Vec3f w1_expected) {
+ Vec3s w0_expected, Vec3s w1_expected) {
using namespace coal;
Capsule capsule(1., 2.); // Radius 1 and length 2
- TriangleP triangle(Vec3f(0., 0., 0.), Vec3f(1., 0., 0.), Vec3f(1., 1., 0.));
+ TriangleP triangle(Vec3s(0., 0., 0.), Vec3s(1., 0., 0.), Vec3s(1., 1., 0.));
Transform3f tf0, tf1;
tf1.setTranslation(T);
@@ -436,7 +436,7 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
details::GJK gjk(10, 1e-6);
if (use_gjk_nesterov_acceleration)
gjk.gjk_variant = GJKVariant::NesterovAcceleration;
- details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
+ details::GJK::Status status = gjk.evaluate(shape, Vec3s(1, 0, 0));
if (expect_collision) {
BOOST_CHECK((status == details::GJK::Collision) ||
@@ -445,19 +445,19 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
BOOST_CHECK_EQUAL(status, details::GJK::NoCollision);
// Check that guess works as expected
- Vec3f guess = gjk.getGuessFromSimplex();
+ Vec3s guess = gjk.getGuessFromSimplex();
details::GJK gjk2(3, 1e-6);
details::GJK::Status status2 = gjk2.evaluate(shape, guess);
BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision);
}
- Vec3f w0, w1, normal;
+ Vec3s w0, w1, normal;
if (status == details::GJK::NoCollision ||
status == details::GJK::CollisionWithPenetrationInformation) {
gjk.getWitnessPointsAndNormal(shape, w0, w1, normal);
} else {
details::EPA epa(64, 1e-6);
- details::EPA::Status epa_status = epa.evaluate(gjk, Vec3f(1, 0, 0));
+ details::EPA::Status epa_status = epa.evaluate(gjk, Vec3s(1, 0, 0));
BOOST_CHECK_EQUAL(epa_status, details::EPA::AccuracyReached);
epa.getWitnessPointsAndNormal(shape, w0, w1, normal);
}
@@ -468,23 +468,23 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
BOOST_AUTO_TEST_CASE(triangle_capsule) {
// GJK -> no collision
- test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, false, Vec3f(1., 0, 0),
- Vec3f(0., 0, 0));
+ test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, false, Vec3s(1., 0, 0),
+ Vec3s(0., 0, 0));
// GJK + Nesterov acceleration -> no collision
- test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, true, Vec3f(1., 0, 0),
- Vec3f(0., 0, 0));
+ test_gjk_triangle_capsule(Vec3s(1.01, 0, 0), false, true, Vec3s(1., 0, 0),
+ Vec3s(0., 0, 0));
// GJK -> collision
- test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, false, Vec3f(1., 0, 0),
- Vec3f(0., 0, 0));
+ test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, false, Vec3s(1., 0, 0),
+ Vec3s(0., 0, 0));
// GJK + Nesterov acceleration -> collision
- test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, true, Vec3f(1., 0, 0),
- Vec3f(0., 0, 0));
+ test_gjk_triangle_capsule(Vec3s(0.5, 0, 0), true, true, Vec3s(1., 0, 0),
+ Vec3s(0., 0, 0));
// GJK + EPA -> collision
- test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, false, Vec3f(0, 1, 0),
- Vec3f(0.5, 0, 0));
+ test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, false, Vec3s(0, 1, 0),
+ Vec3s(0.5, 0, 0));
// GJK + Nesterov accleration + EPA -> collision
- test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, true, Vec3f(0, 1, 0),
- Vec3f(0.5, 0, 0));
+ test_gjk_triangle_capsule(Vec3s(-0.5, -0.01, 0), true, true, Vec3s(0, 1, 0),
+ Vec3s(0.5, 0, 0));
}
diff --git a/test/gjk_asserts.cpp b/test/gjk_asserts.cpp
index 4028b869..d18a17f7 100644
--- a/test/gjk_asserts.cpp
+++ b/test/gjk_asserts.cpp
@@ -13,8 +13,8 @@ double DegToRad(const double& deg) {
static double degToRad = pi / 180.;
return deg * degToRad;
}
-std::vector<Vec3f> dirs{Vec3f::UnitZ(), -Vec3f::UnitZ(), Vec3f::UnitY(),
- -Vec3f::UnitY(), Vec3f::UnitX(), -Vec3f::UnitX()};
+std::vector<Vec3s> dirs{Vec3s::UnitZ(), -Vec3s::UnitZ(), Vec3s::UnitY(),
+ -Vec3s::UnitY(), Vec3s::UnitX(), -Vec3s::UnitX()};
void CreateSphereMesh(BVHModel<OBBRSS>& model, const double& radius) {
size_t polarSteps{32};
@@ -24,7 +24,7 @@ void CreateSphereMesh(BVHModel<OBBRSS>& model, const double& radius) {
const float polarStep = PI / (float)(polarSteps - 1);
const float azimuthStep = 2.0f * PI / (float)(azimuthSteps - 1);
- std::vector<Vec3f> vertices;
+ std::vector<Vec3s> vertices;
std::vector<Triangle> triangles;
for (size_t p = 0; p < polarSteps; ++p) {
@@ -77,9 +77,9 @@ BOOST_AUTO_TEST_CASE(TestSpheres) {
(i == 86 && j == 52) || (i == 89 && j == 17) ||
(i == 89 && j == 58) || (i == 89 && j == 145)) {
sphere2Tf.setQuatRotation(
- Eigen::AngleAxis<double>(DegToRad(i), Vec3f::UnitZ()) *
- Eigen::AngleAxis<double>(DegToRad(j), Vec3f::UnitY()));
- for (const Vec3f& dir : dirs) {
+ Eigen::AngleAxis<double>(DegToRad(i), Vec3s::UnitZ()) *
+ Eigen::AngleAxis<double>(DegToRad(j), Vec3s::UnitY()));
+ for (const Vec3s& dir : dirs) {
sphere2Tf.setTranslation(dir);
CollisionResult result;
diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp
index 5283e451..139df1ad 100644
--- a/test/gjk_convergence_criterion.cpp
+++ b/test/gjk_convergence_criterion.cpp
@@ -54,7 +54,7 @@ using coal::GJKSolver;
using coal::ShapeBase;
using coal::support_func_guess_t;
using coal::Transform3f;
-using coal::Vec3f;
+using coal::Vec3s;
using coal::details::GJK;
using coal::details::MinkowskiDiff;
using std::size_t;
@@ -128,7 +128,7 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1,
Transform3f identity = Transform3f::Identity();
// Same init for both solvers
- Vec3f init_guess = Vec3f(1, 0, 0);
+ Vec3s init_guess = Vec3s(1, 0, 0);
support_func_guess_t init_support_guess;
init_support_guess.setZero();
@@ -138,21 +138,21 @@ void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1,
GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(gjk1.getNumIterations() <= max_iterations);
- Vec3f ray1 = gjk1.ray;
+ Vec3s ray1 = gjk1.ray;
res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res1 != GJK::Status::Failed);
EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, 1e-8);
GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(gjk2.getNumIterations() <= max_iterations);
- Vec3f ray2 = gjk2.ray;
+ Vec3s ray2 = gjk2.ray;
res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res2 != GJK::Status::Failed);
EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, 1e-8);
GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(gjk3.getNumIterations() <= max_iterations);
- Vec3f ray3 = gjk3.ray;
+ Vec3s ray3 = gjk3.ray;
res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess);
BOOST_CHECK(res3 != GJK::Status::Failed);
EIGEN_VECTOR_IS_APPROX(gjk3.ray, ray3, 1e-8);
diff --git a/test/hfields.cpp b/test/hfields.cpp
index 0042cd80..a9fcc4c0 100644
--- a/test/hfields.cpp
+++ b/test/hfields.cpp
@@ -61,18 +61,18 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
const CoalScalar min_altitude,
const CoalScalar max_altitude) {
const CoalScalar x_dim = 1., y_dim = 2.;
- const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);
+ const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude);
HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude);
BOOST_CHECK(hfield.getXDim() == x_dim);
BOOST_CHECK(hfield.getYDim() == y_dim);
- const VecXf& x_grid = hfield.getXGrid();
+ const VecXs& x_grid = hfield.getXGrid();
BOOST_CHECK(x_grid[0] == -x_dim / 2.);
BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.);
- const VecXf& y_grid = hfield.getYGrid();
+ const VecXs& y_grid = hfield.getYGrid();
BOOST_CHECK(y_grid[0] == y_dim / 2.);
BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.);
@@ -81,7 +81,7 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
for (Eigen::DenseIndex i = 0; i < nx; ++i) {
for (Eigen::DenseIndex j = 0; j < ny; ++j) {
- Vec3f point(x_grid[i], y_grid[j], heights(j, i));
+ Vec3s point(x_grid[i], y_grid[j], heights(j, i));
BOOST_CHECK(hfield.aabb_local.contain(point));
}
}
@@ -98,13 +98,13 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
// Build equivalent object
const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude);
const Transform3f box_placement(
- Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.));
+ Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.));
// Test collision
const Sphere sphere(1.);
static const Transform3f IdTransform;
- const Box box(Vec3f::Ones());
+ const Box box(Vec3s::Ones());
Transform3f M_sphere, M_box;
@@ -112,9 +112,9 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
{
const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
CollisionRequest request;
CollisionResult result;
@@ -139,9 +139,9 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
{
const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision));
CollisionRequest
request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));
@@ -165,15 +165,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
// Update height
hfield.updateHeights(
- MatrixXf::Constant(ny, nx, max_altitude / 2.)); // We change nothing
+ MatrixXs::Constant(ny, nx, max_altitude / 2.)); // We change nothing
// No collision case
{
const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
CollisionRequest request;
CollisionResult result;
@@ -198,9 +198,9 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
{
const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision));
CollisionRequest
request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));
@@ -224,15 +224,15 @@ void test_constant_hfields(const Eigen::DenseIndex nx,
// Restore height
hfield.updateHeights(
- MatrixXf::Constant(ny, nx, max_altitude)); // We change nothing
+ MatrixXs::Constant(ny, nx, max_altitude)); // We change nothing
// Collision case
{
const CoalScalar eps_collision = -0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision));
CollisionRequest
request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));
@@ -275,20 +275,20 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
const CoalScalar min_altitude,
const CoalScalar max_altitude) {
const CoalScalar x_dim = 1., y_dim = 2.;
- const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);
+ const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude);
HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude);
// Build equivalent object
const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude);
const Transform3f box_placement(
- Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.));
+ Matrix3s::Identity(), Vec3s(0., 0., (max_altitude + min_altitude) / 2.));
// Test collision
const Sphere sphere(1.);
static const Transform3f IdTransform;
- const Box box(Vec3f::Ones());
+ const Box box(Vec3s::Ones());
Transform3f M_sphere, M_box;
@@ -296,9 +296,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
{
const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
CollisionRequest request;
CollisionResult result;
@@ -323,9 +323,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
{
const CoalScalar eps_no_collision = +0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
CollisionRequest request;
request.security_margin = eps_no_collision + 1e-6;
@@ -351,9 +351,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
{
const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
CollisionRequest request;
CollisionResult result;
@@ -378,9 +378,9 @@ void test_negative_security_margin(const Eigen::DenseIndex nx,
{
const CoalScalar eps_no_collision = -0.1 * (max_altitude - min_altitude);
M_sphere.setTranslation(
- Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
+ Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision));
M_box.setTranslation(
- Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
+ Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
CollisionRequest request;
request.security_margin = eps_no_collision - 1e-4;
@@ -415,22 +415,22 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) {
const Eigen::DenseIndex nx = 100, ny = 100;
typedef AABB BV;
- const MatrixXf X =
+ const MatrixXs X =
Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1);
- const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
+ const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
const CoalScalar dim_square = 0.5;
const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole =
(X.array().abs() < dim_square) && (Y.array().abs() < dim_square);
- const MatrixXf heights =
- MatrixXf::Ones(ny, nx) - hole.cast<double>().matrix();
+ const MatrixXs heights =
+ MatrixXs::Ones(ny, nx) - hole.cast<double>().matrix();
const HeightField<BV> hfield(2., 2., heights, -10.);
Sphere sphere(0.48);
- const Transform3f sphere_pos(Vec3f(0., 0., 0.5));
+ const Transform3f sphere_pos(Vec3s(0., 0., 0.5));
const Transform3f hfield_pos;
const CollisionRequest request;
@@ -459,17 +459,17 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) {
// typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug
// mode), as the overlap of OBBRSS is not satisfactory yet.
typedef AABB BV;
- const MatrixXf X =
+ const MatrixXs X =
Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1);
- const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
+ const MatrixXs Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);
const CoalScalar dim_hole = 1;
const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole =
(X.array().square() + Y.array().square() <= dim_hole);
- const MatrixXf heights =
- MatrixXf::Ones(ny, nx) - hole.cast<double>().matrix();
+ const MatrixXs heights =
+ MatrixXs::Ones(ny, nx) - hole.cast<double>().matrix();
const HeightField<BV> hfield(2., 2., heights, -10.);
@@ -480,7 +480,7 @@ BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) {
BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.);
Sphere sphere(0.975);
- const Transform3f sphere_pos(Vec3f(0., 0., 1.));
+ const Transform3f sphere_pos(Vec3s(0., 0., 1.));
const Transform3f hfield_pos;
const CoalScalar thresholds[3] = {0., 0.01, -0.005};
@@ -520,11 +520,11 @@ bool isApprox(const CoalScalar v1, const CoalScalar v2,
return std::fabs(v1 - v2) <= tol;
}
-Vec3f computeFaceNormal(const Triangle& triangle,
- const std::vector<Vec3f>& points) {
- const Vec3f pointA = points[triangle[0]];
- const Vec3f pointB = points[triangle[1]];
- const Vec3f pointC = points[triangle[2]];
+Vec3s computeFaceNormal(const Triangle& triangle,
+ const std::vector<Vec3s>& points) {
+ const Vec3s pointA = points[triangle[0]];
+ const Vec3s pointB = points[triangle[1]];
+ const Vec3s pointC = points[triangle[2]];
return (pointB - pointA).cross(pointC - pointA).normalized();
}
@@ -532,7 +532,7 @@ Vec3f computeFaceNormal(const Triangle& triangle,
BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
const CoalScalar sphere_radius = 1.;
Sphere sphere(sphere_radius);
- MatrixXf altitutes(2, 2);
+ MatrixXs altitutes(2, 2);
CoalScalar altitude_value = 1.;
altitutes.fill(altitude_value);
@@ -564,20 +564,20 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
// Check face normals for convex1
{
- const std::vector<Vec3f>& points = *(convex1.points);
+ const std::vector<Vec3s>& points = *(convex1.points);
// BOTTOM
{
const Triangle& triangle = (*(convex1.polygons))[0];
BOOST_CHECK(
- computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ()));
+ computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ()));
}
// TOP
{
const Triangle& triangle = (*(convex1.polygons))[1];
- BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ()));
+ BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ()));
}
// WEST sides
@@ -586,14 +586,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
const Triangle& triangle2 = (*(convex1.polygons))[3];
BOOST_CHECK(
- computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitX()));
+ computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitX()));
BOOST_CHECK(
- computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitX()));
+ computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitX()));
}
// SOUTH-EAST sides
{
- const Vec3f south_east_normal = Vec3f(1., -1., 0).normalized();
+ const Vec3s south_east_normal = Vec3s(1., -1., 0).normalized();
const Triangle& triangle1 = (*(convex1.polygons))[4];
const Triangle& triangle2 = (*(convex1.polygons))[5];
@@ -613,29 +613,29 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
<< computeFaceNormal(triangle1, points).transpose()
<< std::endl;
BOOST_CHECK(
- computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitY()));
+ computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitY()));
BOOST_CHECK(
- computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitY()));
+ computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitY()));
}
}
// Check face normals for convex2
{
- const std::vector<Vec3f>& points = *(convex2.points);
+ const std::vector<Vec3s>& points = *(convex2.points);
// BOTTOM
{
const Triangle& triangle = (*(convex2.polygons))[0];
BOOST_CHECK(
- computeFaceNormal(triangle, points).isApprox(-Vec3f::UnitZ()));
+ computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ()));
}
// TOP
{
const Triangle& triangle = (*(convex2.polygons))[1];
- BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3f::UnitZ()));
+ BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ()));
}
// SOUTH sides
@@ -644,14 +644,14 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
const Triangle& triangle2 = (*(convex2.polygons))[3];
BOOST_CHECK(
- computeFaceNormal(triangle1, points).isApprox(-Vec3f::UnitY()));
+ computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitY()));
BOOST_CHECK(
- computeFaceNormal(triangle2, points).isApprox(-Vec3f::UnitY()));
+ computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitY()));
}
// NORTH-WEST sides
{
- const Vec3f north_west_normal = Vec3f(-1., 1., 0).normalized();
+ const Vec3s north_west_normal = Vec3s(-1., 1., 0).normalized();
const Triangle& triangle1 = (*(convex2.polygons))[4];
const Triangle& triangle2 = (*(convex2.polygons))[5];
@@ -668,9 +668,9 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) {
const Triangle& triangle2 = (*(convex2.polygons))[7];
BOOST_CHECK(
- computeFaceNormal(triangle1, points).isApprox(Vec3f::UnitX()));
+ computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitX()));
BOOST_CHECK(
- computeFaceNormal(triangle2, points).isApprox(Vec3f::UnitX()));
+ computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitX()));
}
}
}
@@ -679,7 +679,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) {
typedef HFNodeBase::FaceOrientation FaceOrientation;
const CoalScalar sphere_radius = 1.;
Sphere sphere(sphere_radius);
- MatrixXf altitutes(3, 3);
+ MatrixXs altitutes(3, 3);
CoalScalar altitude_value = 1.;
altitutes.fill(altitude_value);
@@ -716,7 +716,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) {
BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
const CoalScalar sphere_radius = 1.;
Sphere sphere(sphere_radius);
- MatrixXf altitutes(2, 2);
+ MatrixXs altitutes(2, 2);
CoalScalar altitude_value = 1.;
altitutes.fill(altitude_value);
@@ -728,7 +728,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
// Collision from the TOP
{
- const Transform3f sphere_pos(Vec3f(0., 0., 2.));
+ const Transform3f sphere_pos(Vec3s(0., 0., 2.));
const Transform3f hfield_pos;
CollisionResult result;
@@ -743,7 +743,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
// Same, but with a positive margin.
{
- const Transform3f sphere_pos(Vec3f(0., 0., 2.));
+ const Transform3f sphere_pos(Vec3s(0., 0., 2.));
const Transform3f hfield_pos;
CollisionResult result;
@@ -754,7 +754,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
BOOST_CHECK(result.isCollision());
if (result.isCollision()) {
const Contact& contact = result.getContact(0);
- BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitZ()));
+ BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitZ()));
std::cout << "contact.penetration_depth: " << contact.penetration_depth
<< std::endl;
BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
@@ -763,7 +763,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
// Collision from the BOTTOM
{
- const Transform3f sphere_pos(Vec3f(0., 0., -1.));
+ const Transform3f sphere_pos(Vec3s(0., 0., -1.));
const Transform3f hfield_pos;
CollisionResult result;
@@ -775,7 +775,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
}
{
- const Transform3f sphere_pos(Vec3f(0., 0., -1.));
+ const Transform3f sphere_pos(Vec3s(0., 0., -1.));
const Transform3f hfield_pos;
CollisionResult result;
@@ -786,7 +786,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
BOOST_CHECK(result.isCollision());
{
const Contact& contact = result.getContact(0);
- BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitZ()));
+ BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitZ()));
std::cout << "contact.penetration_depth: " << contact.penetration_depth
<< std::endl;
BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
@@ -796,7 +796,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
// Collision from the WEST
{
const Transform3f sphere_pos(
- Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
+ Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -809,7 +809,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
{
const Transform3f sphere_pos(
- Vec3f(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
+ Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -820,7 +820,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
BOOST_CHECK(result.isCollision());
if (result.isCollision()) {
const Contact& contact = result.getContact(0);
- BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitX()));
+ BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitX()));
BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
}
}
@@ -828,7 +828,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
// Collision from the EAST
{
const Transform3f sphere_pos(
- Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
+ Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -841,7 +841,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
{
const Transform3f sphere_pos(
- Vec3f(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
+ Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -853,7 +853,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
if (result.isCollision()) {
const Contact& contact = result.getContact(0);
- BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitX()));
+ BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitX()));
BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
}
}
@@ -861,7 +861,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
// Collision from the NORTH
{
const Transform3f sphere_pos(
- Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
+ Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -874,7 +874,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
{
const Transform3f sphere_pos(
- Vec3f(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
+ Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -886,7 +886,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
if (result.isCollision()) {
const Contact& contact = result.getContact(0);
- BOOST_CHECK(contact.normal.isApprox(Vec3f::UnitY()));
+ BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitY()));
BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
}
}
@@ -894,7 +894,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
// Collision from the SOUTH
{
const Transform3f sphere_pos(
- Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
+ Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -907,7 +907,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
{
const Transform3f sphere_pos(
- Vec3f(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
+ Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5));
const Transform3f hfield_pos;
CollisionResult result;
@@ -919,7 +919,7 @@ BOOST_AUTO_TEST_CASE(test_hfield_single_bin) {
if (result.isCollision()) {
const Contact& contact = result.getContact(0);
- BOOST_CHECK(contact.normal.isApprox(-Vec3f::UnitY()));
+ BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitY()));
BOOST_CHECK(isApprox(contact.penetration_depth, 0.));
}
}
diff --git a/test/math.cpp b/test/math.cpp
index ad44b43f..23173903 100644
--- a/test/math.cpp
+++ b/test/math.cpp
@@ -48,13 +48,13 @@
using namespace coal;
BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) {
- Vec3f v1(1.0, 2.0, 3.0);
+ Vec3s v1(1.0, 2.0, 3.0);
BOOST_CHECK(v1[0] == 1.0);
BOOST_CHECK(v1[1] == 2.0);
BOOST_CHECK(v1[2] == 3.0);
- Vec3f v2 = v1;
- Vec3f v3(3.3, 4.3, 5.3);
+ Vec3s v2 = v1;
+ Vec3s v3(3.3, 4.3, 5.3);
v1 += v3;
BOOST_CHECK(isEqual(v1, v2 + v3));
v1 -= v3;
@@ -87,53 +87,53 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64) {
BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix()));
v1.array() += 2.0;
- BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-1.0, -2.0, -3.0)));
+ BOOST_CHECK(isEqual((-Vec3s(1.0, 2.0, 3.0)), Vec3s(-1.0, -2.0, -3.0)));
- v1 = Vec3f(1.0, 2.0, 3.0);
- v2 = Vec3f(3.0, 4.0, 5.0);
- BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0)));
+ v1 = Vec3s(1.0, 2.0, 3.0);
+ v2 = Vec3s(3.0, 4.0, 5.0);
+ BOOST_CHECK(isEqual((v1.cross(v2)), Vec3s(-2.0, 4.0, -2.0)));
BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
- v1 = Vec3f(3.0, 4.0, 5.0);
+ v1 = Vec3s(3.0, 4.0, 5.0);
BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm()));
- v1 = Vec3f(1.0, 2.0, 3.0);
- v2 = Vec3f(3.0, 4.0, 5.0);
- BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-2.0, 4.0, -2.0)));
+ v1 = Vec3s(1.0, 2.0, 3.0);
+ v2 = Vec3s(3.0, 4.0, 5.0);
+ BOOST_CHECK(isEqual(v1.cross(v2), Vec3s(-2.0, 4.0, -2.0)));
BOOST_CHECK(v1.dot(v2) == 26);
}
-Vec3f rotate(Vec3f input, CoalScalar w, Vec3f vec) {
+Vec3s rotate(Vec3s input, CoalScalar w, Vec3s vec) {
return 2 * vec.dot(input) * vec + (w * w - vec.dot(vec)) * input +
2 * w * vec.cross(input);
}
BOOST_AUTO_TEST_CASE(quaternion) {
Quatf q1(Quatf::Identity()), q2, q3;
- q2 = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
+ q2 = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2);
q3 = q2.inverse();
- Vec3f v(1, -1, 0);
+ Vec3s v(1, -1, 0);
BOOST_CHECK(isEqual(v, q1 * v));
- BOOST_CHECK(isEqual(Vec3f(1, 1, 0), q2 * v));
+ BOOST_CHECK(isEqual(Vec3s(1, 1, 0), q2 * v));
BOOST_CHECK(
- isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3 * v));
+ isEqual(rotate(v, q3.w(), Vec3s(q3.x(), q3.y(), q3.z())), q3 * v));
}
BOOST_AUTO_TEST_CASE(transform) {
- Quatf q = fromAxisAngle(Vec3f(0, 0, 1), M_PI / 2);
- Vec3f T(0, 1, 2);
+ Quatf q = fromAxisAngle(Vec3s(0, 0, 1), M_PI / 2);
+ Vec3s T(0, 1, 2);
Transform3f tf(q, T);
- Vec3f v(1, -1, 0);
+ Vec3s v(1, -1, 0);
BOOST_CHECK(isEqual(q * v + T, q * v + T));
- Vec3f rv(q * v);
- // typename Transform3f::transform_return_type<Vec3f>::type output =
+ Vec3s rv(q * v);
+ // typename Transform3f::transform_return_type<Vec3s>::type output =
// tf * v;
// std::cout << rv << std::endl;
// std::cout << output.lhs() << std::endl;
diff --git a/test/normal_and_nearest_points.cpp b/test/normal_and_nearest_points.cpp
index e440afdc..76e62af0 100644
--- a/test/normal_and_nearest_points.cpp
+++ b/test/normal_and_nearest_points.cpp
@@ -128,16 +128,16 @@ void test_normal_and_nearest_points(
Contact contact = colres.getContact(0);
BOOST_CHECK_CLOSE(dist, contact.penetration_depth, dummy_precision);
- Vec3f cp1 = contact.nearest_points[0];
+ Vec3s cp1 = contact.nearest_points[0];
EIGEN_VECTOR_IS_APPROX(cp1, distres.nearest_points[0], dummy_precision);
- Vec3f cp2 = contact.nearest_points[1];
+ Vec3s cp2 = contact.nearest_points[1];
EIGEN_VECTOR_IS_APPROX(cp2, distres.nearest_points[1], dummy_precision);
BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(),
epa_tolerance);
EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, epa_tolerance);
- Vec3f separation_vector = contact.penetration_depth * contact.normal;
+ Vec3s separation_vector = contact.penetration_depth * contact.normal;
EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, epa_tolerance);
if (dist < 0) {
@@ -160,13 +160,13 @@ void test_normal_and_nearest_points(
BOOST_CHECK(!new_colres.isCollision());
BOOST_CHECK_CLOSE(new_colres.distance_lower_bound, new_dist,
epa_tolerance);
- Vec3f new_cp1 = new_distres.nearest_points[0];
- Vec3f new_cp2 = new_distres.nearest_points[1];
+ Vec3s new_cp1 = new_distres.nearest_points[0];
+ Vec3s new_cp2 = new_distres.nearest_points[1];
BOOST_CHECK_CLOSE(new_dist, (new_cp1 - new_cp2).norm(), epa_tolerance);
EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal,
epa_tolerance);
- Vec3f new_separation_vector = new_dist * new_distres.normal;
+ Vec3s new_separation_vector = new_dist * new_distres.normal;
EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1,
epa_tolerance);
@@ -179,12 +179,12 @@ void test_normal_and_nearest_points(
BOOST_CHECK_CLOSE(distres.min_distance, dist, dummy_precision);
BOOST_CHECK_CLOSE(dist, colres.distance_lower_bound, dummy_precision);
- Vec3f cp1 = distres.nearest_points[0];
- Vec3f cp2 = distres.nearest_points[1];
+ Vec3s cp1 = distres.nearest_points[0];
+ Vec3s cp2 = distres.nearest_points[1];
BOOST_CHECK_CLOSE(dist, (cp1 - cp2).norm(), gjk_tolerance);
EIGEN_VECTOR_IS_APPROX(cp1, cp2 - dist * distres.normal, gjk_tolerance);
- Vec3f separation_vector = dist * distres.normal;
+ Vec3s separation_vector = dist * distres.normal;
EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, gjk_tolerance);
if (dist > 0) {
@@ -214,11 +214,11 @@ void test_normal_and_nearest_points(
// tolerance
if (new_colres.isCollision()) {
Contact contact = new_colres.getContact(0);
- Vec3f new_cp1 = contact.nearest_points[0];
+ Vec3s new_cp1 = contact.nearest_points[0];
EIGEN_VECTOR_IS_APPROX(new_cp1, new_distres.nearest_points[0],
dummy_precision);
- Vec3f new_cp2 = contact.nearest_points[1];
+ Vec3s new_cp2 = contact.nearest_points[1];
EIGEN_VECTOR_IS_APPROX(new_cp2, new_distres.nearest_points[1],
dummy_precision);
BOOST_CHECK_CLOSE(contact.penetration_depth,
@@ -226,7 +226,7 @@ void test_normal_and_nearest_points(
EIGEN_VECTOR_IS_APPROX(new_cp1, new_cp2 - new_dist * new_distres.normal,
epa_tolerance);
- Vec3f new_separation_vector =
+ Vec3s new_separation_vector =
contact.penetration_depth * contact.normal;
EIGEN_VECTOR_IS_APPROX(new_separation_vector, new_cp2 - new_cp1,
epa_tolerance);
@@ -372,7 +372,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_plane) {
for (size_t i = 0; i < 10; ++i) {
shared_ptr<Box> o1(new Box(generateRandomVector<3>(0.05, 1.0)));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Plane> o2(new Plane(n, offset));
@@ -385,7 +385,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_box_halfspace) {
for (size_t i = 0; i < 10; ++i) {
shared_ptr<Box> o1(new Box(generateRandomVector<3>(0.05, 1.0)));
CoalScalar offset = 0.1;
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -400,7 +400,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_halfspace) {
CoalScalar h = generateRandomNumber(0.15, 1.0);
shared_ptr<Capsule> o1(new Capsule(r, h));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -413,7 +413,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_halfspace) {
for (size_t i = 0; i < 10; ++i) {
shared_ptr<Sphere> o1(new Sphere(generateRandomNumber(0.05, 1.0)));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -426,7 +426,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_plane) {
for (size_t i = 0; i < 10; ++i) {
shared_ptr<Sphere> o1(new Sphere(generateRandomNumber(0.05, 1.0)));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Plane> o2(new Plane(n, offset));
@@ -442,7 +442,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_mesh_halfspace) {
shared_ptr<Convex<Triangle>> o1(new Convex<Triangle>(
o1_.points, o1_.num_points, o1_.polygons, o1_.num_polygons));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -497,7 +497,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_halfspace) {
CoalScalar h = generateRandomNumber(0.15, 1.0);
shared_ptr<Cone> o1(new Cone(r, h));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -512,7 +512,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_halfspace) {
CoalScalar h = generateRandomNumber(0.15, 1.0);
shared_ptr<Cylinder> o1(new Cylinder(r, h));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -527,7 +527,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cone_plane) {
CoalScalar h = generateRandomNumber(0.15, 1.0);
shared_ptr<Cone> o1(new Cone(r, h));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Plane> o2(new Plane(n, offset));
@@ -542,7 +542,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_cylinder_plane) {
CoalScalar h = generateRandomNumber(0.15, 1.0);
shared_ptr<Cylinder> o1(new Cylinder(r, h));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Plane> o2(new Plane(n, offset));
@@ -557,7 +557,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_capsule_plane) {
CoalScalar h = generateRandomNumber(0.15, 1.0);
shared_ptr<Capsule> o1(new Capsule(r, h));
CoalScalar offset = generateRandomNumber(-0.5, 0.5);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Plane> o2(new Plane(n, offset));
@@ -593,7 +593,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_sphere_cylinder) {
BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) {
for (size_t i = 0; i < 10; ++i) {
CoalScalar offset = generateRandomNumber(0.15, 1.0);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Ellipsoid> o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
@@ -606,7 +606,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_halfspace) {
BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_ellipsoid_plane) {
for (size_t i = 0; i < 10; ++i) {
CoalScalar offset = generateRandomNumber(0.15, 1.0);
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Ellipsoid> o1(new Ellipsoid(generateRandomVector<3>(0.05, 1.0)));
shared_ptr<Plane> o2(new Plane(n, offset));
@@ -652,13 +652,13 @@ void test_normal_and_nearest_points(const BVHModel<OBBRSS>& o1,
BOOST_CHECK(contact.penetration_depth <= 0);
BOOST_CHECK(contact.penetration_depth >= colres.distance_lower_bound);
- Vec3f cp1 = contact.nearest_points[0];
- Vec3f cp2 = contact.nearest_points[1];
+ Vec3s cp1 = contact.nearest_points[0];
+ Vec3s cp2 = contact.nearest_points[1];
BOOST_CHECK_CLOSE(contact.penetration_depth, -(cp2 - cp1).norm(), 1e-6);
EIGEN_VECTOR_IS_APPROX(
cp1, cp2 - contact.penetration_depth * contact.normal, 1e-6);
- Vec3f separation_vector = contact.penetration_depth * contact.normal;
+ Vec3s separation_vector = contact.penetration_depth * contact.normal;
EIGEN_VECTOR_IS_APPROX(separation_vector, cp2 - cp1, 1e-6);
if (dist < 0) {
@@ -687,7 +687,7 @@ BOOST_AUTO_TEST_CASE(test_normal_and_nearest_points_bvh_halfspace) {
o1.buildConvexRepresentation(false);
CoalScalar offset = 0.1;
- Vec3f n = Vec3f::Random();
+ Vec3s n = Vec3s::Random();
n.normalize();
shared_ptr<Halfspace> o2(new Halfspace(n, offset));
diff --git a/test/obb.cpp b/test/obb.cpp
index d8db1dd8..77c16fe3 100644
--- a/test/obb.cpp
+++ b/test/obb.cpp
@@ -48,16 +48,16 @@
using namespace coal;
-void randomOBBs(Vec3f& a, Vec3f& b, CoalScalar extentNorm) {
+void randomOBBs(Vec3s& a, Vec3s& b, CoalScalar extentNorm) {
// Extent norm is between 0 and extentNorm on each axis
- // a = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3));
- // b = (Vec3f::Ones()+Vec3f::Random()) * extentNorm / (2*sqrt(3));
+ // a = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3));
+ // b = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3));
- a = extentNorm * Vec3f::Random().cwiseAbs().normalized();
- b = extentNorm * Vec3f::Random().cwiseAbs().normalized();
+ a = extentNorm * Vec3s::Random().cwiseAbs().normalized();
+ b = extentNorm * Vec3s::Random().cwiseAbs().normalized();
}
-void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b,
+void randomTransform(Matrix3s& B, Vec3s& T, const Vec3s& a, const Vec3s& b,
const CoalScalar extentNorm) {
// TODO Should we scale T to a and b norm ?
(void)a;
@@ -67,7 +67,7 @@ void randomTransform(Matrix3f& B, Vec3f& T, const Vec3f& a, const Vec3f& b,
CoalScalar N = a.norm() + b.norm();
// A translation of norm N ensures there is no collision.
// Set translation to be between 0 and 2 * N;
- T = (Vec3f::Random() / sqrt(3)) * 1.5 * N;
+ T = (Vec3s::Random() / sqrt(3)) * 1.5 * N;
// T.setZero();
Quatf q;
@@ -103,22 +103,22 @@ const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0,
namespace obbDisjoint_impls {
/// @return true if OBB are disjoint.
-bool distance(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b,
+bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,
CoalScalar& distance) {
GJKSolver gjk;
Box ba(2 * a), bb(2 * b);
Transform3f tfa, tfb(B, T);
- Vec3f p1, p2, normal;
+ Vec3s p1, p2, normal;
bool compute_penetration = true;
distance =
gjk.shapeDistance(ba, tfa, bb, tfb, compute_penetration, p1, p2, normal);
return (distance > gjk.getDistancePrecision(compute_penetration));
}
-inline CoalScalar _computeDistanceForCase1(const Vec3f& T, const Vec3f& a,
- const Vec3f& b, const Matrix3f& Bf) {
- Vec3f AABB_corner;
+inline CoalScalar _computeDistanceForCase1(const Vec3s& T, const Vec3s& a,
+ const Vec3s& b, const Matrix3s& Bf) {
+ Vec3s AABB_corner;
/* This seems to be slower
AABB_corner.noalias() = T.cwiseAbs () - a;
AABB_corner.noalias() -= PRODUCT(Bf,b);
@@ -135,11 +135,11 @@ inline CoalScalar _computeDistanceForCase1(const Vec3f& T, const Vec3f& a,
return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm();
}
-inline CoalScalar _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
- const Matrix3f& Bf) {
+inline CoalScalar _computeDistanceForCase2(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
+ const Matrix3s& Bf) {
/*
- Vec3f AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b);
+ Vec3s AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b);
AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a);
return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm ();
/*/
@@ -153,18 +153,18 @@ inline CoalScalar _computeDistanceForCase2(const Matrix3f& B, const Vec3f& T,
if (s > 0) t += s * s;
return t;
#else
- Vec3f AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b);
+ Vec3s AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b);
return AABB_corner.array().max(CoalScalar(0)).matrix().squaredNorm();
#endif
// */
}
-int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
- const Vec3f& b, const CoalScalar& breakDistance2,
+int separatingAxisId(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b, const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
int id = 0;
- Matrix3f Bf(B.cwiseAbs());
+ Matrix3s Bf(B.cwiseAbs());
// Corner of b axis aligned bounding box the closest to the origin
squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -216,10 +216,10 @@ int separatingAxisId(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
}
// ------------------------ 0 --------------------------------------
-bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
- const Vec3f& b, const CoalScalar& breakDistance2,
+bool withRuntimeLoop(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b, const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
- Matrix3f Bf(B.cwiseAbs());
+ Matrix3s Bf(B.cwiseAbs());
// Corner of b axis aligned bounding box the closest to the origin
squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -268,30 +268,30 @@ bool withRuntimeLoop(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
}
// ------------------------ 1 --------------------------------------
-bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
+bool withManualLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
CoalScalar t, s;
CoalScalar diff;
- // Matrix3f Bf = abs(B);
+ // Matrix3s Bf = abs(B);
// Bf += reps;
- Matrix3f Bf(B.cwiseAbs());
+ Matrix3s Bf(B.cwiseAbs());
// Corner of b axis aligned bounding box the closest to the origin
- Vec3f AABB_corner(T.cwiseAbs() - Bf * b);
- Vec3f diff3(AABB_corner - a);
- diff3 = diff3.cwiseMax(Vec3f::Zero());
+ Vec3s AABB_corner(T.cwiseAbs() - Bf * b);
+ Vec3s diff3(AABB_corner - a);
+ diff3 = diff3.cwiseMax(Vec3s::Zero());
- // for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
+ // for (Vec3s::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
squaredLowerBoundDistance = diff3.squaredNorm();
if (squaredLowerBoundDistance > breakDistance2) return true;
AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a;
// diff3 = | B^T T| - b - Bf^T a
diff3 = AABB_corner - b;
- diff3 = diff3.cwiseMax(Vec3f::Zero());
+ diff3 = diff3.cwiseMax(Vec3s::Zero());
squaredLowerBoundDistance = diff3.squaredNorm();
if (squaredLowerBoundDistance > breakDistance2) return true;
@@ -458,11 +458,11 @@ bool withManualLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
}
// ------------------------ 2 --------------------------------------
-bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
+bool withManualLoopUnrolling_2(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
- Matrix3f Bf(B.cwiseAbs());
+ Matrix3s Bf(B.cwiseAbs());
// Corner of b axis aligned bounding box the closest to the origin
squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -638,8 +638,8 @@ bool withManualLoopUnrolling_2(const Matrix3f& B, const Vec3f& T,
template <int ia, int ib, int ja = (ia + 1) % 3, int ka = (ia + 2) % 3,
int jb = (ib + 1) % 3, int kb = (ib + 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,
+ static inline bool run(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b, const Matrix3s& Bf,
const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
@@ -669,11 +669,11 @@ struct loop_case_1 {
}
};
-bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
+bool withTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
- Matrix3f Bf(B.cwiseAbs());
+ Matrix3s Bf(B.cwiseAbs());
// Corner of b axis aligned bounding box the closest to the origin
squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -718,9 +718,9 @@ bool withTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
template <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 CoalScalar& breakDistance2,
+ static inline bool run(int ia, int ja, int ka, const Matrix3s& B,
+ const Vec3s& T, const Vec3s& a, const Vec3s& b,
+ const Matrix3s& Bf, const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
const CoalScalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
@@ -749,11 +749,11 @@ struct loop_case_2 {
}
};
-bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
+bool withPartialTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
- Matrix3f Bf(B.cwiseAbs());
+ Matrix3s Bf(B.cwiseAbs());
// Corner of b axis aligned bounding box the closest to the origin
squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
@@ -782,13 +782,13 @@ bool withPartialTemplateLoopUnrolling_1(const Matrix3f& B, const Vec3f& T,
}
// ------------------------ 5 --------------------------------------
-bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
- const Vec3f& b, const CoalScalar& breakDistance2,
+bool originalWithLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b, const CoalScalar& breakDistance2,
CoalScalar& squaredLowerBoundDistance) {
CoalScalar t, s;
CoalScalar diff;
- Matrix3f Bf(B.cwiseAbs());
+ Matrix3s Bf(B.cwiseAbs());
squaredLowerBoundDistance = 0;
// if any of these tests are one-sided, then the polyhedra are disjoint
@@ -1003,15 +1003,15 @@ bool originalWithLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
}
// ------------------------ 6 --------------------------------------
-bool originalWithNoLowerBound(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
- const Vec3f& b, const CoalScalar&,
+bool originalWithNoLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
+ const Vec3s& b, const CoalScalar&,
CoalScalar& squaredLowerBoundDistance) {
CoalScalar t, s;
const CoalScalar reps = 1e-6;
squaredLowerBoundDistance = 0;
- Matrix3f Bf(B.array().abs() + reps);
+ Matrix3s Bf(B.array().abs() + reps);
// Bf += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
@@ -1173,8 +1173,8 @@ std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) {
return br.print(os);
}
-BenchmarkResult benchmark_obb_case(const Matrix3f& B, const Vec3f& T,
- const Vec3f& a, const Vec3f& b,
+BenchmarkResult benchmark_obb_case(const Matrix3s& B, const Vec3s& T,
+ const Vec3s& a, const Vec3s& b,
const CollisionRequest& request,
std::size_t N) {
const CoalScalar breakDistance(request.break_distance +
@@ -1283,9 +1283,9 @@ std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) {
std::size_t nbFailure = 0;
// Create two OBBs axis
- Vec3f a, b;
- Matrix3f B;
- Vec3f T;
+ Vec3s a, b;
+ Matrix3s B;
+ Vec3s T;
CollisionRequest request;
#ifndef NDEBUG // if debug mode
diff --git a/test/octree.cpp b/test/octree.cpp
index 894c2803..9cedd2e6 100644
--- a/test/octree.cpp
+++ b/test/octree.cpp
@@ -55,7 +55,7 @@ namespace utf = boost::unit_test::framework;
using namespace coal;
-void makeMesh(const std::vector<Vec3f>& vertices,
+void makeMesh(const std::vector<Vec3s>& vertices,
const std::vector<Triangle>& triangles, BVHModel<OBBRSS>& model) {
coal::SplitMethodType split_method(coal::SPLIT_METHOD_MEAN);
model.bv_splitter.reset(new BVSplitter<OBBRSS>(split_method));
@@ -68,8 +68,8 @@ void makeMesh(const std::vector<Vec3f>& vertices,
coal::OcTree makeOctree(const BVHModel<OBBRSS>& mesh,
const CoalScalar& resolution) {
- Vec3f m(mesh.aabb_local.min_);
- Vec3f M(mesh.aabb_local.max_);
+ Vec3s m(mesh.aabb_local.min_);
+ Vec3s M(mesh.aabb_local.max_);
coal::Box box(resolution, resolution, resolution);
CollisionRequest request(coal::CONTACT | coal::DISTANCE_LOWER_BOUND, 1);
CollisionResult result;
@@ -82,7 +82,7 @@ coal::OcTree makeOctree(const BVHModel<OBBRSS>& mesh,
y += resolution) {
for (CoalScalar z = resolution * floor(m[2] / resolution); z <= M[2];
z += resolution) {
- Vec3f center(x + .5 * resolution, y + .5 * resolution,
+ Vec3s center(x + .5 * resolution, y + .5 * resolution,
z + .5 * resolution);
tfBox.setTranslation(center);
coal::collide(&box, tfBox, &mesh, Transform3f(), request, result);
@@ -105,7 +105,7 @@ BOOST_AUTO_TEST_CASE(octree_mesh) {
Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
"", "", "(", ")");
CoalScalar resolution(10.);
- std::vector<Vec3f> pRob, pEnv;
+ std::vector<Vec3s> pRob, pEnv;
std::vector<Triangle> tRob, tEnv;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "rob.obj").string().c_str(), pRob, tRob);
@@ -179,7 +179,7 @@ BOOST_AUTO_TEST_CASE(octree_height_field) {
Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
"", "", "(", ")");
CoalScalar resolution(10.);
- std::vector<Vec3f> pEnv;
+ std::vector<Vec3s> pEnv;
std::vector<Triangle> tEnv;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), pEnv, tEnv);
@@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(octree_height_field) {
const CoalScalar x_dim = 10, y_dim = 20;
const int nx = 100, ny = 100;
const CoalScalar max_altitude = 1., min_altitude = 0.;
- const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);
+ const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude);
HeightField<AABB> hfield(x_dim, y_dim, heights, min_altitude);
hfield.computeLocalAABB();
diff --git a/test/profiling.cpp b/test/profiling.cpp
index bdc5eb15..5319e422 100644
--- a/test/profiling.cpp
+++ b/test/profiling.cpp
@@ -44,7 +44,7 @@ bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2) {
template <typename BV /*, SplitMethodType split_method*/>
CollisionGeometryPtr_t objToGeom(const std::string& filename) {
- std::vector<Vec3f> pt;
+ std::vector<Vec3s> pt;
std::vector<Triangle> tri;
loadOBJFile(filename.c_str(), pt, tri);
@@ -60,7 +60,7 @@ CollisionGeometryPtr_t objToGeom(const std::string& filename) {
template <typename BV /*, SplitMethodType split_method*/>
CollisionGeometryPtr_t meshToGeom(const std::string& filename,
- const Vec3f& scale = Vec3f(1, 1, 1)) {
+ const Vec3s& scale = Vec3s(1, 1, 1)) {
shared_ptr<BVHModel<BV> > model(new BVHModel<BV>());
loadPolyhedronFromResource(filename, scale, model);
return model;
@@ -190,9 +190,9 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv) {
iarg += 3;
if (iarg < argc && strcmp(argv[iarg], "crop") == 0) {
CHECK_PARAM_NB(6, Crop);
- coal::AABB aabb(Vec3f(atof(argv[iarg + 1]), atof(argv[iarg + 2]),
+ coal::AABB aabb(Vec3s(atof(argv[iarg + 1]), atof(argv[iarg + 2]),
atof(argv[iarg + 3])),
- Vec3f(atof(argv[iarg + 4]), atof(argv[iarg + 5]),
+ Vec3s(atof(argv[iarg + 4]), atof(argv[iarg + 5]),
atof(argv[iarg + 6])));
OUT("Cropping " << aabb.min_.transpose() << " ---- "
<< aabb.max_.transpose() << " ...");
@@ -249,11 +249,11 @@ int main(int argc, char** argv) {
geoms.push_back(Geometry("Cone", new Cone(2, 1)));
geoms.push_back(Geometry("Cylinder", new Cylinder(2, 1)));
// geoms.push_back(Geometry ("Plane" , new Plane
- // (Vec3f(1,1,1).normalized(), 0) ));
+ // (Vec3s(1,1,1).normalized(), 0) ));
// geoms.push_back(Geometry ("Halfspace" , new Halfspace
- // (Vec3f(1,1,1).normalized(), 0) ));
+ // (Vec3s(1,1,1).normalized(), 0) ));
// not implemented // geoms.push_back(Geometry ("TriangleP" , new TriangleP
- // (Vec3f(0,1,0), Vec3f(0,0,1), Vec3f(-1,0,0)) ));
+ // (Vec3s(0,1,0), Vec3s(0,0,1), Vec3s(-1,0,0)) ));
geoms.push_back(Geometry("rob BVHModel<OBB>",
objToGeom<OBB>((path / "rob.obj").string())));
diff --git a/test/python_unit/collision.py b/test/python_unit/collision.py
index 61cdcd24..fbf543bc 100644
--- a/test/python_unit/collision.py
+++ b/test/python_unit/collision.py
@@ -6,7 +6,7 @@ import numpy as np
def tetahedron():
- pts = coal.StdVec_Vec3f()
+ pts = coal.StdVec_Vec3s()
pts.append(np.array((0, 0, 0)))
pts.append(np.array((0, 1, 0)))
pts.append(np.array((1, 0, 0)))
diff --git a/test/python_unit/geometric_shapes.py b/test/python_unit/geometric_shapes.py
index 57e03e21..c7774e31 100644
--- a/test/python_unit/geometric_shapes.py
+++ b/test/python_unit/geometric_shapes.py
@@ -160,7 +160,7 @@ class TestGeometricShapes(TestCase):
self.assertEqual(bvh.vertices().shape, (0, 3))
def test_convex(self):
- verts = coal.StdVec_Vec3f()
+ verts = coal.StdVec_Vec3s()
faces = coal.StdVec_Triangle()
verts.extend(
[
diff --git a/test/python_unit/pickling.py b/test/python_unit/pickling.py
index 8a94bde0..c6143d87 100644
--- a/test/python_unit/pickling.py
+++ b/test/python_unit/pickling.py
@@ -7,7 +7,7 @@ import numpy as np
def tetahedron():
- pts = coal.StdVec_Vec3f()
+ pts = coal.StdVec_Vec3s()
pts.append(np.array((0, 0, 0)))
pts.append(np.array((0, 1, 0)))
pts.append(np.array((1, 0, 0)))
diff --git a/test/security_margin.cpp b/test/security_margin.cpp
index 1f05503f..c56dba7c 100644
--- a/test/security_margin.cpp
+++ b/test/security_margin.cpp
@@ -55,7 +55,7 @@ using coal::CollisionResult;
using coal::DistanceRequest;
using coal::DistanceResult;
using coal::Transform3f;
-using coal::Vec3f;
+using coal::Vec3s;
#define MATH_SQUARED(x) (x * x)
@@ -64,7 +64,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1));
const Transform3f tf1;
- const Transform3f tf2_collision(Vec3f(0, 1, 1));
+ const Transform3f tf2_collision(Vec3s(0, 1, 1));
coal::Box s1(1, 1, 1);
coal::Box s2(1, 1, 1);
const double tol = 1e-8;
@@ -90,7 +90,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
CollisionRequest collisionRequest(CONTACT, 1);
const double distance = 0.01;
Transform3f tf2_no_collision(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
AABB bv2_transformed;
computeBV(s2, tf2_no_collision, bv2_transformed);
CoalScalar sqrDistLowerBound;
@@ -106,7 +106,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
const double distance = 0.01;
collisionRequest.security_margin = distance;
Transform3f tf2_no_collision(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
AABB bv2_transformed;
computeBV(s2, tf2_no_collision, bv2_transformed);
CoalScalar sqrDistLowerBound;
@@ -122,7 +122,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb) {
const double distance = -0.01;
collisionRequest.security_margin = distance;
const Transform3f tf2(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, distance)));
AABB bv2_transformed;
computeBV(s2, tf2, bv2_transformed);
CoalScalar sqrDistLowerBound;
@@ -154,7 +154,7 @@ BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) {
CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1));
const Transform3f tf1;
- const Transform3f tf2_collision(Vec3f(0, 0, 0));
+ const Transform3f tf2_collision(Vec3s(0, 0, 0));
coal::Box s1(1, 1, 1);
coal::Box s2(1, 1, 1);
@@ -184,7 +184,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
CollisionGeometryPtr_t s2(new coal::Sphere(2));
const Transform3f tf1;
- const Transform3f tf2_collision(Vec3f(0, 0, 3));
+ const Transform3f tf2_collision(Vec3s(0, 0, 3));
// NOTE: when comparing a result to zero, **do not use BOOST_CHECK_CLOSE**!
// Zero is not close to any other number, so the test will always fail.
@@ -207,7 +207,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
CollisionResult collisionResult;
const double distance = 0.01;
Transform3f tf2_no_collision(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
collide(s1.get(), tf1, s2.get(), tf2_no_collision, collisionRequest,
collisionResult);
BOOST_CHECK(!collisionResult.isCollision());
@@ -221,7 +221,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
const double distance = 0.01;
collisionRequest.security_margin = distance;
Transform3f tf2(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
@@ -236,7 +236,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) {
const double distance = -0.01;
collisionRequest.security_margin = distance;
Transform3f tf2(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, 0, distance)));
collide(s1.get(), tf1, s2.get(), tf2, collisionRequest, collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
@@ -262,7 +262,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
CollisionGeometryPtr_t c2(new coal::Capsule(0.5, 1.));
const Transform3f tf1;
- const Transform3f tf2_collision(Vec3f(0, 1., 0));
+ const Transform3f tf2_collision(Vec3s(0, 1., 0));
// No security margin - collision
{
@@ -281,7 +281,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
CollisionResult collisionResult;
const double distance = 0.01;
Transform3f tf2_no_collision(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0)));
collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest,
collisionResult);
BOOST_CHECK(!collisionResult.isCollision());
@@ -295,7 +295,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
const double distance = 0.01;
collisionRequest.security_margin = distance;
Transform3f tf2_no_collision(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0)));
collide(c1.get(), tf1, c2.get(), tf2_no_collision, collisionRequest,
collisionResult);
BOOST_CHECK(collisionResult.isCollision());
@@ -311,7 +311,7 @@ BOOST_AUTO_TEST_CASE(capsule_capsule) {
const double distance = -0.01;
collisionRequest.security_margin = distance;
Transform3f tf2(
- Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, 0)));
+ Vec3s(tf2_collision.getTranslation() + Vec3s(0, distance, 0)));
collide(c1.get(), tf1, c2.get(), tf2, collisionRequest, collisionResult);
BOOST_CHECK(collisionResult.isCollision());
BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, 1e-8);
@@ -336,7 +336,7 @@ BOOST_AUTO_TEST_CASE(box_box) {
CollisionGeometryPtr_t b2(new coal::Box(1, 1, 1));
const Transform3f tf1;
- const Transform3f tf2_collision(Vec3f(0, 1, 1));
+ const Transform3f tf2_collision(Vec3s(0, 1, 1));
const double tol = 1e-3;
@@ -356,7 +356,7 @@ BOOST_AUTO_TEST_CASE(box_box) {
CollisionRequest collisionRequest(CONTACT, 1);
const double distance = 0.01;
const Transform3f tf2_no_collision(
- (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval());
+ (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval());
CollisionResult collisionResult;
collide(b1.get(), tf1, b2.get(), tf2_no_collision, collisionRequest,
@@ -399,7 +399,7 @@ BOOST_AUTO_TEST_CASE(box_box) {
CollisionResult collisionResult;
const Transform3f tf2((tf2_collision.getTranslation() +
- Vec3f(0, collisionRequest.security_margin,
+ Vec3s(0, collisionRequest.security_margin,
collisionRequest.security_margin))
.eval());
collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult);
@@ -430,7 +430,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
CollisionRequest collisionRequest(CONTACT, 1);
const double distance = 0.01;
const Transform3f tf2_no_collision(
- (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval());
+ (tf2_collision.getTranslation() + Vec3s(0, 0, distance)).eval());
CollisionResult collisionResult;
collide(&shape1, tf1, &shape2, tf2_no_collision, collisionRequest,
@@ -476,7 +476,7 @@ void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1,
CollisionResult collisionResult;
const Transform3f tf2((tf2_collision.getTranslation() +
- Vec3f(0, collisionRequest.security_margin,
+ Vec3s(0, collisionRequest.security_margin,
collisionRequest.security_margin))
.eval());
collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult);
@@ -497,7 +497,7 @@ BOOST_AUTO_TEST_CASE(sphere_box) {
CollisionGeometryPtr_t s2(new coal::Sphere(0.5));
const Transform3f tf1;
- const Transform3f tf2_collision(Vec3f(0, 0, 1));
+ const Transform3f tf2_collision(Vec3s(0, 0, 1));
const double tol = 1e-6;
diff --git a/test/serialization.cpp b/test/serialization.cpp
index 3d0769f8..c1dc66fb 100644
--- a/test/serialization.cpp
+++ b/test/serialization.cpp
@@ -250,12 +250,12 @@ void test_serialization(const T& value,
}
BOOST_AUTO_TEST_CASE(test_aabb) {
- AABB aabb(-Vec3f::Ones(), Vec3f::Ones());
+ AABB aabb(-Vec3s::Ones(), Vec3s::Ones());
test_serialization(aabb);
}
BOOST_AUTO_TEST_CASE(test_collision_data) {
- Contact contact(NULL, NULL, 1, 2, Vec3f::Ones(), Vec3f::Zero(), -10.);
+ Contact contact(NULL, NULL, 1, 2, Vec3s::Ones(), Vec3s::Zero(), -10.);
test_serialization(contact);
CollisionRequest collision_request(CONTACT, 10);
@@ -290,7 +290,7 @@ BOOST_AUTO_TEST_CASE(test_collision_data) {
Transform3f tf2;
// set translation to have a collision
const CoalScalar offset = 0.001;
- tf2.setTranslation(Vec3f(0, 0, height / 2 - offset));
+ tf2.setTranslation(Vec3s(0, 0, height / 2 - offset));
const size_t num_max_contact = 1;
const CollisionRequest col_req(CollisionRequestFlag::CONTACT,
@@ -326,7 +326,7 @@ void checkEqualStdVector(const std::vector<T>& v1, const std::vector<T>& v2) {
}
BOOST_AUTO_TEST_CASE(test_BVHModel) {
- std::vector<Vec3f> p1, p2;
+ std::vector<Vec3s> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
@@ -367,7 +367,7 @@ BOOST_AUTO_TEST_CASE(test_BVHModel) {
#ifdef COAL_HAS_QHULL
BOOST_AUTO_TEST_CASE(test_Convex) {
- std::vector<Vec3f> p1;
+ std::vector<Vec3s> p1;
std::vector<Triangle> t1;
boost::filesystem::path path(TEST_RESOURCES_DIR);
@@ -420,7 +420,7 @@ BOOST_AUTO_TEST_CASE(test_HeightField) {
const CoalScalar min_altitude = -1.;
const CoalScalar x_dim = 1., y_dim = 2.;
const Eigen::DenseIndex nx = 100, ny = 200;
- const MatrixXf heights = MatrixXf::Random(ny, nx);
+ const MatrixXs heights = MatrixXs::Random(ny, nx);
HeightField<OBBRSS> hfield(x_dim, y_dim, heights, min_altitude);
@@ -438,7 +438,7 @@ BOOST_AUTO_TEST_CASE(test_HeightField) {
BOOST_AUTO_TEST_CASE(test_transform) {
Transform3f T;
T.setQuatRotation(Quaternion3f::UnitRandom());
- T.setTranslation(Vec3f::Random());
+ T.setTranslation(Vec3s::Random());
Transform3f T_copy;
test_serialization(T, T_copy);
@@ -446,15 +446,15 @@ BOOST_AUTO_TEST_CASE(test_transform) {
BOOST_AUTO_TEST_CASE(test_shapes) {
{
- TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), Vec3f::UnitZ());
+ TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(), Vec3s::UnitZ());
triangle.setSweptSphereRadius(1.);
triangle.computeLocalAABB();
- TriangleP triangle_copy(Vec3f::Random(), Vec3f::Random(), Vec3f::Random());
+ TriangleP triangle_copy(Vec3s::Random(), Vec3s::Random(), Vec3s::Random());
test_serialization(triangle, triangle_copy);
}
{
- Box box(Vec3f::UnitX()), box_copy(Vec3f::Random());
+ Box box(Vec3s::UnitX()), box_copy(Vec3s::Random());
box.setSweptSphereRadius(1.);
box.computeLocalAABB();
test_serialization(box, box_copy);
@@ -496,14 +496,14 @@ BOOST_AUTO_TEST_CASE(test_shapes) {
}
{
- Halfspace hs(Vec3f::Random(), 1.), hs_copy(Vec3f::Zero(), 0.);
+ Halfspace hs(Vec3s::Random(), 1.), hs_copy(Vec3s::Zero(), 0.);
hs.setSweptSphereRadius(1.);
hs.computeLocalAABB();
test_serialization(hs, hs_copy);
}
{
- Plane plane(Vec3f::Random(), 1.), plane_copy(Vec3f::Zero(), 0.);
+ Plane plane(Vec3s::Random(), 1.), plane_copy(Vec3s::Zero(), 0.);
plane.setSweptSphereRadius(1.);
plane.computeLocalAABB();
test_serialization(plane, plane_copy);
@@ -511,11 +511,11 @@ BOOST_AUTO_TEST_CASE(test_shapes) {
{
const size_t num_points = 500;
- std::shared_ptr<std::vector<Vec3f>> points =
- std::make_shared<std::vector<Vec3f>>();
+ std::shared_ptr<std::vector<Vec3s>> points =
+ std::make_shared<std::vector<Vec3s>>();
points->reserve(num_points);
for (size_t i = 0; i < num_points; i++) {
- points->emplace_back(Vec3f::Random());
+ points->emplace_back(Vec3s::Random());
}
using Convex = Convex<Triangle>;
std::unique_ptr<Convex> convex =
@@ -532,7 +532,7 @@ BOOST_AUTO_TEST_CASE(test_shapes) {
#ifdef COAL_HAS_OCTOMAP
BOOST_AUTO_TEST_CASE(test_octree) {
const CoalScalar resolution = 1e-2;
- const Matrixx3f points = Matrixx3f::Random(1000, 3);
+ const MatrixX3s points = MatrixX3s::Random(1000, 3);
OcTreePtr_t octree_ptr = makeOctree(points, resolution);
const OcTree& octree = *octree_ptr.get();
@@ -572,7 +572,7 @@ BOOST_AUTO_TEST_CASE(test_memory_footprint) {
Sphere sphere(1.);
BOOST_CHECK(sizeof(Sphere) == computeMemoryFootprint(sphere));
- std::vector<Vec3f> p1;
+ std::vector<Vec3s> p1;
std::vector<Triangle> t1;
boost::filesystem::path path(TEST_RESOURCES_DIR);
diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp
index 3a32cb6c..a571c380 100644
--- a/test/shape_inflation.cpp
+++ b/test/shape_inflation.cpp
@@ -54,7 +54,7 @@ using coal::CollisionResult;
using coal::DistanceRequest;
using coal::DistanceResult;
using coal::Transform3f;
-using coal::Vec3f;
+using coal::Vec3s;
#define MATH_SQUARED(x) (x * x)
@@ -190,10 +190,10 @@ BOOST_AUTO_TEST_CASE(test_inflate) {
test(cone, 0.01, 1e-8);
test_throw(cone, -1.1);
- const coal::Halfspace halfspace(Vec3f::UnitZ(), 0.);
+ const coal::Halfspace halfspace(Vec3s::UnitZ(), 0.);
test(halfspace, 0.01, 1e-8);
- // const coal::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(),
- // Vec3f::UnitZ());
+ // const coal::TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(),
+ // Vec3s::UnitZ());
// test(triangle, 0.01, 1e-8);
}
diff --git a/test/shape_mesh_consistency.cpp b/test/shape_mesh_consistency.cpp
index 9dfd5db8..d234602e 100644
--- a/test/shape_mesh_consistency.cpp
+++ b/test/shape_mesh_consistency.cpp
@@ -64,7 +64,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) {
Transform3f pose;
- pose.setTranslation(Vec3f(50, 0, 0));
+ pose.setTranslation(Vec3s(50, 0, 0));
res.clear();
res1.clear();
@@ -108,7 +108,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere) {
0.05);
}
- pose.setTranslation(Vec3f(40.1, 0, 0));
+ pose.setTranslation(Vec3s(40.1, 0, 0));
res.clear(), res1.clear();
distance(&s1, Transform3f(), &s2, pose, request, res);
@@ -167,7 +167,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) {
Transform3f pose;
- pose.setTranslation(Vec3f(50, 0, 0));
+ pose.setTranslation(Vec3s(50, 0, 0));
res.clear();
res1.clear();
@@ -211,7 +211,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox) {
0.01);
}
- pose.setTranslation(Vec3f(15.1, 0, 0));
+ pose.setTranslation(Vec3s(15.1, 0, 0));
res.clear();
res1.clear();
@@ -271,7 +271,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) {
Transform3f pose;
- pose.setTranslation(Vec3f(20, 0, 0));
+ pose.setTranslation(Vec3s(20, 0, 0));
res.clear();
res1.clear();
@@ -316,7 +316,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder) {
}
pose.setTranslation(
- Vec3f(15, 0, 0)); // libccd cannot use small value here :(
+ Vec3s(15, 0, 0)); // libccd cannot use small value here :(
res.clear();
res1.clear();
@@ -376,7 +376,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) {
Transform3f pose;
- pose.setTranslation(Vec3f(20, 0, 0));
+ pose.setTranslation(Vec3s(20, 0, 0));
res.clear();
res1.clear();
@@ -420,7 +420,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone) {
}
pose.setTranslation(
- Vec3f(15, 0, 0)); // libccd cannot use small value here :(
+ Vec3s(15, 0, 0)); // libccd cannot use small value here :(
res.clear();
res1.clear();
@@ -479,7 +479,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) {
Transform3f pose;
- pose.setTranslation(Vec3f(50, 0, 0));
+ pose.setTranslation(Vec3s(50, 0, 0));
res.clear();
res1.clear();
@@ -523,7 +523,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) {
0.05);
}
- pose.setTranslation(Vec3f(40.1, 0, 0));
+ pose.setTranslation(Vec3s(40.1, 0, 0));
res.clear();
res1.clear();
@@ -583,7 +583,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) {
Transform3f pose;
- pose.setTranslation(Vec3f(50, 0, 0));
+ pose.setTranslation(Vec3s(50, 0, 0));
res.clear();
res1.clear();
@@ -627,7 +627,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) {
0.01);
}
- pose.setTranslation(Vec3f(15.1, 0, 0));
+ pose.setTranslation(Vec3s(15.1, 0, 0));
res.clear();
res1.clear();
@@ -687,7 +687,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) {
Transform3f pose;
- pose.setTranslation(Vec3f(20, 0, 0));
+ pose.setTranslation(Vec3s(20, 0, 0));
res.clear();
res1.clear();
@@ -743,7 +743,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) {
fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
}
- pose.setTranslation(Vec3f(10.1, 0, 0));
+ pose.setTranslation(Vec3s(10.1, 0, 0));
res.clear();
res1.clear();
@@ -803,7 +803,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) {
Transform3f pose;
- pose.setTranslation(Vec3f(20, 0, 0));
+ pose.setTranslation(Vec3s(20, 0, 0));
res.clear();
res1.clear();
@@ -847,7 +847,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) {
0.05);
}
- pose.setTranslation(Vec3f(10.1, 0, 0));
+ pose.setTranslation(Vec3s(10.1, 0, 0));
res.clear();
res1.clear();
@@ -945,9 +945,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(40, 0, 0));
- pose_aabb.setTranslation(Vec3f(40, 0, 0));
- pose_obb.setTranslation(Vec3f(40, 0, 0));
+ pose.setTranslation(Vec3s(40, 0, 0));
+ pose_aabb.setTranslation(Vec3s(40, 0, 0));
+ pose_obb.setTranslation(Vec3s(40, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -977,9 +977,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(30, 0, 0));
- pose_aabb.setTranslation(Vec3f(30, 0, 0));
- pose_obb.setTranslation(Vec3f(30, 0, 0));
+ pose.setTranslation(Vec3s(30, 0, 0));
+ pose_aabb.setTranslation(Vec3s(30, 0, 0));
+ pose_obb.setTranslation(Vec3s(30, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1009,11 +1009,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(29.9, 0, 0));
+ pose.setTranslation(Vec3s(29.9, 0, 0));
pose_aabb.setTranslation(
- Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
pose_obb.setTranslation(
- Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1043,11 +1043,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(-29.9, 0, 0));
+ pose.setTranslation(Vec3s(-29.9, 0, 0));
pose_aabb.setTranslation(
- Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
pose_obb.setTranslation(
- Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1077,9 +1077,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(-30, 0, 0));
- pose_aabb.setTranslation(Vec3f(-30, 0, 0));
- pose_obb.setTranslation(Vec3f(-30, 0, 0));
+ pose.setTranslation(Vec3s(-30, 0, 0));
+ pose_aabb.setTranslation(Vec3s(-30, 0, 0));
+ pose_obb.setTranslation(Vec3s(-30, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1164,9 +1164,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(15.01, 0, 0));
- pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
- pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+ pose.setTranslation(Vec3s(15.01, 0, 0));
+ pose_aabb.setTranslation(Vec3s(15.01, 0, 0));
+ pose_obb.setTranslation(Vec3s(15.01, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1196,9 +1196,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(14.99, 0, 0));
- pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
- pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+ pose.setTranslation(Vec3s(14.99, 0, 0));
+ pose_aabb.setTranslation(Vec3s(14.99, 0, 0));
+ pose_obb.setTranslation(Vec3s(14.99, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1283,9 +1283,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(22.4, 0, 0));
- pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
- pose_obb.setTranslation(Vec3f(22.4, 0, 0));
+ pose.setTranslation(Vec3s(22.4, 0, 0));
+ pose_aabb.setTranslation(Vec3s(22.4, 0, 0));
+ pose_obb.setTranslation(Vec3s(22.4, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1315,9 +1315,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(22.51, 0, 0));
- pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
- pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+ pose.setTranslation(Vec3s(22.51, 0, 0));
+ pose_aabb.setTranslation(Vec3s(22.51, 0, 0));
+ pose_obb.setTranslation(Vec3s(22.51, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1369,9 +1369,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) {
Transform3f pose, pose_aabb, pose_obb;
- pose.setTranslation(Vec3f(9.99, 0, 0));
- pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
- pose_obb.setTranslation(Vec3f(9.99, 0, 0));
+ pose.setTranslation(Vec3s(9.99, 0, 0));
+ pose_aabb.setTranslation(Vec3s(9.99, 0, 0));
+ pose_obb.setTranslation(Vec3s(9.99, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1401,9 +1401,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(10.01, 0, 0));
- pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
- pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+ pose.setTranslation(Vec3s(10.01, 0, 0));
+ pose_aabb.setTranslation(Vec3s(10.01, 0, 0));
+ pose_obb.setTranslation(Vec3s(10.01, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1455,9 +1455,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) {
Transform3f pose, pose_aabb, pose_obb;
- pose.setTranslation(Vec3f(9.9, 0, 0));
- pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
- pose_obb.setTranslation(Vec3f(9.9, 0, 0));
+ pose.setTranslation(Vec3s(9.9, 0, 0));
+ pose_aabb.setTranslation(Vec3s(9.9, 0, 0));
+ pose_obb.setTranslation(Vec3s(9.9, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1487,9 +1487,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(10.1, 0, 0));
- pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
- pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+ pose.setTranslation(Vec3s(10.1, 0, 0));
+ pose_aabb.setTranslation(Vec3s(10.1, 0, 0));
+ pose_obb.setTranslation(Vec3s(10.1, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1519,9 +1519,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(0, 0, 9.9));
- pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
- pose_obb.setTranslation(Vec3f(0, 0, 9.9));
+ pose.setTranslation(Vec3s(0, 0, 9.9));
+ pose_aabb.setTranslation(Vec3s(0, 0, 9.9));
+ pose_obb.setTranslation(Vec3s(0, 0, 9.9));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1551,9 +1551,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(0, 0, 10.1));
- pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
- pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+ pose.setTranslation(Vec3s(0, 0, 10.1));
+ pose_aabb.setTranslation(Vec3s(0, 0, 10.1));
+ pose_obb.setTranslation(Vec3s(0, 0, 10.1));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1638,9 +1638,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(40, 0, 0));
- pose_aabb.setTranslation(Vec3f(40, 0, 0));
- pose_obb.setTranslation(Vec3f(40, 0, 0));
+ pose.setTranslation(Vec3s(40, 0, 0));
+ pose_aabb.setTranslation(Vec3s(40, 0, 0));
+ pose_obb.setTranslation(Vec3s(40, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1670,9 +1670,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(30, 0, 0));
- pose_aabb.setTranslation(Vec3f(30, 0, 0));
- pose_obb.setTranslation(Vec3f(30, 0, 0));
+ pose.setTranslation(Vec3s(30, 0, 0));
+ pose_aabb.setTranslation(Vec3s(30, 0, 0));
+ pose_obb.setTranslation(Vec3s(30, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1702,11 +1702,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(29.9, 0, 0));
+ pose.setTranslation(Vec3s(29.9, 0, 0));
pose_aabb.setTranslation(
- Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
pose_obb.setTranslation(
- Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1736,11 +1736,11 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(-29.9, 0, 0));
+ pose.setTranslation(Vec3s(-29.9, 0, 0));
pose_aabb.setTranslation(
- Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
pose_obb.setTranslation(
- Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+ Vec3s(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1770,9 +1770,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(-30, 0, 0));
- pose_aabb.setTranslation(Vec3f(-30, 0, 0));
- pose_obb.setTranslation(Vec3f(-30, 0, 0));
+ pose.setTranslation(Vec3s(-30, 0, 0));
+ pose_aabb.setTranslation(Vec3s(-30, 0, 0));
+ pose_obb.setTranslation(Vec3s(-30, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1858,9 +1858,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(15.01, 0, 0));
- pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
- pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+ pose.setTranslation(Vec3s(15.01, 0, 0));
+ pose_aabb.setTranslation(Vec3s(15.01, 0, 0));
+ pose_obb.setTranslation(Vec3s(15.01, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1890,9 +1890,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(14.99, 0, 0));
- pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
- pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+ pose.setTranslation(Vec3s(14.99, 0, 0));
+ pose_aabb.setTranslation(Vec3s(14.99, 0, 0));
+ pose_obb.setTranslation(Vec3s(14.99, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1978,9 +1978,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(22.4, 0, 0));
- pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
- pose_obb.setTranslation(Vec3f(22.4, 0, 0));
+ pose.setTranslation(Vec3s(22.4, 0, 0));
+ pose_aabb.setTranslation(Vec3s(22.4, 0, 0));
+ pose_obb.setTranslation(Vec3s(22.4, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -2010,9 +2010,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(22.51, 0, 0));
- pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
- pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+ pose.setTranslation(Vec3s(22.51, 0, 0));
+ pose_aabb.setTranslation(Vec3s(22.51, 0, 0));
+ pose_obb.setTranslation(Vec3s(22.51, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -2065,9 +2065,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) {
Transform3f pose, pose_aabb, pose_obb;
- pose.setTranslation(Vec3f(9.99, 0, 0));
- pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
- pose_obb.setTranslation(Vec3f(9.99, 0, 0));
+ pose.setTranslation(Vec3s(9.99, 0, 0));
+ pose_aabb.setTranslation(Vec3s(9.99, 0, 0));
+ pose_obb.setTranslation(Vec3s(9.99, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -2097,9 +2097,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(10.01, 0, 0));
- pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
- pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+ pose.setTranslation(Vec3s(10.01, 0, 0));
+ pose_aabb.setTranslation(Vec3s(10.01, 0, 0));
+ pose_obb.setTranslation(Vec3s(10.01, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -2152,9 +2152,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) {
Transform3f pose, pose_aabb, pose_obb;
- pose.setTranslation(Vec3f(9.9, 0, 0));
- pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
- pose_obb.setTranslation(Vec3f(9.9, 0, 0));
+ pose.setTranslation(Vec3s(9.9, 0, 0));
+ pose_aabb.setTranslation(Vec3s(9.9, 0, 0));
+ pose_obb.setTranslation(Vec3s(9.9, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -2184,9 +2184,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(10.1, 0, 0));
- pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
- pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+ pose.setTranslation(Vec3s(10.1, 0, 0));
+ pose_aabb.setTranslation(Vec3s(10.1, 0, 0));
+ pose_obb.setTranslation(Vec3s(10.1, 0, 0));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -2216,9 +2216,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
- pose.setTranslation(Vec3f(0, 0, 9.9));
- pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
- pose_obb.setTranslation(Vec3f(0, 0, 9.9));
+ pose.setTranslation(Vec3s(0, 0, 9.9));
+ pose_aabb.setTranslation(Vec3s(0, 0, 9.9));
+ pose_obb.setTranslation(Vec3s(0, 0, 9.9));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -2248,9 +2248,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) {
res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
- pose.setTranslation(Vec3f(0, 0, 10.1));
- pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
- pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+ pose.setTranslation(Vec3s(0, 0, 10.1));
+ pose_aabb.setTranslation(Vec3s(0, 0, 10.1));
+ pose_obb.setTranslation(Vec3s(0, 0, 10.1));
result.clear();
res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
diff --git a/test/simple.cpp b/test/simple.cpp
index cbeceab0..fd1f7a39 100644
--- a/test/simple.cpp
+++ b/test/simple.cpp
@@ -16,24 +16,24 @@ static bool approx(CoalScalar x, CoalScalar y) {
}
BOOST_AUTO_TEST_CASE(projection_test_line) {
- Vec3f v1(0, 0, 0);
- Vec3f v2(2, 0, 0);
+ Vec3s v1(0, 0, 0);
+ Vec3s v2(2, 0, 0);
- Vec3f p(1, 0, 0);
+ Vec3s p(1, 0, 0);
Project::ProjectResult res = Project::projectLine(v1, v2, p);
BOOST_CHECK(res.encode == 3);
BOOST_CHECK(approx(res.sqr_distance, 0));
BOOST_CHECK(approx(res.parameterization[0], 0.5));
BOOST_CHECK(approx(res.parameterization[1], 0.5));
- p = Vec3f(-1, 0, 0);
+ p = Vec3s(-1, 0, 0);
res = Project::projectLine(v1, v2, p);
BOOST_CHECK(res.encode == 1);
BOOST_CHECK(approx(res.sqr_distance, 1));
BOOST_CHECK(approx(res.parameterization[0], 1));
BOOST_CHECK(approx(res.parameterization[1], 0));
- p = Vec3f(3, 0, 0);
+ p = Vec3s(3, 0, 0);
res = Project::projectLine(v1, v2, p);
BOOST_CHECK(res.encode == 2);
BOOST_CHECK(approx(res.sqr_distance, 1));
@@ -42,11 +42,11 @@ BOOST_AUTO_TEST_CASE(projection_test_line) {
}
BOOST_AUTO_TEST_CASE(projection_test_triangle) {
- Vec3f v1(0, 0, 1);
- Vec3f v2(0, 1, 0);
- Vec3f v3(1, 0, 0);
+ Vec3s v1(0, 0, 1);
+ Vec3s v2(0, 1, 0);
+ Vec3s v3(1, 0, 0);
- Vec3f p(1, 1, 1);
+ Vec3s p(1, 1, 1);
Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 7);
BOOST_CHECK(approx(res.sqr_distance, 4 / 3.0));
@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) {
BOOST_CHECK(approx(res.parameterization[1], 1 / 3.0));
BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
- p = Vec3f(0, 0, 1.5);
+ p = Vec3s(0, 0, 1.5);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 1);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -62,7 +62,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) {
BOOST_CHECK(approx(res.parameterization[1], 0));
BOOST_CHECK(approx(res.parameterization[2], 0));
- p = Vec3f(1.5, 0, 0);
+ p = Vec3s(1.5, 0, 0);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 4);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -70,7 +70,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) {
BOOST_CHECK(approx(res.parameterization[1], 0));
BOOST_CHECK(approx(res.parameterization[2], 1));
- p = Vec3f(0, 1.5, 0);
+ p = Vec3s(0, 1.5, 0);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 2);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) {
BOOST_CHECK(approx(res.parameterization[1], 1));
BOOST_CHECK(approx(res.parameterization[2], 0));
- p = Vec3f(1, 1, 0);
+ p = Vec3s(1, 1, 0);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 6);
BOOST_CHECK(approx(res.sqr_distance, 0.5));
@@ -86,7 +86,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) {
BOOST_CHECK(approx(res.parameterization[1], 0.5));
BOOST_CHECK(approx(res.parameterization[2], 0.5));
- p = Vec3f(1, 0, 1);
+ p = Vec3s(1, 0, 1);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 5);
BOOST_CHECK(approx(res.sqr_distance, 0.5));
@@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) {
BOOST_CHECK(approx(res.parameterization[1], 0));
BOOST_CHECK(approx(res.parameterization[2], 0.5));
- p = Vec3f(0, 1, 1);
+ p = Vec3s(0, 1, 1);
res = Project::projectTriangle(v1, v2, v3, p);
BOOST_CHECK(res.encode == 3);
BOOST_CHECK(approx(res.sqr_distance, 0.5));
@@ -104,12 +104,12 @@ BOOST_AUTO_TEST_CASE(projection_test_triangle) {
}
BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
- Vec3f v1(0, 0, 1);
- Vec3f v2(0, 1, 0);
- Vec3f v3(1, 0, 0);
- Vec3f v4(1, 1, 1);
+ Vec3s v1(0, 0, 1);
+ Vec3s v2(0, 1, 0);
+ Vec3s v3(1, 0, 0);
+ Vec3s v4(1, 1, 1);
- Vec3f p(0.5, 0.5, 0.5);
+ Vec3s p(0.5, 0.5, 0.5);
Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 15);
BOOST_CHECK(approx(res.sqr_distance, 0));
@@ -118,7 +118,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0.25));
BOOST_CHECK(approx(res.parameterization[3], 0.25));
- p = Vec3f(0, 0, 0);
+ p = Vec3s(0, 0, 0);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 7);
BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
@@ -127,7 +127,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
BOOST_CHECK(approx(res.parameterization[3], 0));
- p = Vec3f(0, 1, 1);
+ p = Vec3s(0, 1, 1);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 11);
BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
@@ -136,7 +136,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0));
BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
- p = Vec3f(1, 1, 0);
+ p = Vec3s(1, 1, 0);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 14);
BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
@@ -145,7 +145,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
- p = Vec3f(1, 0, 1);
+ p = Vec3s(1, 0, 1);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 13);
BOOST_CHECK(approx(res.sqr_distance, 1 / 3.0));
@@ -154,7 +154,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 1 / 3.0));
BOOST_CHECK(approx(res.parameterization[3], 1 / 3.0));
- p = Vec3f(1.5, 1.5, 1.5);
+ p = Vec3s(1.5, 1.5, 1.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 8);
BOOST_CHECK(approx(res.sqr_distance, 0.75));
@@ -163,7 +163,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0));
BOOST_CHECK(approx(res.parameterization[3], 1));
- p = Vec3f(1.5, -0.5, -0.5);
+ p = Vec3s(1.5, -0.5, -0.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 4);
BOOST_CHECK(approx(res.sqr_distance, 0.75));
@@ -172,7 +172,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 1));
BOOST_CHECK(approx(res.parameterization[3], 0));
- p = Vec3f(-0.5, -0.5, 1.5);
+ p = Vec3s(-0.5, -0.5, 1.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 1);
BOOST_CHECK(approx(res.sqr_distance, 0.75));
@@ -181,7 +181,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0));
BOOST_CHECK(approx(res.parameterization[3], 0));
- p = Vec3f(-0.5, 1.5, -0.5);
+ p = Vec3s(-0.5, 1.5, -0.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 2);
BOOST_CHECK(approx(res.sqr_distance, 0.75));
@@ -190,7 +190,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0));
BOOST_CHECK(approx(res.parameterization[3], 0));
- p = Vec3f(0.5, -0.5, 0.5);
+ p = Vec3s(0.5, -0.5, 0.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 5);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0.5));
BOOST_CHECK(approx(res.parameterization[3], 0));
- p = Vec3f(0.5, 1.5, 0.5);
+ p = Vec3s(0.5, 1.5, 0.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 10);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -208,7 +208,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0));
BOOST_CHECK(approx(res.parameterization[3], 0.5));
- p = Vec3f(1.5, 0.5, 0.5);
+ p = Vec3s(1.5, 0.5, 0.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 12);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0.5));
BOOST_CHECK(approx(res.parameterization[3], 0.5));
- p = Vec3f(-0.5, 0.5, 0.5);
+ p = Vec3s(-0.5, 0.5, 0.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 3);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -226,7 +226,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0));
BOOST_CHECK(approx(res.parameterization[3], 0));
- p = Vec3f(0.5, 0.5, 1.5);
+ p = Vec3s(0.5, 0.5, 1.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 9);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
@@ -235,7 +235,7 @@ BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) {
BOOST_CHECK(approx(res.parameterization[2], 0));
BOOST_CHECK(approx(res.parameterization[3], 0.5));
- p = Vec3f(0.5, 0.5, -0.5);
+ p = Vec3s(0.5, 0.5, -0.5);
res = Project::projectTetrahedra(v1, v2, v3, v4, p);
BOOST_CHECK(res.encode == 6);
BOOST_CHECK(approx(res.sqr_distance, 0.25));
diff --git a/test/swept_sphere_radius.cpp b/test/swept_sphere_radius.cpp
index a89c8937..b1a687a9 100644
--- a/test/swept_sphere_radius.cpp
+++ b/test/swept_sphere_radius.cpp
@@ -115,8 +115,8 @@ struct SweptSphereGJKSolver : public GJKSolver {
template <typename S1, typename S2>
CoalScalar shapeDistance(
const S1& s1, const Transform3f& tf1, const S2& s2,
- const Transform3f& tf2, bool compute_penetration, Vec3f& p1, Vec3f& p2,
- Vec3f& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const {
+ const Transform3f& tf2, bool compute_penetration, Vec3s& p1, Vec3s& p2,
+ Vec3s& normal, bool use_swept_sphere_radius_in_gjk_epa_iterations) const {
if (use_swept_sphere_radius_in_gjk_epa_iterations) {
CoalScalar distance;
this->runGJKAndEPA<S1, S2, details::SupportOptions::WithSweptSphere>(
@@ -163,9 +163,9 @@ void test_gjksolver_swept_sphere_radius(S1& shape1, S2& shape2) {
SET_LINE;
std::array<CoalScalar, 2> distance;
- std::array<Vec3f, 2> p1;
- std::array<Vec3f, 2> p2;
- std::array<Vec3f, 2> normal;
+ std::array<Vec3s, 2> p1;
+ std::array<Vec3s, 2> p2;
+ std::array<Vec3s, 2> normal;
// Default hppfcl behavior - Don't take swept sphere radius into account
// during GJK/EPA iterations. Correct the solution afterwards.
diff --git a/test/utility.cpp b/test/utility.cpp
index bddb9b4d..80efceba 100644
--- a/test/utility.cpp
+++ b/test/utility.cpp
@@ -85,16 +85,16 @@ const Eigen::IOFormat vfmt = Eigen::IOFormat(
const Eigen::IOFormat pyfmt = Eigen::IOFormat(
Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
-const Vec3f UnitX = Vec3f(1, 0, 0);
-const Vec3f UnitY = Vec3f(0, 1, 0);
-const Vec3f UnitZ = Vec3f(0, 0, 1);
+const Vec3s UnitX = Vec3s(1, 0, 0);
+const Vec3s UnitY = Vec3s(0, 1, 0);
+const Vec3s UnitZ = Vec3s(0, 0, 1);
CoalScalar rand_interval(CoalScalar rmin, CoalScalar rmax) {
CoalScalar t = rand() / ((CoalScalar)RAND_MAX + 1);
return (t * (rmax - rmin) + rmin);
}
-void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
+void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
std::vector<Triangle>& triangles) {
FILE* file = fopen(filename, "rb");
if (!file) {
@@ -124,7 +124,7 @@ void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
CoalScalar x = (CoalScalar)atof(strtok(NULL, "\t "));
CoalScalar y = (CoalScalar)atof(strtok(NULL, "\t "));
CoalScalar z = (CoalScalar)atof(strtok(NULL, "\t "));
- Vec3f p(x, y, z);
+ Vec3s p(x, y, z);
points.push_back(p);
}
} break;
@@ -160,7 +160,7 @@ void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
}
}
-void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
+void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
std::vector<Triangle>& triangles) {
std::ofstream os(filename);
if (!os) {
@@ -195,7 +195,7 @@ OcTree loadOctreeFile(const std::string& filename,
}
#endif
-void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3f& R) {
+void eulerToMatrix(CoalScalar a, CoalScalar b, CoalScalar c, Matrix3s& R) {
CoalScalar c1 = cos(a);
CoalScalar c2 = cos(b);
CoalScalar c3 = cos(c);
@@ -217,9 +217,9 @@ void generateRandomTransform(CoalScalar extents[6], Transform3f& transform) {
CoalScalar b = rand_interval(0, 2 * pi);
CoalScalar c = rand_interval(0, 2 * pi);
- Matrix3f R;
+ Matrix3s R;
eulerToMatrix(a, b, c, R);
- Vec3f T(x, y, z);
+ Vec3s T(x, y, z);
transform.setTransform(R, T);
}
@@ -238,9 +238,9 @@ void generateRandomTransforms(CoalScalar extents[6],
CoalScalar c = rand_interval(0, 2 * pi);
{
- Matrix3f R;
+ Matrix3s R;
eulerToMatrix(a, b, c, R);
- Vec3f T(x, y, z);
+ Vec3s T(x, y, z);
transforms[i].setTransform(R, T);
}
}
@@ -264,9 +264,9 @@ void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3],
CoalScalar c = rand_interval(0, 2 * pi);
{
- Matrix3f R;
+ Matrix3s R;
eulerToMatrix(a, b, c, R);
- Vec3f T(x, y, z);
+ Vec3s T(x, y, z);
transforms[i].setTransform(R, T);
}
@@ -279,9 +279,9 @@ void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3],
CoalScalar deltac = rand_interval(-delta_rot, delta_rot);
{
- Matrix3f R;
+ Matrix3s R;
eulerToMatrix(a + deltaa, b + deltab, c + deltac, R);
- Vec3f T(x + deltax, y + deltay, z + deltaz);
+ Vec3s T(x + deltax, y + deltay, z + deltaz);
transforms2[i].setTransform(R, T);
}
}
@@ -458,10 +458,10 @@ void generateEnvironmentsMesh(std::vector<CollisionObject*>& env,
}
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),
- Vec3f(-l, -w, -d)}));
+ std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>(
+ {Vec3s(l, w, d), Vec3s(l, w, -d), Vec3s(l, -w, d), Vec3s(l, -w, -d),
+ Vec3s(-l, w, d), Vec3s(-l, w, -d), Vec3s(-l, -w, d),
+ Vec3s(-l, -w, -d)}));
std::shared_ptr<std::vector<Quadrilateral>> polygons(
new std::vector<Quadrilateral>(6));
@@ -480,7 +480,7 @@ Convex<Quadrilateral> buildBox(CoalScalar l, CoalScalar w, CoalScalar d) {
}
/// Takes a point and projects it onto the surface of the unit sphere
-void toSphere(Vec3f& point) {
+void toSphere(Vec3s& point) {
assert(point.norm() > 1e-8);
point /= point.norm();
}
@@ -491,7 +491,7 @@ void toSphere(Vec3f& point) {
/// ellipsoid. Thus, the point y = A^(1/2) * x belongs to the unit sphere if y *
/// y = 1. Therefore, the tranformation which brings y to x is A^(-1/2) =
/// diag(r).
-void toEllipsoid(Vec3f& point, const Ellipsoid& ellipsoid) {
+void toEllipsoid(Vec3s& point, const Ellipsoid& ellipsoid) {
toSphere(point);
point[0] *= ellipsoid.radii[0];
point[1] *= ellipsoid.radii[1];
@@ -502,24 +502,24 @@ Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
CoalScalar PHI = (1 + std::sqrt(5)) / 2;
// vertices
- std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
- Vec3f(-1, PHI, 0),
- Vec3f(1, PHI, 0),
- Vec3f(-1, -PHI, 0),
- Vec3f(1, -PHI, 0),
-
- Vec3f(0, -1, PHI),
- Vec3f(0, 1, PHI),
- Vec3f(0, -1, -PHI),
- Vec3f(0, 1, -PHI),
-
- Vec3f(PHI, 0, -1),
- Vec3f(PHI, 0, 1),
- Vec3f(-PHI, 0, -1),
- Vec3f(-PHI, 0, 1),
+ std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
+ Vec3s(-1, PHI, 0),
+ Vec3s(1, PHI, 0),
+ Vec3s(-1, -PHI, 0),
+ Vec3s(1, -PHI, 0),
+
+ Vec3s(0, -1, PHI),
+ Vec3s(0, 1, PHI),
+ Vec3s(0, -1, -PHI),
+ Vec3s(0, 1, -PHI),
+
+ Vec3s(PHI, 0, -1),
+ Vec3s(PHI, 0, 1),
+ Vec3s(-PHI, 0, -1),
+ Vec3s(-PHI, 0, 1),
}));
- std::vector<Vec3f>& pts_ = *pts;
+ std::vector<Vec3s>& pts_ = *pts;
for (size_t i = 0; i < 12; ++i) {
toEllipsoid(pts_[i], ellipsoid);
}
@@ -557,7 +557,7 @@ Convex<Triangle> constructPolytopeFromEllipsoid(const Ellipsoid& ellipsoid) {
}
Box makeRandomBox(CoalScalar min_size, CoalScalar max_size) {
- return Box(Vec3f(rand_interval(min_size, max_size),
+ return Box(Vec3s(rand_interval(min_size, max_size),
rand_interval(min_size, max_size),
rand_interval(min_size, max_size)));
}
@@ -567,7 +567,7 @@ Sphere makeRandomSphere(CoalScalar min_size, CoalScalar max_size) {
}
Ellipsoid makeRandomEllipsoid(CoalScalar min_size, CoalScalar max_size) {
- return Ellipsoid(Vec3f(rand_interval(min_size, max_size),
+ return Ellipsoid(Vec3s(rand_interval(min_size, max_size),
rand_interval(min_size, max_size),
rand_interval(min_size, max_size)));
}
@@ -596,11 +596,11 @@ Convex<Triangle> makeRandomConvex(CoalScalar min_size, CoalScalar max_size) {
}
Plane makeRandomPlane(CoalScalar min_size, CoalScalar max_size) {
- return Plane(Vec3f::Random().normalized(), rand_interval(min_size, max_size));
+ return Plane(Vec3s::Random().normalized(), rand_interval(min_size, max_size));
}
Halfspace makeRandomHalfspace(CoalScalar min_size, CoalScalar max_size) {
- return Halfspace(Vec3f::Random().normalized(),
+ return Halfspace(Vec3s::Random().normalized(),
rand_interval(min_size, max_size));
}
diff --git a/test/utility.h b/test/utility.h
index 70c86809..2969bb28 100644
--- a/test/utility.h
+++ b/test/utility.h
@@ -126,15 +126,15 @@ struct TStruct {
extern const Eigen::IOFormat vfmt;
extern const Eigen::IOFormat pyfmt;
typedef Eigen::AngleAxis<CoalScalar> AngleAxis;
-extern const Vec3f UnitX;
-extern const Vec3f UnitY;
-extern const Vec3f UnitZ;
+extern const Vec3s UnitX;
+extern const Vec3s UnitY;
+extern const Vec3s UnitZ;
/// @brief Load an obj mesh file
-void loadOBJFile(const char* filename, std::vector<Vec3f>& points,
+void loadOBJFile(const char* filename, std::vector<Vec3s>& points,
std::vector<Triangle>& triangles);
-void saveOBJFile(const char* filename, std::vector<Vec3f>& points,
+void saveOBJFile(const char* filename, std::vector<Vec3s>& points,
std::vector<Triangle>& triangles);
#ifdef COAL_HAS_OCTOMAP
@@ -167,8 +167,8 @@ void generateRandomTransforms(CoalScalar extents[6], CoalScalar delta_trans[3],
/// corresponding nearest point pair
struct DistanceRes {
double distance;
- Vec3f p1;
- Vec3f p2;
+ Vec3s p1;
+ Vec3s p2;
};
/// @brief Default collision callback for two objects o1 and o2 in broad phase.
--
GitLab