From 928b1756335451c61edf05903d65d99d9d74f632 Mon Sep 17 00:00:00 2001
From: panjia1983 <panjia1983@gmail.com>
Date: Fri, 9 Aug 2013 17:23:25 -0700
Subject: [PATCH] fix some bug in gjk; add continuous collision, still need
more test
---
include/fcl/BVH/BVH_model.h | 1 +
include/fcl/broadphase/hierarchy_tree.hxx | 2 +-
include/fcl/ccd/conservative_advancement.h | 23 +-
include/fcl/ccd/motion.h | 124 ++-
include/fcl/ccd/motion_base.h | 45 +-
include/fcl/collision.h | 22 -
include/fcl/collision_data.h | 138 ++-
include/fcl/collision_object.h | 1 +
include/fcl/continuous_collision.h | 28 +
include/fcl/distance.h | 10 -
include/fcl/intersect.h | 62 +-
include/fcl/math/transform.h | 1 +
include/fcl/narrowphase/gjk.h | 57 +-
include/fcl/narrowphase/gjk_libccd.h | 2 +-
include/fcl/narrowphase/narrowphase.h | 257 +++--
include/fcl/penetration_depth.h | 24 +
include/fcl/shape/geometric_shapes.h | 4 +-
include/fcl/shape/geometric_shapes_utility.h | 4 +-
include/fcl/traversal/traversal_node_base.h | 14 +
.../fcl/traversal/traversal_node_bvh_shape.h | 581 ++++++++++-
include/fcl/traversal/traversal_node_bvhs.h | 45 +-
include/fcl/traversal/traversal_node_octree.h | 13 +-
include/fcl/traversal/traversal_node_setup.h | 186 +++-
include/fcl/traversal/traversal_node_shapes.h | 60 +-
src/broadphase/broadphase_SSaP.cpp | 2 +-
src/ccd/conservative_advancement.cpp | 875 +++++++++++++++--
src/ccd/motion.cpp | 54 +-
src/collision.cpp | 81 +-
src/collision_func_matrix.cpp | 13 +
src/continuous_collision.cpp | 307 ++++++
src/distance.cpp | 40 +-
src/intersect.cpp | 274 ++++++
src/narrowphase/gjk.cpp | 173 +---
src/narrowphase/gjk_libccd.cpp | 360 ++++---
src/narrowphase/narrowphase.cpp | 280 +++---
src/shape/geometric_shapes.cpp | 2 +-
src/shape/geometric_shapes_utility.cpp | 4 +-
src/traversal/traversal_node_bvhs.cpp | 22 +-
test/CMakeLists.txt | 1 +
test/test_fcl_geometric_shapes.cpp | 185 ++--
test/test_fcl_shape_mesh_consistency.cpp | 914 +++++++++---------
test/test_fcl_utility.cpp | 7 +-
test/test_fcl_utility.h | 18 +
43 files changed, 3993 insertions(+), 1323 deletions(-)
create mode 100644 include/fcl/continuous_collision.h
create mode 100644 include/fcl/penetration_depth.h
create mode 100644 src/continuous_collision.cpp
diff --git a/include/fcl/BVH/BVH_model.h b/include/fcl/BVH/BVH_model.h
index 3560a57f..4c6a0699 100644
--- a/include/fcl/BVH/BVH_model.h
+++ b/include/fcl/BVH/BVH_model.h
@@ -216,6 +216,7 @@ public:
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
+
private:
int num_tris_allocated;
diff --git a/include/fcl/broadphase/hierarchy_tree.hxx b/include/fcl/broadphase/hierarchy_tree.hxx
index 07b62912..03bb175e 100644
--- a/include/fcl/broadphase/hierarchy_tree.hxx
+++ b/include/fcl/broadphase/hierarchy_tree.hxx
@@ -1390,7 +1390,7 @@ void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend)
size_t* lcur_end = lend;
while(lbeg < lcur_end - 1)
{
- size_t* min_it1, *min_it2;
+ size_t* min_it1 = NULL, *min_it2 = NULL;
FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
for(size_t* it1 = lbeg; it1 < lcur_end; ++it1)
{
diff --git a/include/fcl/ccd/conservative_advancement.h b/include/fcl/ccd/conservative_advancement.h
index 4129a967..d6d7aeb8 100644
--- a/include/fcl/ccd/conservative_advancement.h
+++ b/include/fcl/ccd/conservative_advancement.h
@@ -46,15 +46,22 @@
namespace fcl
{
-template<typename BV, typename ConservativeAdvancementNode, typename CollisionNode>
-int conservativeAdvancement(const CollisionGeometry* o1,
- const MotionBase* motion1,
- const CollisionGeometry* o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc);
+template<typename NarrowPhaseSolver>
+struct ConservativeAdvancementFunctionMatrix
+{
+ typedef FCL_REAL (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result);
+
+ ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT];
+
+ ConservativeAdvancementFunctionMatrix();
+};
+
+
}
#endif
+
+
+
+
diff --git a/include/fcl/ccd/motion.h b/include/fcl/ccd/motion.h
index 3494cabf..a281a182 100644
--- a/include/fcl/ccd/motion.h
+++ b/include/fcl/ccd/motion.h
@@ -46,6 +46,64 @@
namespace fcl
{
+class TranslationMotion : public MotionBase
+{
+public:
+ /// @brief Construct motion from intial and goal transform
+ TranslationMotion(const Transform3f& tf1,
+ const Transform3f& tf2) : MotionBase(),
+ rot(tf1.getQuatRotation()),
+ trans_start(tf1.getTranslation()),
+ trans_range(tf2.getTranslation() - tf1.getTranslation())
+ {
+ }
+
+ TranslationMotion(const Matrix3f& R, const Vec3f& T1, const Vec3f& T2) : MotionBase()
+ {
+ rot.fromRotation(R);
+ trans_start = T1;
+ trans_range = T2 - T1;
+ }
+
+ bool integrate(FCL_REAL dt) const
+ {
+ if(dt > 1) dt = 1;
+ tf = Transform3f(rot, trans_start + trans_range * dt);
+ return true;
+ }
+
+ FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const
+ {
+ return mb_visitor.visit(*this);
+ }
+
+ FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const
+ {
+ return mb_visitor.visit(*this);
+ }
+
+ void getCurrentTransform(Transform3f& tf_) const
+ {
+ tf_ = tf;
+ }
+
+ void getTaylorModel(TMatrix3& tm, TVector3& tv) const
+ {
+ }
+
+ Vec3f getVelocity() const
+ {
+ return trans_range;
+ }
+
+ private:
+ /// @brief initial and goal transforms
+ Quaternion3f rot;
+ Vec3f trans_start, trans_range;
+
+ mutable Transform3f tf;
+};
+
class SplineMotion : public MotionBase
{
public:
@@ -53,6 +111,19 @@ public:
SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
+ // @brief Construct motion from initial and goal transform
+ SplineMotion(const Matrix3f& R1, const Vec3f& T1,
+ const Matrix3f& R2, const Vec3f& T2) : MotionBase()
+ {
+ // TODO
+ }
+
+ SplineMotion(const Transform3f& tf1,
+ const Transform3f& tf2) : MotionBase()
+ {
+ // TODO
+ }
+
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
@@ -71,22 +142,6 @@ public:
}
/// @brief Get the rotation and translation in current step
- void getCurrentTransform(Matrix3f& R, Vec3f& T) const
- {
- R = tf.getRotation();
- T = tf.getTranslation();
- }
-
- void getCurrentRotation(Matrix3f& R) const
- {
- R = tf.getRotation();
- }
-
- void getCurrentTranslation(Vec3f& T) const
- {
- T = tf.getTranslation();
- }
-
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
@@ -209,7 +264,7 @@ class ScrewMotion : public MotionBase
{
public:
/// @brief Default transformations are all identities
- ScrewMotion()
+ ScrewMotion() : MotionBase()
{
// Default angular velocity is zero
axis.setValue(1, 0, 0);
@@ -223,7 +278,8 @@ public:
/// @brief Construct motion from the initial rotation/translation and goal rotation/translation
ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
+ const Matrix3f& R2, const Vec3f& T2) : MotionBase(),
+ tf1(R1, T1),
tf2(R2, T2),
tf(tf1)
{
@@ -267,22 +323,6 @@ public:
/// @brief Get the rotation and translation in current step
- void getCurrentTransform(Matrix3f& R, Vec3f& T) const
- {
- R = tf.getRotation();
- T = tf.getTranslation();
- }
-
- void getCurrentRotation(Matrix3f& R) const
- {
- R = tf.getRotation();
- }
-
- void getCurrentTranslation(Vec3f& T) const
- {
- T = tf.getTranslation();
- }
-
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
@@ -438,22 +478,6 @@ public:
}
/// @brief Get the rotation and translation in current step
- void getCurrentTransform(Matrix3f& R, Vec3f& T) const
- {
- R = tf.getRotation();
- T = tf.getTranslation();
- }
-
- void getCurrentRotation(Matrix3f& R) const
- {
- R = tf.getRotation();
- }
-
- void getCurrentTranslation(Vec3f& T) const
- {
- T = tf.getTranslation();
- }
-
void getCurrentTransform(Transform3f& tf_) const
{
tf_ = tf;
diff --git a/include/fcl/ccd/motion_base.h b/include/fcl/ccd/motion_base.h
index edce660f..aba89e3d 100644
--- a/include/fcl/ccd/motion_base.h
+++ b/include/fcl/ccd/motion_base.h
@@ -52,6 +52,7 @@ class MotionBase;
class SplineMotion;
class ScrewMotion;
class InterpMotion;
+class TranslationMotion;
/// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
class BVMotionBoundVisitor
@@ -61,6 +62,7 @@ public:
virtual FCL_REAL visit(const SplineMotion& motion) const = 0;
virtual FCL_REAL visit(const ScrewMotion& motion) const = 0;
virtual FCL_REAL visit(const InterpMotion& motion) const = 0;
+ virtual FCL_REAL visit(const TranslationMotion& motion) const = 0;
};
template<typename BV>
@@ -73,6 +75,7 @@ public:
virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; }
virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; }
virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; }
+ virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; }
protected:
BV bv;
@@ -88,6 +91,8 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const;
template<>
FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const;
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const TranslationMotion& motion) const;
class TriangleMotionBoundVisitor
@@ -100,6 +105,7 @@ public:
virtual FCL_REAL visit(const SplineMotion& motion) const;
virtual FCL_REAL visit(const ScrewMotion& motion) const;
virtual FCL_REAL visit(const InterpMotion& motion) const;
+ virtual FCL_REAL visit(const TranslationMotion& motion) const;
protected:
Vec3f a, b, c, n;
@@ -120,17 +126,48 @@ public:
virtual bool integrate(double dt) const = 0;
/** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
- virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const= 0;
+ virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0;
/** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */
virtual FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0;
/** \brief Get the rotation and translation in current step */
- virtual void getCurrentTransform(Matrix3f& R, Vec3f& T) const = 0;
+ void getCurrentTransform(Matrix3f& R, Vec3f& T) const
+ {
+ Transform3f tf;
+ getCurrentTransform(tf);
+ R = tf.getRotation();
+ T = tf.getTranslation();
+ }
- virtual void getCurrentRotation(Matrix3f& R) const = 0;
+ void getCurrentTransform(Quaternion3f& Q, Vec3f& T) const
+ {
+ Transform3f tf;
+ getCurrentTransform(tf);
+ Q = tf.getQuatRotation();
+ T = tf.getTranslation();
+ }
- virtual void getCurrentTranslation(Vec3f& T) const = 0;
+ void getCurrentRotation(Matrix3f& R) const
+ {
+ Transform3f tf;
+ getCurrentTransform(tf);
+ R = tf.getRotation();
+ }
+
+ void getCurrentRotation(Quaternion3f& Q) const
+ {
+ Transform3f tf;
+ getCurrentTransform(tf);
+ Q = tf.getQuatRotation();
+ }
+
+ void getCurrentTranslation(Vec3f& T) const
+ {
+ Transform3f tf;
+ getCurrentTransform(tf);
+ T = tf.getTranslation();
+ }
virtual void getCurrentTransform(Transform3f& tf) const = 0;
diff --git a/include/fcl/collision.h b/include/fcl/collision.h
index 3c70aaac..2288b3a1 100644
--- a/include/fcl/collision.h
+++ b/include/fcl/collision.h
@@ -50,34 +50,12 @@ namespace fcl
/// performs the collision between them.
/// Return value is the number of contacts generated between the two objects.
-template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
- const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result);
-template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result);
-
-std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
- const CollisionRequest& request,
- CollisionResult& result);
-
-std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
- const CollisionGeometry* o2, const Transform3f& tf2,
- const CollisionRequest& request,
- CollisionResult& result);
-
-std::size_t collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
- const CollisionRequest& request,
- CollisionResult& result);
-
-std::size_t collide(const CollisionGeometry* o1, const MotionBase* motion1,
- const CollisionGeometry* o2, const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result);
}
diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h
index 7b95e4a9..9621d33d 100644
--- a/include/fcl/collision_data.h
+++ b/include/fcl/collision_data.h
@@ -48,6 +48,9 @@
namespace fcl
{
+/// @brief Type of narrow phase GJK solver
+enum GJKSolverType {GST_LIBCCD, GST_INDEP};
+
/// @brief Contact information returned by collision
struct Contact
{
@@ -167,7 +170,7 @@ struct CollisionResult;
/// @brief request to the collision algorithm
struct CollisionRequest
-{
+{
/// @brief The maximum number of contacts will return
size_t num_max_contacts;
@@ -183,16 +186,29 @@ struct CollisionRequest
/// @brief whether the cost computation is approximated
bool use_approximate_cost;
+ /// @brief narrow phase solver
+ GJKSolverType gjk_solver_type;
+
+ /// @brief whether enable gjk intial guess
+ bool enable_cached_gjk_guess;
+
+ /// @brief the gjk intial guess set by user
+ Vec3f cached_gjk_guess;
+
CollisionRequest(size_t num_max_contacts_ = 1,
bool enable_contact_ = false,
size_t num_max_cost_sources_ = 1,
bool enable_cost_ = false,
- bool use_approximate_cost_ = true) : num_max_contacts(num_max_contacts_),
- enable_contact(enable_contact_),
- num_max_cost_sources(num_max_cost_sources_),
- enable_cost(enable_cost_),
- use_approximate_cost(use_approximate_cost_)
+ bool use_approximate_cost_ = true,
+ GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_),
+ enable_contact(enable_contact_),
+ num_max_cost_sources(num_max_cost_sources_),
+ enable_cost(enable_cost_),
+ use_approximate_cost(use_approximate_cost_),
+ gjk_solver_type(gjk_solver_type_)
{
+ enable_cached_gjk_guess = false;
+ cached_gjk_guess = Vec3f(1, 0, 0);
}
bool isSatisfied(const CollisionResult& result) const;
@@ -208,6 +224,9 @@ private:
/// @brief cost sources
std::set<CostSource> cost_sources;
+public:
+ Vec3f cached_gjk_guess;
+
public:
CollisionResult()
{
@@ -277,6 +296,7 @@ public:
}
};
+
struct DistanceResult;
/// @brief request to the distance computation
@@ -285,13 +305,29 @@ struct DistanceRequest
/// @brief whether to return the nearest points
bool enable_nearest_points;
- DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
+ /// @brief error threshold for approximate distance
+ FCL_REAL rel_err; // relative error, between 0 and 1
+ FCL_REAL abs_err; // absoluate error
+
+ /// @brief narrow phase solver type
+ GJKSolverType gjk_solver_type;
+
+
+
+ DistanceRequest(bool enable_nearest_points_ = false,
+ FCL_REAL rel_err_ = 0.0,
+ FCL_REAL abs_err_ = 0.0,
+ GJKSolverType gjk_solver_type_ = GST_LIBCCD) : enable_nearest_points(enable_nearest_points_),
+ rel_err(rel_err_),
+ abs_err(abs_err_),
+ gjk_solver_type(gjk_solver_type_)
{
}
bool isSatisfied(const DistanceResult& result) const;
};
+
/// @brief distance result
struct DistanceResult
{
@@ -389,6 +425,94 @@ public:
};
+enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE};
+enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER};
+
+
+struct ContinuousCollisionRequest
+{
+ /// @brief maximum num of iterations
+ std::size_t num_max_iterations;
+
+ /// @brief error in first contact time
+ FCL_REAL toc_err;
+
+ /// @brief ccd motion type
+ CCDMotionType ccd_motion_type;
+
+ /// @brief gjk solver type
+ GJKSolverType gjk_solver_type;
+
+ /// @brief ccd solver type
+ CCDSolverType ccd_solver_type;
+
+ ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10,
+ FCL_REAL toc_err_ = 0,
+ CCDMotionType ccd_motion_type_ = CCDM_TRANS,
+ GJKSolverType gjk_solver_type_ = GST_LIBCCD,
+ CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_),
+ toc_err(toc_err_),
+ ccd_motion_type(ccd_motion_type_),
+ gjk_solver_type(gjk_solver_type_),
+ ccd_solver_type(ccd_solver_type_)
+ {
+ }
+
+};
+/// @brief continuous collision result
+struct ContinuousCollisionResult
+{
+ /// @brief collision or not
+ bool is_collide;
+
+ /// @brief time of contact in [0, 1]
+ FCL_REAL time_of_contact;
+
+ ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0)
+ {
+ }
+};
+
+
+enum PenetrationDepthType {PDT_LOCAL, PDT_AL};
+
+struct PenetrationDepthMetricBase
+{
+ virtual FCL_REAL operator() (const Transform3f& tf1, const Transform3f& tf2) const = 0;
+};
+
+struct WeightEuclideanPDMetric : public PenetrationDepthMetricBase
+{
+
+};
+
+struct PenetrationDepthRequest
+{
+ /// @brief PD algorithm type
+ PenetrationDepthType pd_type;
+
+ /// @brief gjk solver type
+ GJKSolverType gjk_solver_type;
+
+ PenetrationDepthRequest(PenetrationDepthType pd_type_ = PDT_LOCAL,
+ GJKSolverType gjk_solver_type_ = GST_LIBCCD) : pd_type(pd_type_),
+ gjk_solver_type(gjk_solver_type_)
+ {
+ }
+};
+
+struct PenetrationDepthResult
+{
+ /// @brief penetration depth value
+ FCL_REAL pd_value;
+
+ /// @brief the transform where the collision is resolved
+ Transform3f resolve_trans;
+
+
+};
+
+
}
diff --git a/include/fcl/collision_object.h b/include/fcl/collision_object.h
index 2a4492a1..00716051 100644
--- a/include/fcl/collision_object.h
+++ b/include/fcl/collision_object.h
@@ -115,6 +115,7 @@ public:
/// @brief threshold for free (<= is free)
FCL_REAL threshold_free;
+
};
/// @brief the object for collision or distance computation, contains the geometry and the transform information
diff --git a/include/fcl/continuous_collision.h b/include/fcl/continuous_collision.h
new file mode 100644
index 00000000..0a6ab1ab
--- /dev/null
+++ b/include/fcl/continuous_collision.h
@@ -0,0 +1,28 @@
+#ifndef FCL_CONTINUOUS_COLLISION_H
+#define FCL_CONTINUOUS_COLLISION_H
+
+#include "fcl/math/vec_3f.h"
+#include "fcl/collision_object.h"
+#include "fcl/collision_data.h"
+
+namespace fcl
+{
+
+/// @brief continous collision checking between two objects
+FCL_REAL continuous_collide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
+ const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result);
+
+FCL_REAL continuous_collide(const CollisionObject* o1, const Transform3f& tf1_end,
+ const CollisionObject* o2, const Transform3f& tf2_end,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result);
+
+FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result);
+
+}
+
+#endif
diff --git a/include/fcl/distance.h b/include/fcl/distance.h
index 36208ec2..be65f07d 100644
--- a/include/fcl/distance.h
+++ b/include/fcl/distance.h
@@ -46,16 +46,6 @@ namespace fcl
/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them.
/// Return value is the minimum distance generated between the two objects.
-template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
- const DistanceRequest& request, DistanceResult& result);
-
-template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
- const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- const DistanceRequest& request, DistanceResult& result);
-
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
const DistanceRequest& request, DistanceResult& result);
diff --git a/include/fcl/intersect.h b/include/fcl/intersect.h
index d0841de4..047b50a2 100644
--- a/include/fcl/intersect.h
+++ b/include/fcl/intersect.h
@@ -91,13 +91,13 @@ public:
/// @brief CCD intersect between one vertex and one face, using additional filter
static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/// @brief CCD intersect between two edges, using additional filter
static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/// @brief CCD intersect between one vertex and and one edge
static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
@@ -204,7 +204,7 @@ private:
/// @brief filter for intersection, works for both VF and EE
static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
/// @brief distance of point v to a plane n * x - t = 0
static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
@@ -246,6 +246,46 @@ private:
static const unsigned int MAX_TRIANGLE_CLIPS = 8;
};
+/// @brief Project functions
+class Project
+{
+public:
+ struct ProjectResult
+ {
+ /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array)
+ FCL_REAL parameterization[4];
+
+ /// @brief square distance from the query point to the projected simplex
+ FCL_REAL sqr_distance;
+
+ /// @brief the code of the projection type
+ unsigned int encode;
+
+ ProjectResult() : sqr_distance(-1), encode(0)
+ {
+ }
+ };
+
+ /// @brief Project point p onto line a-b
+ static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, const Vec3f& p);
+
+ /// @brief Project point p onto triangle a-b-c
+ static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& p);
+
+ /// @brief Project point p onto tetrahedra a-b-c-d
+ static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, const Vec3f& p);
+
+ /// @brief Project origin (0) onto line a-b
+ static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b);
+
+ /// @brief Project origin (0) onto triangle a-b-c
+ static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c);
+
+ /// @brief Project origin (0) onto tetrahedran a-b-c-d
+ static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d);
+};
+
+/// @brief Triangle distance functions
class TriangleDistance
{
public:
@@ -256,7 +296,7 @@ public:
/// 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);
+ Vec3f& VEC, Vec3f& X, Vec3f& Y);
/// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
/// S and T are two triangles
@@ -279,10 +319,20 @@ public:
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
+ static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
+ const Transform3f& tf,
+ Vec3f& P, Vec3f& Q);
+
static FCL_REAL triDistance(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 FCL_REAL triDistance(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);
+
};
diff --git a/include/fcl/math/transform.h b/include/fcl/math/transform.h
index 5082e74d..e82c1f69 100644
--- a/include/fcl/math/transform.h
+++ b/include/fcl/math/transform.h
@@ -342,6 +342,7 @@ Transform3f inverse(const Transform3f& tf);
void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf);
+
}
#endif
diff --git a/include/fcl/narrowphase/gjk.h b/include/fcl/narrowphase/gjk.h
index 3cebbd84..eba8c926 100644
--- a/include/fcl/narrowphase/gjk.h
+++ b/include/fcl/narrowphase/gjk.h
@@ -68,18 +68,20 @@ struct MinkowskiDiff
{
return getSupport(shapes[0], d);
}
-
+
/// @brief support function for shape1
inline Vec3f support1(const Vec3f& d) const
{
return toshape0.transform(getSupport(shapes[1], toshape1 * d));
}
+ /// @brief support function for the pair of shapes
inline Vec3f support(const Vec3f& d) const
{
return support0(d) - support1(-d);
}
+ /// @brief support function for the d-th shape (d = 0 or 1)
inline Vec3f support(const Vec3f& d, size_t index) const
{
if(index)
@@ -88,23 +90,31 @@ struct MinkowskiDiff
return support0(d);
}
-};
+ /// @brief support function for translating shape0, which is translating at velocity v
+ inline Vec3f support0(const Vec3f& d, const Vec3f& v) const
+ {
+ if(d.dot(v) <= 0)
+ return getSupport(shapes[0], d);
+ else
+ return getSupport(shapes[0], d) + v;
+ }
-/// @brief project origin on to a line (a, b). w[0:1] return the (0, 1) parameterization of the projected point.
-/// m is a encode about the result case: 0x10--> project is larger than b; 0x01--> project is smaller than a;
-/// 0x11-> within the line;
-/// return value is distance between the origin and its projection.
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m);
+ /// @brief support function for the pair of shapes, where shape0 is translating at velocity v
+ inline Vec3f support(const Vec3f& d, const Vec3f& v) const
+ {
+ return support0(d, v) - support1(-d);
+ }
-/// @brief project origin on to a triangle (a, b, c). w[0:2] return the (0, 1) parameterization of the projected point.
-/// m is a encode about the result case.
-/// return value is distance between the origin and its projection.
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m);
+ /// @brief support function for the d-th shape (d = 0 or 1), where shape0 is translating at velocity v
+ inline Vec3f support(const Vec3f& d, const Vec3f& v, size_t index) const
+ {
+ if(index)
+ return support1(d);
+ else
+ return support0(d, v);
+ }
+};
-/// @brief project origin on to a tetrahedra (a, b, c, d). w[0:3] return the (0, 1) parameterization of the projected point.
-/// m is a encode about the result case.
-/// return value is distance between the origin and its projection.
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m);
/// @brief class for GJK algorithm
struct GJK
@@ -113,7 +123,7 @@ struct GJK
{
/// @brief support direction
Vec3f d;
- /// @brieg support vector
+ /// @brieg support vector (i.e., the furthest point on the shape along the support direction)
Vec3f w;
};
@@ -124,7 +134,9 @@ struct GJK
/// @brief weight
FCL_REAL p[4];
/// @brief size of simplex (number of vertices)
- size_t rank;
+ size_t rank;
+
+ Simplex() : rank(0) {}
};
enum Status {Valid, Inside, Failed};
@@ -149,6 +161,9 @@ struct GJK
/// @brief apply the support function along a direction, the result is return in sv
void getSupport(const Vec3f& d, SimplexV& sv) const;
+ /// @brief apply the support function along a direction, the result is return is sv, here shape0 is translating at velocity v
+ void getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const;
+
/// @brief discard one vertex from the simplex
void removeVertex(Simplex& simplex);
@@ -158,12 +173,15 @@ struct GJK
/// @brief whether the simplex enclose the origin
bool encloseOrigin();
- /// @brief get the underlying simplex using in GJK
+ /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration
inline Simplex* getSimplex() const
{
return simplex;
}
-
+
+ /// @brief get the guess from current simplex
+ Vec3f getGuessFromSimplex() const;
+
private:
SimplexV store_v[4];
SimplexV* free_v[4];
@@ -289,7 +307,6 @@ public:
-
}
diff --git a/include/fcl/narrowphase/gjk_libccd.h b/include/fcl/narrowphase/gjk_libccd.h
index a222507f..43c27c85 100644
--- a/include/fcl/narrowphase/gjk_libccd.h
+++ b/include/fcl/narrowphase/gjk_libccd.h
@@ -160,7 +160,7 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
unsigned int max_iterations, FCL_REAL tolerance,
- FCL_REAL* dist);
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2);
} // details
diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 9f3862e9..0ef7245c 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -45,6 +45,8 @@
namespace fcl
{
+
+
/// @brief collision and distance solver based on libccd library.
struct GJKSolver_libccd
{
@@ -112,7 +114,7 @@ struct GJKSolver_libccd
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
@@ -120,7 +122,10 @@ struct GJKSolver_libccd
bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
o2, details::GJKInitializer<S2>::getSupportFunction(),
max_distance_iterations, distance_tolerance,
- dist);
+ dist, p1, p2);
+
+ if(p1) *p1 = inverse(tf1).transform(*p1);
+ if(p2) *p2 = inverse(tf2).transform(*p2);
details::GJKInitializer<S1>::deleteGJKObject(o1);
details::GJKInitializer<S2>::deleteGJKObject(o2);
@@ -128,12 +133,20 @@ struct GJKSolver_libccd
return res;
}
+ template<typename S1, typename S2>
+ bool shapeDistance(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ FCL_REAL* dist) const
+ {
+ return shapeDistance(s1, tf1, s2, tf2, dist, NULL, NULL);
+ }
+
/// @brief distance computation between one shape and a triangle
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3);
@@ -141,7 +154,8 @@ struct GJKSolver_libccd
bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
o2, details::triGetSupportFunction(),
max_distance_iterations, distance_tolerance,
- dist);
+ dist, p1, p2);
+ if(p1) *p1 = inverse(tf).transform(*p1);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
@@ -149,11 +163,19 @@ struct GJKSolver_libccd
return res;
}
+ template<typename S>
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ FCL_REAL* dist)
+ {
+ return shapeTriangleDistance(s, tf, P1, P2, P3, dist, NULL, NULL);
+ }
+
/// @brief distance computation between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf1);
void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2);
@@ -161,7 +183,9 @@ struct GJKSolver_libccd
bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
o2, details::triGetSupportFunction(),
max_distance_iterations, distance_tolerance,
- dist);
+ dist, p1, p2);
+ if(p1) *p1 = inverse(tf1).transform(*p1);
+ if(p2) *p2 = inverse(tf2).transform(*p2);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
@@ -169,7 +193,14 @@ struct GJKSolver_libccd
return res;
}
-
+ template<typename S>
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL* dist) const
+ {
+ return shapeTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, NULL, NULL);
+ }
+
/// @brief default setting for GJK algorithm
GJKSolver_libccd()
{
@@ -179,6 +210,23 @@ struct GJKSolver_libccd
distance_tolerance = 1e-6;
}
+
+ void enableCachedGuess(bool if_enable) const
+ {
+ // TODO: need change libccd to exploit spatial coherence
+ }
+
+ void setCachedGuess(const Vec3f& guess) const
+ {
+ // TODO: need change libccd to exploit spatial coherence
+ }
+
+ Vec3f getCachedGuess() const
+ {
+ return Vec3f(-1, 0, 0);
+ }
+
+
/// @brief maximum number of iterations used in GJK algorithm for collision
unsigned int max_collision_iterations;
@@ -198,8 +246,8 @@ struct GJKSolver_libccd
/// @brief Fast implementation for sphere-capsule collision
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-sphere collision
template<>
@@ -306,30 +354,30 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist) const;
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
- FCL_REAL* dist) const;
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist) const;
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist) const;
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver_indep
-{
+{
/// @brief intersection checking between two shapes
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
@@ -337,6 +385,8 @@ struct GJKSolver_indep
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Vec3f guess(1, 0, 0);
+ if(enable_cached_guess) guess = cached_guess;
+
details::MinkowskiDiff shape;
shape.shapes[0] = &s1;
shape.shapes[1] = &s2;
@@ -345,6 +395,8 @@ struct GJKSolver_indep
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+
switch(gjk_status)
{
case details::GJK::Inside:
@@ -379,8 +431,11 @@ struct GJKSolver_indep
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- Triangle2 tri(P1, P2, P3);
+ TriangleP tri(P1, P2, P3);
+
Vec3f guess(1, 0, 0);
+ if(enable_cached_guess) guess = cached_guess;
+
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
@@ -389,6 +444,8 @@ struct GJKSolver_indep
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+
switch(gjk_status)
{
case details::GJK::Inside:
@@ -423,8 +480,11 @@ struct GJKSolver_indep
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- Triangle2 tri(P1, P2, P3);
+ TriangleP tri(P1, P2, P3);
+
Vec3f guess(1, 0, 0);
+ if(enable_cached_guess) guess = cached_guess;
+
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
@@ -433,6 +493,8 @@ struct GJKSolver_indep
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+
switch(gjk_status)
{
case details::GJK::Inside:
@@ -465,9 +527,11 @@ struct GJKSolver_indep
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
- FCL_REAL* distance) const
+ FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
{
Vec3f guess(1, 0, 0);
+ if(enable_cached_guess) guess = cached_guess;
+
details::MinkowskiDiff shape;
shape.shapes[0] = &s1;
shape.shapes[1] = &s2;
@@ -476,6 +540,8 @@ struct GJKSolver_indep
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+
if(gjk_status == details::GJK::Valid)
{
Vec3f w0, w1;
@@ -486,24 +552,38 @@ struct GJKSolver_indep
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- *distance = (w0 - w1).length();
+ if(distance) *distance = (w0 - w1).length();
+
+ if(p1) *p1 = w0;
+ if(p2) *p2 = shape.toshape0.transform(w1);
+
return true;
}
else
{
- *distance = -1;
+ if(distance) *distance = -1;
return false;
}
}
+ template<typename S1, typename S2>
+ bool shapeDistance(const S1& s1, const Transform3f& tf1,
+ const S2& s2, const Transform3f& tf2,
+ FCL_REAL* distance) const
+ {
+ return shapeDistance(s1, tf1, s2, tf2, distance, NULL, NULL);
+ }
+
/// @brief distance computation between one shape and a triangle
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* distance) const
+ FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
{
- Triangle2 tri(P1, P2, P3);
+ TriangleP tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
+ if(enable_cached_guess) guess = cached_guess;
+
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
@@ -512,6 +592,8 @@ struct GJKSolver_indep
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+
if(gjk_status == details::GJK::Valid)
{
Vec3f w0, w1;
@@ -522,24 +604,36 @@ struct GJKSolver_indep
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- *distance = (w0 - w1).length();
+ if(distance) *distance = (w0 - w1).length();
+ if(p1) *p1 = w0;
+ if(p2) *p2 = shape.toshape0.transform(w1);
return true;
}
else
{
- *distance = -1;
+ if(distance) *distance = -1;
return false;
}
}
+ template<typename S>
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ FCL_REAL* distance) const
+ {
+ return shapeTriangleDistance(s, tf, P1, P2, P3, distance, NULL, NULL);
+ }
+
/// @brief distance computation between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* distance) const
+ FCL_REAL* distance, Vec3f* p1, Vec3f* p2) const
{
- Triangle2 tri(P1, P2, P3);
+ TriangleP tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
+ if(enable_cached_guess) guess = cached_guess;
+
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
@@ -548,6 +642,8 @@ struct GJKSolver_indep
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
+
if(gjk_status == details::GJK::Valid)
{
Vec3f w0, w1;
@@ -558,16 +654,26 @@ struct GJKSolver_indep
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- *distance = (w0 - w1).length();
+ if(distance) *distance = (w0 - w1).length();
+ if(p1) *p1 = w0;
+ if(p2) *p2 = shape.toshape0.transform(w1);
return true;
}
else
{
- *distance = -1;
+ if(distance) *distance = -1;
return false;
}
}
+ template<typename S>
+ bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL* distance) const
+ {
+ return shapeTriangleDistance(s, tf1, P1, P2, P3, tf2, distance, NULL, NULL);
+ }
+
/// @brief default setting for GJK algorithm
GJKSolver_indep()
{
@@ -577,6 +683,23 @@ struct GJKSolver_indep
epa_max_vertex_num = 64;
epa_max_iterations = 255;
epa_tolerance = 1e-6;
+ enable_cached_guess = false;
+ cached_guess = Vec3f(1, 0, 0);
+ }
+
+ void enableCachedGuess(bool if_enable) const
+ {
+ enable_cached_guess = if_enable;
+ }
+
+ void setCachedGuess(const Vec3f& guess) const
+ {
+ cached_guess = guess;
+ }
+
+ Vec3f getCachedGuess() const
+ {
+ return cached_guess;
}
/// @brief maximum number of simplex face used in EPA algorithm
@@ -596,12 +719,18 @@ struct GJKSolver_indep
/// @brief maximum number of iterations used for GJK iterations
FCL_REAL gjk_max_iterations;
+
+ /// @brief Whether smart guess can be provided
+ mutable bool enable_cached_guess;
+
+ /// @brief smart guess
+ mutable Vec3f cached_guess;
};
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
- const Capsule &s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Capsule &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-sphere collision
template<>
@@ -617,73 +746,73 @@ bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f&
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
template<>
@@ -697,35 +826,35 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f&
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist) const;
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
- FCL_REAL* dist) const;
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist) const;
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist) const;
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
}
diff --git a/include/fcl/penetration_depth.h b/include/fcl/penetration_depth.h
new file mode 100644
index 00000000..97ccff5c
--- /dev/null
+++ b/include/fcl/penetration_depth.h
@@ -0,0 +1,24 @@
+#ifndef FCL_PENETRATION_DEPTH_H
+#define FCL_PENETRATION_DEPTH_H
+
+#include "fcl/math/vec_3f.h"
+#include "fcl/collision_object.h"
+#include "fcl/collision_data.h"
+
+namespace fcl
+{
+
+/// @brief penetration depth computation between two in-collision objects
+FCL_REAL penetration_depth(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const PenetrationDepthRequest& request,
+ PenetrationDepthResult& result);
+
+FCL_REAL penetration_depth(const CollisionObject* o1,
+ const CollisionObject* o2,
+ const PenetrationDepthRequest& request,
+ PenetrationDepthResult& result);
+
+}
+
+#endif
diff --git a/include/fcl/shape/geometric_shapes.h b/include/fcl/shape/geometric_shapes.h
index 5c64ae4c..818833ff 100644
--- a/include/fcl/shape/geometric_shapes.h
+++ b/include/fcl/shape/geometric_shapes.h
@@ -56,10 +56,10 @@ public:
};
/// @brief Triangle stores the points instead of only indices of points
-class Triangle2 : public ShapeBase
+class TriangleP : public ShapeBase
{
public:
- Triangle2(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
+ TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
{
}
diff --git a/include/fcl/shape/geometric_shapes_utility.h b/include/fcl/shape/geometric_shapes_utility.h
index 15acae9f..3497239f 100644
--- a/include/fcl/shape/geometric_shapes_utility.h
+++ b/include/fcl/shape/geometric_shapes_utility.h
@@ -55,7 +55,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& t
std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf);
-std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const TriangleP& triangle, const Transform3f& tf);
}
/// @endcond
@@ -87,7 +87,7 @@ template<>
void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv);
template<>
-void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv);
+void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv);
diff --git a/include/fcl/traversal/traversal_node_base.h b/include/fcl/traversal/traversal_node_base.h
index c61ba68f..a53c2b24 100644
--- a/include/fcl/traversal/traversal_node_base.h
+++ b/include/fcl/traversal/traversal_node_base.h
@@ -145,6 +145,20 @@ public:
bool enable_statistics;
};
+
+struct ConservativeAdvancementStackData
+{
+ ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_)
+ : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
+
+ Vec3f P1;
+ Vec3f P2;
+ int c1;
+ int c2;
+ FCL_REAL d;
+};
+
+
}
#endif
diff --git a/include/fcl/traversal/traversal_node_bvh_shape.h b/include/fcl/traversal/traversal_node_bvh_shape.h
index 52394030..fe62b289 100644
--- a/include/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/fcl/traversal/traversal_node_bvh_shape.h
@@ -732,9 +732,10 @@ public:
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL d;
- nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d);
+ Vec3f closest_p1, closest_p2;
+ nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &closest_p2, &closest_p1);
- this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE);
+ this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2);
}
/// @brief Whether the traversal process can stop early
@@ -781,9 +782,10 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */,
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL distance;
- nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance);
+ Vec3f closest_p1, closest_p2;
+ nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1);
- result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE);
+ result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2);
}
@@ -802,9 +804,10 @@ static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
const Vec3f& p3 = vertices[init_tri[2]];
FCL_REAL distance;
- nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance);
+ Vec3f closest_p1, closest_p2;
+ nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1);
- result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE);
+ result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, closest_p1, closest_p2);
}
@@ -944,9 +947,10 @@ public:
const Vec3f& p3 = vertices[tri_id[2]];
FCL_REAL distance;
- nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance);
+ Vec3f closest_p1, closest_p2;
+ nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance, &closest_p1, &closest_p2);
- this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id);
+ this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id, closest_p1, closest_p2);
}
/// @brief Whether the traversal process can stop early
@@ -1061,6 +1065,567 @@ public:
}
};
+
+
+/// @brief Traversal node for conservative advancement computation between BVH and shape
+template<typename BV, typename S, typename NarrowPhaseSolver>
+class MeshShapeConservativeAdvancementTraversalNode : public MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>()
+ {
+ delta_t = 1;
+ toc = 0;
+ t_err = (FCL_REAL)0.0001;
+
+ w = w_;
+
+ motion1 = NULL;
+ motion2 = NULL;
+ }
+
+ /// @brief BV culling test in one BVTT node
+ FCL_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = this->model2_bv.distance(this->model1->getBV(b1).bv, &P2, &P1);
+
+ stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
+
+ return d;
+ }
+
+ /// @brief Conservative advancement testing between leaves (one triangle and one shape)
+ void leafTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_leaf_tests++;
+
+ const BVNode<BV>& node = this->model1->getBV(b1);
+
+ int primitive_id = node.primitiveId();
+
+ const Triangle& tri_id = this->tri_indices[primitive_id];
+
+ const Vec3f& p1 = this->vertices[tri_id[0]];
+ const Vec3f& p2 = this->vertices[tri_id[1]];
+ const Vec3f& p3 = this->vertices[tri_id[2]];
+
+ FCL_REAL d;
+ Vec3f P1, P2;
+ this->nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &P2, &P1);
+
+ if(d < this->min_distance)
+ {
+ this->min_distance = d;
+
+ closest_p1 = P1;
+ closest_p2 = P2;
+
+ last_tri_id = primitive_id;
+ }
+
+ Vec3f n = this->tf2.transform(P2) - P1; n.normalize();
+ // here n should be in global frame
+ TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n);
+ TBVMotionBoundVisitor<BV> mb_visitor2(this->model2_bv, -n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
+
+ FCL_REAL bound = bound1 + bound2;
+
+ FCL_REAL cur_delta_t;
+ if(bound <= d) cur_delta_t = 1;
+ else cur_delta_t = d / bound;
+
+ if(cur_delta_t < delta_t)
+ delta_t = cur_delta_t;
+ }
+
+ /// @brief Whether the traversal process can stop early
+ bool canStop(FCL_REAL c) const
+ {
+ if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
+ {
+ const ConservativeAdvancementStackData& data = stack.back();
+
+ Vec3f n = this->tf2.transform(data.P2) - data.P1; n.normalize();
+ int c1 = data.c1;
+
+ TBVMotionBoundVisitor<BV> mb_visitor1(this->model1->getBV(c1).bv, n);
+ TBVMotionBoundVisitor<BV> mb_visitor2(this->model2_bv, -n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
+
+ FCL_REAL bound = bound1 + bound2;
+
+ FCL_REAL cur_delta_t;
+ if(bound < c) cur_delta_t = 1;
+ else cur_delta_t = c / bound;
+
+ if(cur_delta_t < delta_t)
+ delta_t = cur_delta_t;
+
+ stack.pop_back();
+
+ return true;
+ }
+ else
+ {
+ stack.pop_back();
+
+ return false;
+ }
+ }
+
+ mutable FCL_REAL min_distance;
+
+ mutable Vec3f closest_p1, closest_p2;
+
+ mutable int last_tri_id;
+
+ /// @brief CA controlling variable: early stop for the early iterations of CA
+ FCL_REAL w;
+
+ /// @brief The time from beginning point
+ FCL_REAL toc;
+ FCL_REAL t_err;
+
+ /// @brief The delta_t each step
+ mutable FCL_REAL delta_t;
+
+ /// @brief Motions for the two objects in query
+ const MotionBase* motion1;
+ const MotionBase* motion2;
+
+ mutable std::vector<ConservativeAdvancementStackData> stack;
+};
+
+
+template<typename S, typename BV, typename NarrowPhaseSolver>
+class ShapeMeshConservativeAdvancementTraversalNode : public ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>()
+ {
+ delta_t = 1;
+ toc = 0;
+ t_err = (FCL_REAL)0.0001;
+
+ w = w_;
+
+ motion1 = NULL;
+ motion2 = NULL;
+ }
+
+ /// @brief BV culling test in one BVTT node
+ FCL_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = this->model1_bv.distance(this->model2->getBV(b2).bv, &P1, &P2);
+
+ stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
+
+ return d;
+ }
+
+ /// @brief Conservative advancement testing between leaves (one triangle and one shape)
+ void leafTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_leaf_tests++;
+
+ const BVNode<BV>& node = this->model2->getBV(b2);
+
+ int primitive_id = node.primitiveId();
+
+ const Triangle& tri_id = this->tri_indices[primitive_id];
+
+ const Vec3f& p1 = this->vertices[tri_id[0]];
+ const Vec3f& p2 = this->vertices[tri_id[1]];
+ const Vec3f& p3 = this->vertices[tri_id[2]];
+
+ FCL_REAL d;
+ Vec3f P1, P2;
+ this->nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d, &P1, &P2);
+
+ if(d < this->min_distance)
+ {
+ this->min_distance = d;
+
+ closest_p1 = P1;
+ closest_p2 = P2;
+
+ last_tri_id = primitive_id;
+ }
+
+ Vec3f n = P2 - this->tf1.transform(P1); n.normalize();
+ // here n should be in global frame
+ TBVMotionBoundVisitor<BV> mb_visitor1(this->model1_bv, n);
+ TriangleMotionBoundVisitor mb_visitor2(p1, p2, p3, -n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
+
+ FCL_REAL bound = bound1 + bound2;
+
+ FCL_REAL cur_delta_t;
+ if(bound <= d) cur_delta_t = 1;
+ else cur_delta_t = d / bound;
+
+ if(cur_delta_t < delta_t)
+ delta_t = cur_delta_t;
+ }
+
+ /// @brief Whether the traversal process can stop early
+ bool canStop(FCL_REAL c) const
+ {
+ if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
+ {
+ const ConservativeAdvancementStackData& data = stack.back();
+
+ Vec3f n = data.P2 - this->tf1.transform(data.P1); n.normalize();
+ int c2 = data.c2;
+
+ TBVMotionBoundVisitor<BV> mb_visitor1(this->model1_bv, n);
+ TBVMotionBoundVisitor<BV> mb_visitor2(this->model2->getBV(c2).bv, -n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
+
+ FCL_REAL bound = bound1 + bound2;
+
+ FCL_REAL cur_delta_t;
+ if(bound < c) cur_delta_t = 1;
+ else cur_delta_t = c / bound;
+
+ if(cur_delta_t < delta_t)
+ delta_t = cur_delta_t;
+
+ stack.pop_back();
+
+ return true;
+ }
+ else
+ {
+ stack.pop_back();
+
+ return false;
+ }
+ }
+
+ mutable FCL_REAL min_distance;
+
+ mutable Vec3f closest_p1, closest_p2;
+
+ mutable int last_tri_id;
+
+ /// @brief CA controlling variable: early stop for the early iterations of CA
+ FCL_REAL w;
+
+ /// @brief The time from beginning point
+ FCL_REAL toc;
+ FCL_REAL t_err;
+
+ /// @brief The delta_t each step
+ mutable FCL_REAL delta_t;
+
+ /// @brief Motions for the two objects in query
+ const MotionBase* motion1;
+ const MotionBase* motion2;
+
+ mutable std::vector<ConservativeAdvancementStackData> stack;
+};
+
+namespace details
+{
+template<typename BV, typename S, typename NarrowPhaseSolver>
+void meshShapeConservativeAdvancementOrientedNodeLeafTesting(int b1, int /* b2 */,
+ const BVHModel<BV>* model1, const S& model2,
+ const BV& model2_bv,
+ Vec3f* vertices, Triangle* tri_indices,
+ const Transform3f& tf1,
+ const Transform3f& tf2,
+ const MotionBase* motion1, const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ bool enable_statistics,
+ FCL_REAL& min_distance,
+ Vec3f& p1, Vec3f& p2,
+ int& last_tri_id,
+ FCL_REAL& delta_t,
+ int& num_leaf_tests)
+{
+ if(enable_statistics) num_leaf_tests++;
+
+ const BVNode<BV>& node = model1->getBV(b1);
+ int primitive_id = node.primitiveId();
+
+ const Triangle& tri_id = tri_indices[primitive_id];
+ const Vec3f& t1 = vertices[tri_id[0]];
+ const Vec3f& t2 = vertices[tri_id[1]];
+ const Vec3f& t3 = vertices[tri_id[2]];
+
+ FCL_REAL distance;
+ Vec3f P1, P2;
+ nsolver->shapeTriangleDistance(model2, tf2, t1, t2, t3, tf1, &distance, &P2, &P1);
+
+ if(distance < min_distance)
+ {
+ min_distance = distance;
+
+ p1 = P1;
+ p2 = P2;
+
+ last_tri_id = primitive_id;
+ }
+
+ // n is in global frame
+ Vec3f n = P2 - P1; n.normalize();
+
+ TriangleMotionBoundVisitor mb_visitor1(t1, t2, t3, n);
+ TBVMotionBoundVisitor<BV> mb_visitor2(model2_bv, -n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
+
+ FCL_REAL bound = bound1 + bound2;
+
+ FCL_REAL cur_delta_t;
+ if(bound <= distance) cur_delta_t = 1;
+ else cur_delta_t = distance / bound;
+
+ if(cur_delta_t < delta_t)
+ delta_t = cur_delta_t;
+}
+
+
+template<typename BV, typename S>
+bool meshShapeConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
+ FCL_REAL min_distance,
+ FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
+ const BVHModel<BV>* model1, const S& model2,
+ const BV& model2_bv,
+ const MotionBase* motion1, const MotionBase* motion2,
+ std::vector<ConservativeAdvancementStackData>& stack,
+ FCL_REAL& delta_t)
+{
+ if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
+ {
+ const ConservativeAdvancementStackData& data = stack.back();
+ Vec3f n = data.P2 - data.P1; n.normalize();
+ int c1 = data.c1;
+
+ TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n);
+ TBVMotionBoundVisitor<BV> mb_visitor2(model2_bv, -n);
+
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
+
+ FCL_REAL bound = bound1 + bound2;
+
+ FCL_REAL cur_delta_t;
+ if(bound <= c) cur_delta_t = 1;
+ else cur_delta_t = c / bound;
+
+ if(cur_delta_t < delta_t)
+ delta_t = cur_delta_t;
+
+ stack.pop_back();
+
+ return true;
+ }
+ else
+ {
+ stack.pop_back();
+ return false;
+ }
+}
+
+
+}
+
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeConservativeAdvancementTraversalNodeRSS : public MeshShapeConservativeAdvancementTraversalNode<RSS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode<RSS, S, NarrowPhaseSolver>(w_)
+ {
+ }
+
+ FCL_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
+
+ this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
+
+ return d;
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2),
+ this->model2_bv,
+ this->vertices, this->tri_indices,
+ this->tf1, this->tf2,
+ this->motion1, this->motion2,
+ this->nsolver,
+ this->enable_statistics,
+ this->min_distance,
+ this->closest_p1, this->closest_p2,
+ this->last_tri_id,
+ this->delta_t,
+ this->num_leaf_tests);
+ }
+
+ bool canStop(FCL_REAL c) const
+ {
+ return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
+ this->abs_err, this->rel_err, this->w,
+ this->model1, *(this->model2), this->model2_bv,
+ this->motion1, this->motion2,
+ this->stack, this->delta_t);
+ }
+};
+
+
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeConservativeAdvancementTraversalNodeOBBRSS : public MeshShapeConservativeAdvancementTraversalNode<OBBRSS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode<OBBRSS, S, NarrowPhaseSolver>(w_)
+ {
+ }
+
+ FCL_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2);
+
+ this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
+
+ return d;
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2),
+ this->model2_bv,
+ this->vertices, this->tri_indices,
+ this->tf1, this->tf2,
+ this->motion1, this->motion2,
+ this->nsolver,
+ this->enable_statistics,
+ this->min_distance,
+ this->closest_p1, this->closest_p2,
+ this->last_tri_id,
+ this->delta_t,
+ this->num_leaf_tests);
+ }
+
+ bool canStop(FCL_REAL c) const
+ {
+ return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
+ this->abs_err, this->rel_err, this->w,
+ this->model1, *(this->model2), this->model2_bv,
+ this->motion1, this->motion2,
+ this->stack, this->delta_t);
+ }
+};
+
+
+
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshConservativeAdvancementTraversalNodeRSS : public ShapeMeshConservativeAdvancementTraversalNode<S, RSS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode<S, RSS, NarrowPhaseSolver>(w_)
+ {
+ }
+
+ FCL_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
+
+ this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
+
+ return d;
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1),
+ this->model1_bv,
+ this->vertices, this->tri_indices,
+ this->tf2, this->tf1,
+ this->motion2, this->motion1,
+ this->nsolver,
+ this->enable_statistics,
+ this->min_distance,
+ this->closest_p2, this->closest_p1,
+ this->last_tri_id,
+ this->delta_t,
+ this->num_leaf_tests);
+ }
+
+ bool canStop(FCL_REAL c) const
+ {
+ return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
+ this->abs_err, this->rel_err, this->w,
+ this->model2, *(this->model1), this->model1_bv,
+ this->motion2, this->motion1,
+ this->stack, this->delta_t);
+ }
+};
+
+
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshConservativeAdvancementTraversalNodeOBBRSS : public ShapeMeshConservativeAdvancementTraversalNode<S, OBBRSS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode<S, OBBRSS, NarrowPhaseSolver>(w_)
+ {
+ }
+
+ FCL_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1);
+
+ this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
+
+ return d;
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1),
+ this->model1_bv,
+ this->vertices, this->tri_indices,
+ this->tf2, this->tf1,
+ this->motion2, this->motion1,
+ this->nsolver,
+ this->enable_statistics,
+ this->min_distance,
+ this->closest_p2, this->closest_p1,
+ this->last_tri_id,
+ this->delta_t,
+ this->num_leaf_tests);
+ }
+
+ bool canStop(FCL_REAL c) const
+ {
+ return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance,
+ this->abs_err, this->rel_err, this->w,
+ this->model2, *(this->model1), this->model1_bv,
+ this->motion2, this->motion1,
+ this->stack, this->delta_t);
+ }
+};
+
+
}
#endif
diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h
index 9ddd8b54..3ceb22e7 100644
--- a/include/fcl/traversal/traversal_node_bvhs.h
+++ b/include/fcl/traversal/traversal_node_bvhs.h
@@ -340,6 +340,7 @@ public:
num_vf_tests = 0;
num_ee_tests = 0;
+ time_of_contact = 1;
}
/// @brief Intersection testing between leaves (two triangles)
@@ -422,7 +423,10 @@ public:
}
if(!(collision_time > 1)) // collision happens
+ {
pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
+ time_of_contact = std::min(time_of_contact, collision_time);
+ }
}
/// @brief Whether the traversal process can stop early
@@ -444,6 +448,8 @@ public:
mutable int num_ee_tests;
mutable std::vector<BVHContinuousCollisionPair> pairs;
+
+ mutable FCL_REAL time_of_contact;
};
@@ -544,8 +550,8 @@ public:
tri_indices1 = NULL;
tri_indices2 = NULL;
- rel_err = 0;
- abs_err = 0;
+ rel_err = this->request.rel_err;
+ abs_err = this->request.abs_err;
}
/// @brief Distance testing between leaves (two triangles)
@@ -659,17 +665,6 @@ public:
};
-struct ConservativeAdvancementStackData
-{
- ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_)
- : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
-
- Vec3f P1;
- Vec3f P2;
- int c1;
- int c2;
- FCL_REAL d;
-};
/// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
template<typename BV>
@@ -732,16 +727,16 @@ public:
{
this->min_distance = d;
- this->p1 = P1;
- this->p2 = P2;
-
- this->last_tri_id1 = primitive_id1;
- this->last_tri_id2 = primitive_id2;
+ closest_p1 = P1;
+ closest_p2 = P2;
+
+ last_tri_id1 = primitive_id1;
+ last_tri_id2 = primitive_id2;
}
- // n is the local frame of object 1
Vec3f n = P2 - P1;
+ n.normalize();
// here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n), mb_visitor2(q1, q2, q3, n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
@@ -771,21 +766,21 @@ public:
{
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
- n = data2.P2 - data2.P1;
+ n = data2.P2 - data2.P1; n.normalize();
c1 = data2.c1;
c2 = data2.c2;
stack[stack.size() - 2] = stack[stack.size() - 1];
}
else
{
- n = data.P2 - data.P1;
+ n = data.P2 - data.P1; n.normalize();
c1 = data.c1;
c2 = data.c2;
}
assert(c == d);
- TBVMotionBoundVisitor<BV> mb_visitor1((this->tree1 + c1)->bv, n), mb_visitor2((this->tree2 + c2)->bv, n);
+ TBVMotionBoundVisitor<BV> mb_visitor1(this->model1->getBV(c1).bv, n), mb_visitor2(this->model2->getBV(c2).bv, n);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
@@ -817,9 +812,9 @@ public:
}
mutable FCL_REAL min_distance;
-
- mutable Vec3f p1, p2;
-
+
+ mutable Vec3f closest_p1, closest_p2;
+
mutable int last_tri_id1, last_tri_id2;
diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h
index 9cc1d50d..3ef5da22 100644
--- a/include/fcl/traversal/traversal_node_octree.h
+++ b/include/fcl/traversal/traversal_node_octree.h
@@ -247,9 +247,10 @@ private:
constructBox(bv1, tf1, box, box_tf);
FCL_REAL dist;
- solver->shapeDistance(box, box_tf, s, tf2, &dist);
+ Vec3f closest_p1, closest_p2;
+ solver->shapeDistance(box, box_tf, s, tf2, &dist, &closest_p1, &closest_p2);
- dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE);
+ dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE, closest_p1, closest_p2);
return drequest->isSatisfied(*dresult);
}
@@ -440,7 +441,8 @@ private:
const Vec3f& p3 = tree2->vertices[tri_id[2]];
FCL_REAL dist;
- solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist);
+ Vec3f closest_p1, closest_p2;
+ solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2);
dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id);
@@ -709,9 +711,10 @@ private:
constructBox(bv2, tf2, box2, box2_tf);
FCL_REAL dist;
- solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
+ Vec3f closest_p1, closest_p2;
+ solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist, &closest_p1, &closest_p2);
- dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot());
+ dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2);
return drequest->isSatisfied(*dresult);
}
diff --git a/include/fcl/traversal/traversal_node_setup.h b/include/fcl/traversal/traversal_node_setup.h
index fd73e34b..5f01167c 100644
--- a/include/fcl/traversal/traversal_node_setup.h
+++ b/include/fcl/traversal/traversal_node_setup.h
@@ -1023,9 +1023,6 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
const BVHModel<BV>& model2, const Transform3f& tf2,
const CollisionRequest& request)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
@@ -1053,9 +1050,6 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
FCL_REAL w = 1,
bool use_refit = false, bool refit_bottomup = false)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
for(int i = 0; i < model1.num_vertices; ++i)
{
@@ -1107,6 +1101,186 @@ bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
FCL_REAL w = 1);
+template<typename S1, typename S2, typename NarrowPhaseSolver>
+bool initialize(ShapeConservativeAdvancementTraversalNode<S1, S2, NarrowPhaseSolver>& node,
+ const S1& shape1, const Transform3f& tf1,
+ const S2& shape2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ node.model1 = &shape1;
+ node.tf1 = tf1;
+ node.model2 = &shape2;
+ node.tf2 = tf2;
+ node.nsolver = nsolver;
+
+ computeBV<RSS, S1>(shape1, Transform3f(), node.model1_bv);
+ computeBV<RSS, S2>(shape2, Transform3f(), node.model2_bv);
+
+ return true;
+}
+
+template<typename BV, typename S, typename NarrowPhaseSolver>
+bool initialize(MeshShapeConservativeAdvancementTraversalNode<BV, S, NarrowPhaseSolver>& node,
+ BVHModel<BV>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver,
+ FCL_REAL w = 1,
+ bool use_refit = false, bool refit_bottomup = false)
+{
+ std::vector<Vec3f> vertices_transformed(model1.num_vertices);
+ for(int i = 0; i < model1.num_vertices; ++i)
+ {
+ Vec3f& p = model1.vertices[i];
+ Vec3f new_v = tf1.transform(p);
+ vertices_transformed[i] = new_v;
+ }
+
+ model1.beginReplaceModel();
+ model1.replaceSubModel(vertices_transformed);
+ model1.endReplaceModel(use_refit, refit_bottomup);
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.vertices = model1.vertices;
+ node.tri_indices = model1.tri_indices;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ node.nsolver = nsolver;
+ node.w = w;
+
+ computeBV<BV, S>(model2, Transform3f(), node.model2_bv);
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(MeshShapeConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver,
+ FCL_REAL w = 1)
+{
+ node.model1 = &model1;
+ node.tf1 = tf1;
+ node.model2 = &model2;
+ node.tf2 = tf2;
+ node.nsolver = nsolver;
+
+ node.w = w;
+
+ computeBV<RSS, S>(model2, Transform3f(), node.model2_bv);
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(MeshShapeConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const S& model2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver,
+ FCL_REAL w = 1)
+{
+ node.model1 = &model1;
+ node.tf1 = tf1;
+ node.model2 = &model2;
+ node.tf2 = tf2;
+ node.nsolver = nsolver;
+
+ node.w = w;
+
+ computeBV<OBBRSS, S>(model2, Transform3f(), node.model2_bv);
+
+ return true;
+}
+
+
+template<typename S, typename BV, typename NarrowPhaseSolver>
+bool initialize(ShapeMeshConservativeAdvancementTraversalNode<S, BV, NarrowPhaseSolver>& node,
+ const S& model1, const Transform3f& tf1,
+ BVHModel<BV>& model2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver,
+ FCL_REAL w = 1,
+ bool use_refit = false, bool refit_bottomup = false)
+{
+ std::vector<Vec3f> vertices_transformed(model2.num_vertices);
+ for(int i = 0; i < model2.num_vertices; ++i)
+ {
+ Vec3f& p = model2.vertices[i];
+ Vec3f new_v = tf2.transform(p);
+ vertices_transformed[i] = new_v;
+ }
+
+ model2.beginReplaceModel();
+ model2.replaceSubModel(vertices_transformed);
+ model2.endReplaceModel(use_refit, refit_bottomup);
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.vertices = model2.vertices;
+ node.tri_indices = model2.tri_indices;
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ node.nsolver = nsolver;
+ node.w = w;
+
+ computeBV<BV, S>(model1, Transform3f(), node.model1_bv);
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeMeshConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver,
+ FCL_REAL w = 1)
+{
+ node.model1 = &model1;
+ node.tf1 = tf1;
+ node.model2 = &model2;
+ node.tf2 = tf2;
+ node.nsolver = nsolver;
+
+ node.w = w;
+
+ computeBV<RSS, S>(model1, Transform3f(), node.model1_bv);
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+ const S& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
+ const NarrowPhaseSolver* nsolver,
+ FCL_REAL w = 1)
+{
+ node.model1 = &model1;
+ node.tf1 = tf1;
+ node.model2 = &model2;
+ node.tf2 = tf2;
+ node.nsolver = nsolver;
+
+ node.w = w;
+
+ computeBV<OBBRSS, S>(model1, Transform3f(), node.model1_bv);
+
+ return true;
+}
+
+
+
+
}
#endif
diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h
index 3bb3cc3d..fb2c4062 100644
--- a/include/fcl/traversal/traversal_node_shapes.h
+++ b/include/fcl/traversal/traversal_node_shapes.h
@@ -42,6 +42,9 @@
#include "fcl/traversal/traversal_node_base.h"
#include "fcl/narrowphase/narrowphase.h"
#include "fcl/shape/geometric_shapes_utility.h"
+#include "fcl/BV/BV.h"
+#include "fcl/shape/geometric_shapes_utility.h"
+#include "fcl/ccd/motion.h"
namespace fcl
{
@@ -148,8 +151,9 @@ public:
void leafTesting(int, int) const
{
FCL_REAL distance;
- nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance);
- result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE);
+ Vec3f closest_p1, closest_p2;
+ nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance, &closest_p1, &closest_p2);
+ result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE, closest_p1, closest_p2);
}
const S1* model1;
@@ -158,6 +162,58 @@ public:
const NarrowPhaseSolver* nsolver;
};
+template<typename S1, typename S2, typename NarrowPhaseSolver>
+class ShapeConservativeAdvancementTraversalNode : public ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>
+{
+public:
+ ShapeConservativeAdvancementTraversalNode() : ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>()
+ {
+ delta_t = 1;
+ toc = 0;
+ t_err = (FCL_REAL)0.0001;
+
+ motion1 = NULL;
+ motion2 = NULL;
+ }
+
+ void leafTesting(int, int) const
+ {
+ FCL_REAL distance;
+ Vec3f closest_p1, closest_p2;
+ this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2);
+
+ Vec3f n = this->tf2.transform(closest_p2) - this->tf1.transform(closest_p1); n.normalize();
+ TBVMotionBoundVisitor<RSS> mb_visitor1(model1_bv, n);
+ TBVMotionBoundVisitor<RSS> mb_visitor2(model2_bv, -n);
+ FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
+ FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2);
+
+ FCL_REAL bound = bound1 + bound2;
+
+ FCL_REAL cur_delta_t;
+ if(bound <= distance) cur_delta_t = 1;
+ else cur_delta_t = distance / bound;
+
+ if(cur_delta_t < delta_t)
+ delta_t = cur_delta_t;
+ }
+
+ mutable FCL_REAL min_distance;
+
+ /// @brief The time from beginning point
+ FCL_REAL toc;
+ FCL_REAL t_err;
+
+ /// @brief The delta_t each step
+ mutable FCL_REAL delta_t;
+
+ /// @brief Motions for the two objects in query
+ const MotionBase* motion1;
+ const MotionBase* motion2;
+
+ RSS model1_bv, model2_bv; // local bv for the two shapes
+};
+
}
diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
index c47160f8..ea23055e 100644
--- a/src/broadphase/broadphase_SSaP.cpp
+++ b/src/broadphase/broadphase_SSaP.cpp
@@ -432,7 +432,7 @@ void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) cons
if(size() == 0) return;
std::vector<CollisionObject*>::const_iterator it, it_end;
- size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
+ // size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
for(; it != it_end; ++it)
diff --git a/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp
index 9447c932..4cf9e39e 100644
--- a/src/ccd/conservative_advancement.cpp
+++ b/src/ccd/conservative_advancement.cpp
@@ -44,83 +44,513 @@
namespace fcl
+{
+
+
+
+template<typename BV>
+bool conservativeAdvancement(const BVHModel<BV>& o1,
+ const MotionBase* motion1,
+ const BVHModel<BV>& o2,
+ const MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
{
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+
+ // whether the first start configuration is in collision
+ if(collide(&o1, tf1, &o2, tf2, request, result))
+ {
+ toc = 0;
+ return true;
+ }
+
+
+ BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
+ BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
+
+
+ MeshConservativeAdvancementTraversalNode<BV> node;
+
+ node.motion1 = motion1;
+ node.motion2 = motion2;
+
+ do
+ {
+ // repeatedly update mesh to global coordinate, so time consuming
+ initialize(node, *o1_tmp, tf1, *o2_tmp, tf2);
+
+ node.delta_t = 1;
+ node.min_distance = std::numeric_limits<FCL_REAL>::max();
+
+ distanceRecurse(&node, 0, 0, NULL);
+
+ if(node.delta_t <= node.t_err)
+ {
+ // std::cout << node.delta_t << " " << node.t_err << std::endl;
+ break;
+ }
+
+ node.toc += node.delta_t;
+ if(node.toc > 1)
+ {
+ node.toc = 1;
+ break;
+ }
+
+ node.motion1->integrate(node.toc);
+ node.motion2->integrate(node.toc);
+
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+ }
+ while(1);
-template<typename BV, typename ConservativeAdvancementNode, typename CollisionNode>
-int conservativeAdvancement(const CollisionGeometry* o1,
- const MotionBase* motion1,
- const CollisionGeometry* o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc)
+ delete o1_tmp;
+ delete o2_tmp;
+
+ toc = node.toc;
+
+ if(node.toc < 1)
+ return true;
+
+ return false;
+}
+
+namespace details
+{
+
+template<typename BV, typename ConservativeAdvancementOrientedNode>
+bool conservativeAdvancementMeshOriented(const BVHModel<BV>& o1,
+ const MotionBase* motion1,
+ const BVHModel<BV>& o2,
+ const MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
{
- if(request.num_max_contacts == 0)
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+
+ // whether the first start configuration is in collision
+ if(collide(&o1, tf1, &o2, tf2, request, result))
{
- std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl;
- return 0;
+ toc = 0;
+ return true;
}
+
+
+ ConservativeAdvancementOrientedNode node;
+
+ initialize(node, o1, tf1, o2, tf2);
+
+ node.motion1 = motion1;
+ node.motion2 = motion2;
+
+ do
+ {
+ node.motion1->getCurrentTransform(tf1);
+ node.motion2->getCurrentTransform(tf2);
- const OBJECT_TYPE object_type1 = o1->getObjectType();
- const NODE_TYPE node_type1 = o1->getNodeType();
+ // compute the transformation from 1 to 2
+ Transform3f tf;
+ relativeTransform(tf1, tf2, tf);
+ node.R = tf.getRotation();
+ node.T = tf.getTranslation();
+
+ node.delta_t = 1;
+ node.min_distance = std::numeric_limits<FCL_REAL>::max();
- const OBJECT_TYPE object_type2 = o2->getObjectType();
- const NODE_TYPE node_type2 = o2->getNodeType();
+ distanceRecurse(&node, 0, 0, NULL);
- if(object_type1 != OT_BVH || object_type2 != OT_BVH)
- return 0;
+ if(node.delta_t <= node.t_err)
+ {
+ // std::cout << node.delta_t << " " << node.t_err << std::endl;
+ break;
+ }
- if(node_type1 != BV_RSS || node_type2 != BV_RSS)
- return 0;
+ node.toc += node.delta_t;
+ if(node.toc > 1)
+ {
+ node.toc = 1;
+ break;
+ }
+ node.motion1->integrate(node.toc);
+ node.motion2->integrate(node.toc);
+ }
+ while(1);
- const BVHModel<BV>* model1 = static_cast<const BVHModel<BV>*>(o1);
- const BVHModel<BV>* model2 = static_cast<const BVHModel<BV>*>(o2);
+ toc = node.toc;
+ if(node.toc < 1)
+ return true;
+
+ return false;
+}
+
+
+}
+
+template<>
+bool conservativeAdvancement(const BVHModel<RSS>& o1,
+ const MotionBase* motion1,
+ const BVHModel<RSS>& o2,
+ const MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc);
+
+
+template<>
+bool conservativeAdvancement(const BVHModel<OBBRSS>& o1,
+ const MotionBase* motion1,
+ const BVHModel<OBBRSS>& o2,
+ const MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc);
+
+template<typename S1, typename S2, typename NarrowPhaseSolver>
+bool conservativeAdvancement(const S1& o1,
+ const MotionBase* motion1,
+ const S2& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* solver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
- CollisionNode cnode;
- if(!initialize(cnode, *model1, tf1, *model2, tf2, request, result))
- std::cout << "initialize error" << std::endl;
+ if(collide(&o1, tf1, &o2, tf2, request, result))
+ {
+ toc = 0;
+ return true;
+ }
+
+ ShapeConservativeAdvancementTraversalNode<S1, S2, NarrowPhaseSolver> node;
+
+ initialize(node, o1, tf1, o2, tf2, solver);
+
+ node.motion1 = motion1;
+ node.motion2 = motion2;
+
+ do
+ {
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ node.delta_t = 1;
+ node.min_distance = std::numeric_limits<FCL_REAL>::max();
+
+ distanceRecurse(&node, 0, 0, NULL);
+
+ if(node.delta_t <= node.t_err)
+ {
+ // std::cout << node.delta_t << " " << node.t_err << std::endl;
+ break;
+ }
+
+ node.toc += node.delta_t;
+ if(node.toc > 1)
+ {
+ node.toc = 1;
+ break;
+ }
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), cnode.R, cnode.T);
+ node.motion1->integrate(node.toc);
+ node.motion2->integrate(node.toc);
+ }
+ while(1);
- cnode.enable_statistics = false;
- cnode.request = request;
+ toc = node.toc;
- collide(&cnode);
+ if(node.toc < 1)
+ return true;
+
+ return false;
+}
- int b = result.numContacts();
+template<typename BV, typename S, typename NarrowPhaseSolver>
+bool conservativeAdvancement(const BVHModel<BV>& o1,
+ const MotionBase* motion1,
+ const S& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
- if(b > 0)
+ if(collide(&o1, tf1, &o2, tf2, request, result))
{
toc = 0;
- // std::cout << "zero collide" << std::endl;
- return b;
+ return true;
}
-
- ConservativeAdvancementNode node;
- initialize(node, *model1, tf1, *model2, tf2);
+ BVHModel<BV>* o1_tmp = new BVHModel<BV>(o1);
+
+ MeshShapeConservativeAdvancementTraversalNode<BV, S, NarrowPhaseSolver> node;
node.motion1 = motion1;
node.motion2 = motion2;
do
{
- Matrix3f R1_t, R2_t;
- Vec3f T1_t, T2_t;
+ // initialize update mesh to global coordinate, so time consuming
+ initialize(node, *o1_tmp, tf1, o2, tf2, nsolver);
- node.motion1->getCurrentTransform(R1_t, T1_t);
- node.motion2->getCurrentTransform(R2_t, T2_t);
+ node.delta_t = 1;
+ node.min_distance = std::numeric_limits<FCL_REAL>::max();
- // compute the transformation from 1 to 2
- relativeTransform(R1_t, T1_t, R2_t, T2_t, node.R, node.T);
+ distanceRecurse(&node, 0, 0, NULL);
+
+ if(node.delta_t <= node.t_err)
+ {
+ break;
+ }
+
+ node.toc += node.delta_t;
+ if(node.toc > 1)
+ {
+ node.toc = 1;
+ break;
+ }
+ node.motion1->integrate(node.toc);
+ node.motion2->integrate(node.toc);
+
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+ }
+ while(1);
+
+ delete o1_tmp;
+
+ toc = node.toc;
+
+ if(node.toc < 1)
+ return true;
+
+ return false;
+}
+
+namespace details
+{
+
+template<typename BV, typename S, typename NarrowPhaseSolver, typename ConservativeAdvancementOrientedNode>
+bool conservativeAdvancementMeshShapeOriented(const BVHModel<BV>& o1,
+ const MotionBase* motion1,
+ const S& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+
+ if(collide(&o1, tf1, &o2, tf2, request, result))
+ {
+ toc = 0;
+ return true;
+ }
+
+ ConservativeAdvancementOrientedNode node;
+
+ initialize(node, o1, tf1, o2, tf2, nsolver);
+
+ node.motion1 = motion1;
+ node.motion2 = motion2;
+
+ do
+ {
+ node.motion1->getCurrentTransform(tf1);
+ node.motion2->getCurrentTransform(tf2);
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
+ node.delta_t = 1;
+ node.min_distance = std::numeric_limits<FCL_REAL>::max();
+
+ distanceRecurse(&node, 0, 0, NULL);
+
+ if(node.delta_t <= node.t_err)
+ {
+ break;
+ }
+
+ node.toc += node.delta_t;
+ if(node.toc > 1)
+ {
+ node.toc = 1;
+ break;
+ }
+
+ node.motion1->integrate(node.toc);
+ node.motion2->integrate(node.toc);
+ }
+ while(1);
+
+ toc = node.toc;
+
+ if(node.toc < 1)
+ return true;
+
+ return false;
+}
+
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool conservativeAdvancement(const BVHModel<RSS>& o1,
+ const MotionBase* motion1,
+ const S& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ return details::conservativeAdvancementMeshShapeOriented<RSS, S, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
+}
+
+template<typename S, typename NarrowPhaseSolver>
+bool conservativeAdvancement(const BVHModel<OBBRSS>& o1,
+ const MotionBase* motion1,
+ const S& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ return details::conservativeAdvancementMeshShapeOriented<OBBRSS, S, NarrowPhaseSolver, MeshShapeConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
+}
+
+template<typename S, typename BV, typename NarrowPhaseSolver>
+bool conservativeAdvancement(const S& o1,
+ const MotionBase* motion1,
+ const BVHModel<BV>& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+
+ if(collide(&o1, tf1, &o2, tf2, request, result))
+ {
+ toc = 0;
+ return true;
+ }
+
+ BVHModel<BV>* o2_tmp = new BVHModel<BV>(o2);
+
+ ShapeMeshConservativeAdvancementTraversalNode<S, BV, NarrowPhaseSolver> node;
+
+ node.motion1 = motion1;
+ node.motion2 = motion2;
+
+ do
+ {
+ // initialize update mesh to global coordinate, so time consuming
+ initialize(node, o1, tf1, *o2_tmp, tf2, nsolver);
+
+ node.delta_t = 1;
+ node.min_distance = std::numeric_limits<FCL_REAL>::max();
+
+ distanceRecurse(&node, 0, 0, NULL);
+
+ if(node.delta_t <= node.t_err)
+ {
+ break;
+ }
+
+ node.toc += node.delta_t;
+ if(node.toc > 1)
+ {
+ node.toc = 1;
+ break;
+ }
+
+ node.motion1->integrate(node.toc);
+ node.motion2->integrate(node.toc);
+
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+ }
+ while(1);
+
+ delete o2_tmp;
+
+ toc = node.toc;
+
+ if(node.toc < 1)
+ return true;
+
+ return false;
+}
+
+namespace details
+{
+
+template<typename S, typename BV, typename NarrowPhaseSolver, typename ConservativeAdvancementOrientedNode>
+bool conservativeAdvancementShapeMeshOriented(const S& o1,
+ const MotionBase* motion1,
+ const BVHModel<BV>& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+
+ if(collide(&o1, tf1, &o2, tf2, request, result))
+ {
+ toc = 0;
+ return true;
+ }
+
+ ConservativeAdvancementOrientedNode node;
+
+ initialize(node, o1, tf1, o2, tf2, nsolver);
+
+ node.motion1 = motion1;
+ node.motion2 = motion2;
+
+ do
+ {
+ node.motion1->getCurrentTransform(tf1);
+ node.motion2->getCurrentTransform(tf2);
+
+ node.tf1 = tf1;
+ node.tf2 = tf2;
+
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
@@ -128,7 +558,6 @@ int conservativeAdvancement(const CollisionGeometry* o1,
if(node.delta_t <= node.t_err)
{
- // std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
}
@@ -147,27 +576,355 @@ int conservativeAdvancement(const CollisionGeometry* o1,
toc = node.toc;
if(node.toc < 1)
- return 1;
+ return true;
+
+ return false;
+}
+
+}
+
+template<typename S, typename NarrowPhaseSolver>
+bool conservativeAdvancement(const S& o1,
+ const MotionBase* motion1,
+ const BVHModel<RSS>& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ return details::conservativeAdvancementShapeMeshOriented<S, RSS, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool conservativeAdvancement(const S& o1,
+ const MotionBase* motion1,
+ const BVHModel<OBBRSS>& o2,
+ const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ return details::conservativeAdvancementShapeMeshOriented<S, OBBRSS, NarrowPhaseSolver, ShapeMeshConservativeAdvancementTraversalNodeOBBRSS<S, NarrowPhaseSolver> >(o1, motion1, o2, motion2, nsolver, request, result, toc);
+}
+
+
+
+template<>
+bool conservativeAdvancement(const BVHModel<RSS>& o1,
+ const MotionBase* motion1,
+ const BVHModel<RSS>& o2,
+ const MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ return details::conservativeAdvancementMeshOriented<RSS, MeshConservativeAdvancementTraversalNodeRSS>(o1, motion1, o2, motion2, request, result, toc);
+}
+
+template<>
+bool conservativeAdvancement(const BVHModel<OBBRSS>& o1,
+ const MotionBase* motion1,
+ const BVHModel<OBBRSS>& o2,
+ const MotionBase* motion2,
+ const CollisionRequest& request,
+ CollisionResult& result,
+ FCL_REAL& toc)
+{
+ return details::conservativeAdvancementMeshOriented<OBBRSS, MeshConservativeAdvancementTraversalNodeOBBRSS>(o1, motion1, o2, motion2, request, result, toc);
+}
- return 0;
+
+template<typename BV, typename NarrowPhaseSolver>
+FCL_REAL BVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
+{
+ const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
+ const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
+
+ CollisionRequest c_request;
+ CollisionResult c_result;
+ FCL_REAL toc;
+ bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, c_request, c_result, toc);
+
+ result.is_collide = is_collide;
+ result.time_of_contact = toc;
+
+ return toc;
+}
+
+template<typename S1, typename S2, typename NarrowPhaseSolver>
+FCL_REAL ShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
+{
+ const S1* obj1 = static_cast<const S1*>(o1);
+ const S2* obj2 = static_cast<const S2*>(o2);
+
+ CollisionRequest c_request;
+ CollisionResult c_result;
+ FCL_REAL toc;
+ bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
+
+ result.is_collide = is_collide;
+ result.time_of_contact = toc;
+
+ return toc;
+}
+
+template<typename S, typename BV, typename NarrowPhaseSolver>
+FCL_REAL ShapeBVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
+{
+ const S* obj1 = static_cast<const S*>(o1);
+ const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
+
+ CollisionRequest c_request;
+ CollisionResult c_result;
+ FCL_REAL toc;
+
+ bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
+
+ result.is_collide = is_collide;
+ result.time_of_contact = toc;
+
+ return toc;
+}
+
+template<typename BV, typename S, typename NarrowPhaseSolver>
+FCL_REAL BVHShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result)
+{
+ const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
+ const S* obj2 = static_cast<const S*>(o2);
+
+ CollisionRequest c_request;
+ CollisionResult c_result;
+ FCL_REAL toc;
+
+ bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc);
+
+ result.is_collide = is_collide;
+ result.time_of_contact = toc;
+
+ return toc;
+}
+
+template<typename NarrowPhaseSolver>
+ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>::ConservativeAdvancementFunctionMatrix()
+{
+ for(int i = 0; i < NODE_COUNT; ++i)
+ {
+ for(int j = 0; j < NODE_COUNT; ++j)
+ conservative_advancement_matrix[i][j] = NULL;
+ }
+
+
+ conservative_advancement_matrix[GEOM_BOX][GEOM_BOX] = &ShapeConservativeAdvancement<Box, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Box, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Box, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_BOX][GEOM_CONE] = &ShapeConservativeAdvancement<Box, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Box, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Box, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement<Box, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement<Sphere, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Sphere, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Sphere, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeConservativeAdvancement<Sphere, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Sphere, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Sphere, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement<Sphere, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement<Capsule, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Capsule, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Capsule, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeConservativeAdvancement<Capsule, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Capsule, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Capsule, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement<Capsule, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement<Cone, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cone, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Cone, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][GEOM_CONE] = &ShapeConservativeAdvancement<Cone, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cone, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cone, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement<Cone, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement<Cylinder, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement<Cylinder, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Cylinder, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeConservativeAdvancement<Cylinder, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Cylinder, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement<Cylinder, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement<Cylinder, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement<Convex, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement<Convex, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Convex, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeConservativeAdvancement<Convex, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Convex, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement<Convex, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement<Convex, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement<Plane, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement<Plane, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeConservativeAdvancement<Plane, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeConservativeAdvancement<Plane, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement<Plane, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement<Plane, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement<Plane, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement<AABB, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<AABB, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<AABB, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_AABB][GEOM_CONE] = &BVHShapeConservativeAdvancement<AABB, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<AABB, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<AABB, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<AABB, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBB, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBB, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<OBB, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][GEOM_CONE] = &BVHShapeConservativeAdvancement<OBB, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBB, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBB, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBB, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<OBBRSS, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<OBBRSS, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<OBBRSS, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeConservativeAdvancement<OBBRSS, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<OBBRSS, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<OBBRSS, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<OBBRSS, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement<RSS, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<RSS, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<RSS, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][GEOM_CONE] = &BVHShapeConservativeAdvancement<RSS, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<RSS, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<RSS, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<RSS, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<16>, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<16>, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<16>, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<16>, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<16>, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<16>, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<16>, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<18>, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<18>, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<18>, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<18>, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<18>, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<18>, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<18>, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement<KDOP<24>, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<KDOP<24>, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<KDOP<24>, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeConservativeAdvancement<KDOP<24>, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<KDOP<24>, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<KDOP<24>, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement<KDOP<24>, Plane, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement<kIOS, Box, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement<kIOS, Sphere, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement<kIOS, Capsule, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeConservativeAdvancement<kIOS, Cone, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement<kIOS, Cylinder, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement<kIOS, Convex, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement<kIOS, Plane, NarrowPhaseSolver>;
+
+
+ conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement<Box, AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_AABB] = &ShapeBVHConservativeAdvancement<Sphere, AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_AABB] = &ShapeBVHConservativeAdvancement<Capsule, AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_AABB] = &ShapeBVHConservativeAdvancement<Cone, AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement<Cylinder, AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement<Convex, AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement<Plane, AABB, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement<Box, OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement<Sphere, OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_OBB] = &ShapeBVHConservativeAdvancement<Capsule, OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_OBB] = &ShapeBVHConservativeAdvancement<Cone, OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement<Cylinder, OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement<Convex, OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement<Plane, OBB, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement<Box, RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement<Sphere, RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_RSS] = &ShapeBVHConservativeAdvancement<Capsule, RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_RSS] = &ShapeBVHConservativeAdvancement<Cone, RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement<Cylinder, RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement<Convex, RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement<Plane, RSS, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Box, OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Sphere, OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Capsule, OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cone, OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Cylinder, OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Convex, OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement<Plane, OBBRSS, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Box, KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Capsule, KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cone, KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Convex, KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement<Plane, KDOP<16>, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Box, KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Capsule, KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cone, KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Convex, KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement<Plane, KDOP<18>, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Box, KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Sphere, KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Capsule, KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cone, KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Cylinder, KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Convex, KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement<Plane, KDOP<24>, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Box, kIOS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Sphere, kIOS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CAPSULE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Capsule, kIOS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cone, kIOS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement<Cylinder, kIOS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement<Convex, kIOS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement<Plane, kIOS, NarrowPhaseSolver>;
+
+ conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement<AABB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement<OBB, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_RSS][BV_RSS] = &BVHConservativeAdvancement<RSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHConservativeAdvancement<OBBRSS, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP16][BV_KDOP16] = &BVHConservativeAdvancement<KDOP<16>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP18][BV_KDOP18] = &BVHConservativeAdvancement<KDOP<18>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_KDOP24][BV_KDOP24] = &BVHConservativeAdvancement<KDOP<24>, NarrowPhaseSolver>;
+ conservative_advancement_matrix[BV_kIOS][BV_kIOS] = &BVHConservativeAdvancement<kIOS, NarrowPhaseSolver>;
+
}
-template
-int conservativeAdvancement<RSS, MeshConservativeAdvancementTraversalNodeRSS, MeshCollisionTraversalNodeRSS>(const CollisionGeometry* o1,
- const MotionBase* motion1,
- const CollisionGeometry* o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc);
+template struct ConservativeAdvancementFunctionMatrix<GJKSolver_libccd>;
+template struct ConservativeAdvancementFunctionMatrix<GJKSolver_indep>;
+
+
+
+
+
+
+
-template int conservativeAdvancement<OBBRSS, MeshConservativeAdvancementTraversalNodeOBBRSS, MeshCollisionTraversalNodeOBBRSS>(const CollisionGeometry* o1,
- const MotionBase* motion1,
- const CollisionGeometry* o2,
- const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result,
- FCL_REAL& toc);
}
+
+
diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp
index 93124161..016c19b6 100644
--- a/src/ccd/motion.cpp
+++ b/src/ccd/motion.cpp
@@ -115,7 +115,7 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const SplineMotion& motion) const
}
SplineMotion::SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
- const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3)
+ const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3) : MotionBase()
{
Td[0] = Td0;
Td[1] = Td1;
@@ -437,7 +437,7 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
return mu;
}
-InterpMotion::InterpMotion()
+InterpMotion::InterpMotion() : MotionBase()
{
// Default angular velocity is zero
angular_axis.setValue(1, 0, 0);
@@ -449,20 +449,20 @@ InterpMotion::InterpMotion()
}
InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2) :
- tf1(R1, T1),
- tf2(R2, T2),
- tf(tf1)
+ const Matrix3f& R2, const Vec3f& T2) : MotionBase(),
+ tf1(R1, T1),
+ tf2(R2, T2),
+ tf(tf1)
{
// Compute the velocities for the motion
computeVelocity();
}
-InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) :
- tf1(tf1_),
- tf2(tf2_),
- tf(tf1)
+InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) : MotionBase(),
+ tf1(tf1_),
+ tf2(tf2_),
+ tf(tf1)
{
// Compute the velocities for the motion
computeVelocity();
@@ -470,21 +470,21 @@ InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) :
InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2,
- const Vec3f& O) :
- tf1(R1, T1),
- tf2(R2, T2),
- tf(tf1),
- reference_p(O)
+ const Vec3f& O) : MotionBase(),
+ tf1(R1, T1),
+ tf2(R2, T2),
+ tf(tf1),
+ reference_p(O)
{
// Compute the velocities for the motion
computeVelocity();
}
-InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) :
- tf1(tf1_),
- tf2(tf2_),
- tf(tf1),
- reference_p(O)
+InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) : MotionBase(),
+ tf1(tf1_),
+ tf2(tf2_),
+ tf(tf1),
+ reference_p(O)
{
}
@@ -525,6 +525,20 @@ Quaternion3f InterpMotion::absoluteRotation(FCL_REAL dt) const
return delta_t * tf1.getQuatRotation();
}
+
+/// @brief Compute the motion bound for a bounding volume along a given direction n
+template<>
+FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const TranslationMotion& motion) const
+{
+ return motion.getVelocity().dot(n);
+}
+
+/// @brief Compute the motion bound for a triangle along a given direction n
+FCL_REAL TriangleMotionBoundVisitor::visit(const TranslationMotion& motion) const
+{
+ return motion.getVelocity().dot(n);
+}
+
template class TBVMotionBoundVisitor<RSS>;
}
diff --git a/src/collision.cpp b/src/collision.cpp
index 68f51f6b..b481e2d4 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -70,7 +70,7 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
{
const NarrowPhaseSolver* nsolver = nsolver_;
if(!nsolver_)
- nsolver = new NarrowPhaseSolver();
+ nsolver = new NarrowPhaseSolver();
const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
@@ -115,64 +115,47 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
return res;
}
-template std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
-template std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
-template std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
-template std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
-
-
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
const CollisionRequest& request, CollisionResult& result)
{
- GJKSolver_libccd solver;
- return collide<GJKSolver_libccd>(o1, o2, &solver, request, result);
+ switch(request.gjk_solver_type)
+ {
+ case GST_LIBCCD:
+ {
+ GJKSolver_libccd solver;
+ return collide<GJKSolver_libccd>(o1, o2, &solver, request, result);
+ }
+ case GST_INDEP:
+ {
+ GJKSolver_indep solver;
+ return collide<GJKSolver_indep>(o1, o2, &solver, request, result);
+ }
+ default:
+ return -1; // error
+ }
}
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result)
{
- GJKSolver_libccd solver;
- return collide<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
- // GJKSolver_indep solver;
- // return collide<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, request, result);
-}
-
-}
-
-#include "fcl/ccd/conservative_advancement.h"
-#include "fcl/traversal/traversal_node_bvhs.h"
-namespace fcl
-{
-std::size_t collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
- const CollisionRequest& request,
- CollisionResult& result)
-{
- FCL_REAL toc;
- return conservativeAdvancement<RSS, MeshConservativeAdvancementTraversalNodeRSS, MeshCollisionTraversalNodeRSS>(
- o1->getCollisionGeometry(),
- o1->getMotion(),
- o2->getCollisionGeometry(),
- o2->getMotion(),
- request,
- result,
- toc);
+ switch(request.gjk_solver_type)
+ {
+ case GST_LIBCCD:
+ {
+ GJKSolver_libccd solver;
+ return collide<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
+ }
+ case GST_INDEP:
+ {
+ GJKSolver_indep solver;
+ return collide<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, request, result);
+ }
+ default:
+ std::cerr << "Warning! Invalid GJK solver" << std::endl;
+ return -1; // error
+ }
}
-std::size_t collide(const CollisionGeometry* o1, const MotionBase* motion1,
- const CollisionGeometry* o2, const MotionBase* motion2,
- const CollisionRequest& request,
- CollisionResult& result)
-{
- FCL_REAL toc;
- return conservativeAdvancement<RSS, MeshConservativeAdvancementTraversalNodeRSS, MeshCollisionTraversalNodeRSS>(
- o1, motion1,
- o2, motion2,
- request,
- result,
- toc);
}
-
-
-}
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 62f1f45d..0b3c2db9 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -203,9 +203,22 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
+ if(request.enable_cached_gjk_guess)
+ {
+ nsolver->enableCachedGuess(true);
+ nsolver->setCachedGuess(request.cached_gjk_guess);
+ }
+ else
+ {
+ nsolver->enableCachedGuess(true);
+ }
+
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
collide(&node);
+ if(request.enable_cached_gjk_guess)
+ result.cached_gjk_guess = nsolver->getCachedGuess();
+
return result.numContacts();
}
diff --git a/src/continuous_collision.cpp b/src/continuous_collision.cpp
new file mode 100644
index 00000000..260145fa
--- /dev/null
+++ b/src/continuous_collision.cpp
@@ -0,0 +1,307 @@
+#include "fcl/collision.h"
+#include "fcl/continuous_collision.h"
+#include "fcl/ccd/motion.h"
+#include "fcl/BVH/BVH_model.h"
+#include "fcl/traversal/traversal_node_bvhs.h"
+#include "fcl/traversal/traversal_node_setup.h"
+#include "fcl/collision_node.h"
+#include "fcl/ccd/conservative_advancement.h"
+#include <iostream>
+
+namespace fcl
+{
+
+template<typename GJKSolver>
+ConservativeAdvancementFunctionMatrix<GJKSolver>& getConservativeAdvancementFunctionLookTable()
+{
+ static ConservativeAdvancementFunctionMatrix<GJKSolver> table;
+ return table;
+}
+
+MotionBase* getMotionBase(const Transform3f& tf_beg, const Transform3f& tf_end, CCDMotionType motion_type)
+{
+ switch(motion_type)
+ {
+ case CCDM_TRANS:
+ return new TranslationMotion(tf_beg, tf_end);
+ break;
+ case CCDM_LINEAR:
+ return new InterpMotion(tf_beg, tf_end);
+ break;
+ case CCDM_SCREW:
+ return new ScrewMotion(tf_beg, tf_end);
+ break;
+ case CCDM_SPLINE:
+ return new SplineMotion(tf_beg, tf_end);
+ break;
+ default:
+ return NULL;
+ }
+}
+
+
+FCL_REAL continuousCollideNaive(const CollisionGeometry* o1, const MotionBase* motion1,
+ const CollisionGeometry* o2, const MotionBase* motion2,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ std::size_t n_iter = std::min(request.num_max_iterations, (std::size_t)ceil(1 / request.toc_err));
+ Transform3f cur_tf1, cur_tf2;
+ for(std::size_t i = 0; i < n_iter; ++i)
+ {
+ FCL_REAL t = i / (FCL_REAL) (n_iter - 1);
+ motion1->getCurrentTransform(cur_tf1);
+ motion2->getCurrentTransform(cur_tf2);
+
+ CollisionRequest c_request;
+ CollisionResult c_result;
+
+ if(collide(o1, cur_tf1, o2, cur_tf2, c_request, c_result))
+ {
+ result.is_collide = true;
+ result.time_of_contact = t;
+ return t;
+ }
+ }
+
+ return 1.0;
+}
+
+namespace details
+{
+template<typename BV>
+FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1_, const TranslationMotion* motion1,
+ const CollisionGeometry* o2_, const TranslationMotion* motion2,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ const BVHModel<BV>* o1__ = static_cast<const BVHModel<BV>*>(o1_);
+ const BVHModel<BV>* o2__ = static_cast<const BVHModel<BV>*>(o2_);
+
+ // ugly, but lets do it now.
+ BVHModel<BV>* o1 = const_cast<BVHModel<BV>*>(o1__);
+ BVHModel<BV>* o2 = const_cast<BVHModel<BV>*>(o2__);
+ std::vector<Vec3f> new_v1(o1->num_vertices);
+ std::vector<Vec3f> new_v2(o2->num_vertices);
+
+ for(std::size_t i = 0; i < new_v1.size(); ++i)
+ new_v1[i] = o1->vertices[i] + motion1->getVelocity();
+ for(std::size_t i = 0; i < new_v2.size(); ++i)
+ new_v2[i] = o2->vertices[i] + motion2->getVelocity();
+
+ o1->beginUpdateModel();
+ o1->updateSubModel(new_v1);
+ o1->endUpdateModel(true, true);
+
+ o2->beginUpdateModel();
+ o2->updateSubModel(new_v2);
+ o2->endUpdateModel(true, true);
+
+ MeshContinuousCollisionTraversalNode<BV> node;
+ CollisionRequest c_request;
+
+ motion1->integrate(0);
+ motion2->integrate(0);
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+ if(!initialize<BV>(node, *o1, tf1, *o2, tf2, c_request))
+ return -1.0;
+
+ collide(&node);
+
+ result.is_collide = (node.pairs.size() > 0);
+ result.time_of_contact = node.time_of_contact;
+
+ return result.time_of_contact;
+}
+
+}
+
+FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1, const TranslationMotion* motion1,
+ const CollisionGeometry* o2, const TranslationMotion* motion2,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ switch(o1->getNodeType())
+ {
+ case BV_AABB:
+ if(o2->getNodeType() == BV_AABB)
+ return details::continuousCollideBVHPolynomial<AABB>(o1, motion1, o2, motion2, request, result);
+ break;
+ case BV_OBB:
+ if(o2->getNodeType() == BV_OBB)
+ return details::continuousCollideBVHPolynomial<OBB>(o1, motion1, o2, motion2, request, result);
+ break;
+ case BV_RSS:
+ if(o2->getNodeType() == BV_RSS)
+ return details::continuousCollideBVHPolynomial<RSS>(o1, motion1, o2, motion2, request, result);
+ break;
+ case BV_kIOS:
+ if(o2->getNodeType() == BV_kIOS)
+ return details::continuousCollideBVHPolynomial<kIOS>(o1, motion1, o2, motion2, request, result);
+ break;
+ case BV_OBBRSS:
+ if(o2->getNodeType() == BV_OBBRSS)
+ return details::continuousCollideBVHPolynomial<OBBRSS>(o1, motion1, o2, motion2, request, result);
+ break;
+ case BV_KDOP16:
+ if(o2->getNodeType() == BV_KDOP16)
+ return details::continuousCollideBVHPolynomial<KDOP<16> >(o1, motion1, o2, motion2, request, result);
+ break;
+ case BV_KDOP18:
+ if(o2->getNodeType() == BV_KDOP18)
+ return details::continuousCollideBVHPolynomial<KDOP<18> >(o1, motion1, o2, motion2, request, result);
+ break;
+ case BV_KDOP24:
+ if(o2->getNodeType() == BV_KDOP24)
+ return details::continuousCollideBVHPolynomial<KDOP<24> >(o1, motion1, o2, motion2, request, result);
+ break;
+ default:
+ ;
+ }
+
+ std::cerr << "Warning: BV type not supported by polynomial solver CCD" << std::endl;
+
+ return -1;
+}
+
+namespace details
+{
+
+template<typename NarrowPhaseSolver>
+FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1,
+ const CollisionGeometry* o2, const MotionBase* motion2,
+ const NarrowPhaseSolver* nsolver_,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ const NarrowPhaseSolver* nsolver = nsolver_;
+ if(!nsolver_)
+ nsolver = new NarrowPhaseSolver();
+
+ const ConservativeAdvancementFunctionMatrix<NarrowPhaseSolver>& looktable = getConservativeAdvancementFunctionLookTable<NarrowPhaseSolver>();
+
+ NODE_TYPE node_type1 = o1->getNodeType();
+ NODE_TYPE node_type2 = o2->getNodeType();
+
+ FCL_REAL res = -1;
+
+ if(!looktable.conservative_advancement_matrix[node_type1][node_type2])
+ {
+ std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+ }
+ else
+ {
+ res = looktable.conservative_advancement_matrix[node_type1][node_type2](o1, motion1, o2, motion2, nsolver, request, result);
+ }
+
+ if(!nsolver_)
+ delete nsolver;
+
+ return res;
+}
+
+}
+
+
+FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1,
+ const CollisionGeometry* o2, const MotionBase* motion2,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ switch(request.gjk_solver_type)
+ {
+ case GST_LIBCCD:
+ {
+ GJKSolver_libccd solver;
+ return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result);
+ }
+ case GST_INDEP:
+ {
+ GJKSolver_indep solver;
+ return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result);
+ }
+ default:
+ return -1;
+ }
+}
+
+
+FCL_REAL continuousCollide(const CollisionGeometry* o1, const MotionBase* motion1,
+ const CollisionGeometry* o2, const MotionBase* motion2,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ switch(request.ccd_solver_type)
+ {
+ case CCDC_NAIVE:
+ return continuousCollideNaive(o1, motion1,
+ o2, motion2,
+ request,
+ result);
+ break;
+ case CCDC_CONSERVATIVE_ADVANCEMENT:
+ return continuousCollideConservativeAdvancement(o1, motion1,
+ o2, motion2,
+ request,
+ result);
+ break;
+ case CCDC_RAY_SHOOTING:
+ if(o1->getObjectType() == OT_GEOM && o2->getObjectType() == OT_GEOM && request.ccd_motion_type == CCDM_TRANS)
+ {
+
+ }
+ else
+ std::cerr << "Warning! Invalid continuous collision setting" << std::endl;
+ break;
+ case CCDC_POLYNOMIAL_SOLVER:
+ if(o1->getObjectType() == OT_BVH && o2->getObjectType() == OT_BVH && request.ccd_motion_type == CCDM_TRANS)
+ {
+ return continuousCollideBVHPolynomial(o1, (const TranslationMotion*)motion1,
+ o2, (const TranslationMotion*)motion2,
+ request, result);
+ }
+ else
+ std::cerr << "Warning! Invalid continuous collision checking" << std::endl;
+ break;
+ default:
+ std::cerr << "Warning! Invalid continuous collision setting" << std::endl;
+ }
+
+ return -1;
+}
+
+FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
+ const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ MotionBase* motion1 = getMotionBase(tf1_beg, tf1_end, request.ccd_motion_type);
+ MotionBase* motion2 = getMotionBase(tf2_beg, tf2_end, request.ccd_motion_type);
+
+ return continuousCollide(o1, motion1, o2, motion2, request, result);
+}
+
+
+FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end,
+ const CollisionObject* o2, const Transform3f& tf2_end,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ return continuousCollide(o1->getCollisionGeometry(), o1->getTransform(), tf1_end,
+ o2->getCollisionGeometry(), o2->getTransform(), tf2_end,
+ request, result);
+}
+
+
+FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
+ const ContinuousCollisionRequest& request,
+ ContinuousCollisionResult& result)
+{
+ return continuousCollide(o1->getCollisionGeometry(), o1->getMotion(),
+ o2->getCollisionGeometry(), o2->getMotion(),
+ request, result);
+}
+
+}
diff --git a/src/distance.cpp b/src/distance.cpp
index fde2650f..d20da2f5 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -106,25 +106,45 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
return res;
}
-template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
-template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
-template FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_libccd* nsolver, const DistanceRequest& request, DistanceResult& result);
-template FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_indep* nsolver, const DistanceRequest& request, DistanceResult& result);
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result)
{
- GJKSolver_libccd solver;
- return distance<GJKSolver_libccd>(o1, o2, &solver, request, result);
+ switch(request.gjk_solver_type)
+ {
+ case GST_LIBCCD:
+ {
+ GJKSolver_libccd solver;
+ return distance<GJKSolver_libccd>(o1, o2, &solver, request, result);
+ }
+ case GST_INDEP:
+ {
+ GJKSolver_indep solver;
+ return distance<GJKSolver_indep>(o1, o2, &solver, request, result);
+ }
+ default:
+ return -1; // error
+ }
}
FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
- GJKSolver_libccd solver;
- return distance<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
- // GJKSolver_indep solver;
- // return distance<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, request, result);
+ switch(request.gjk_solver_type)
+ {
+ case GST_LIBCCD:
+ {
+ GJKSolver_libccd solver;
+ return distance<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
+ }
+ case GST_INDEP:
+ {
+ GJKSolver_indep solver;
+ return distance<GJKSolver_indep>(o1, tf1, o2, tf2, &solver, request, result);
+ }
+ default:
+ return -1;
+ }
}
diff --git a/src/intersect.cpp b/src/intersect.cpp
index efce14ba..791d0209 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -1533,6 +1533,20 @@ FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
return triDistance(S, T_transformed, P, Q);
}
+
+FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
+ const Transform3f& tf,
+ Vec3f& P, Vec3f& Q)
+{
+ Vec3f T_transformed[3];
+ T_transformed[0] = tf.transform(T[0]);
+ T_transformed[1] = tf.transform(T[1]);
+ T_transformed[2] = tf.transform(T[2]);
+
+ return triDistance(S, T_transformed, P, Q);
+}
+
+
FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
const Matrix3f& R, const Vec3f& Tl,
@@ -1544,4 +1558,264 @@ FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const V
return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
}
+FCL_REAL TriangleDistance::triDistance(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);
+ return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
+}
+
+
+
+
+
+Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, const Vec3f& p)
+{
+ ProjectResult res;
+
+ const Vec3f d = b - a;
+ const FCL_REAL l = d.sqrLength();
+
+ if(l > 0)
+ {
+ const FCL_REAL t = (p - a).dot(d);
+ res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
+ res.parameterization[0] = 1 - res.parameterization[1];
+ if(t >= l) { res.sqr_distance = (p - b).sqrLength(); res.encode = 2; /* 0x10 */ }
+ else if(t <= 0) { res.sqr_distance = (p - a).sqrLength(); res.encode = 1; /* 0x01 */ }
+ else { res.sqr_distance = (a + d * res.parameterization[1] - p).sqrLength(); res.encode = 3; /* 0x00 */ }
+ }
+
+ return res;
+}
+
+Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& 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 FCL_REAL l = n.sqrLength();
+
+ if(l > 0)
+ {
+ FCL_REAL mindist = -1;
+ for(size_t i = 0; i < 3; ++i)
+ {
+ if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
+ {
+ size_t j = nexti[i];
+ ProjectResult res_line = projectLine(*vt[i], *vt[j], p);
+
+ if(mindist < 0 || res_line.sqr_distance < mindist)
+ {
+ mindist = res_line.sqr_distance;
+ res.encode = static_cast<size_t>(((res_line.encode&1)?1<<i:0) + ((res_line.encode&2)?1<<j:0));
+ res.parameterization[i] = res_line.parameterization[0];
+ res.parameterization[j] = res_line.parameterization[1];
+ res.parameterization[nexti[j]] = 0;
+ }
+ }
+ }
+
+ if(mindist < 0) // the origin project is within the triangle
+ {
+ FCL_REAL d = (a - p).dot(n);
+ FCL_REAL s = sqrt(l);
+ Vec3f p_to_project = n * (d / l);
+ mindist = p_to_project.sqrLength();
+ res.encode = 7; // m = 0x111
+ res.parameterization[0] = dl[1].cross(b - p -p_to_project).length() / s;
+ res.parameterization[1] = dl[2].cross(c - p -p_to_project).length() / s;
+ res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1];
+ }
+
+ res.sqr_distance = mindist;
+ }
+
+ return res;
+
+}
+
+Project::ProjectResult Project::projectTetrahedra(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, const Vec3f& 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};
+ FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
+ bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0;
+ if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
+ {
+ FCL_REAL mindist = -1;
+
+ for(size_t i = 0; i < 3; ++i)
+ {
+ size_t j = nexti[i];
+ FCL_REAL s = vl * (d-p).dot(dl[i].cross(dl[j]));
+ if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
+ {
+ ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p);
+ if(mindist < 0 || res_triangle.sqr_distance < mindist)
+ {
+ mindist = res_triangle.sqr_distance;
+ res.encode = static_cast<size_t>( (res_triangle.encode&1?1<<i:0) + (res_triangle.encode&2?1<<j:0) + (res_triangle.encode&4?8:0) );
+ res.parameterization[i] = res_triangle.parameterization[0];
+ res.parameterization[j] = res_triangle.parameterization[1];
+ res.parameterization[nexti[j]] = 0;
+ res.parameterization[3] = res_triangle.parameterization[2];
+ }
+ }
+ }
+
+ if(mindist < 0)
+ {
+ mindist = 0;
+ res.encode = 15;
+ res.parameterization[0] = triple(c - p, b - p, d - p) / vl;
+ res.parameterization[1] = triple(a - p, c - p, d - p) / vl;
+ res.parameterization[2] = triple(b - p, a - p, d - p) / vl;
+ res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]);
+ }
+
+ res.sqr_distance = mindist;
+ }
+ else if(!ng)
+ {
+ res = projectTriangle(a, b, c, p);
+ res.parameterization[3] = 0;
+ }
+ return res;
+}
+
+Project::ProjectResult Project::projectLineOrigin(const Vec3f& a, const Vec3f& b)
+{
+ ProjectResult res;
+
+ const Vec3f d = b - a;
+ const FCL_REAL l = d.sqrLength();
+
+ if(l > 0)
+ {
+ const FCL_REAL t = - a.dot(d);
+ res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l));
+ res.parameterization[0] = 1 - res.parameterization[1];
+ if(t >= l) { res.sqr_distance = b.sqrLength(); res.encode = 2; /* 0x10 */ }
+ else if(t <= 0) { res.sqr_distance = a.sqrLength(); res.encode = 1; /* 0x01 */ }
+ else { res.sqr_distance = (a + d * res.parameterization[1]).sqrLength(); res.encode = 3; /* 0x00 */ }
+ }
+
+ return res;
+}
+
+Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& 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 FCL_REAL l = n.sqrLength();
+
+ if(l > 0)
+ {
+ FCL_REAL mindist = -1;
+ for(size_t i = 0; i < 3; ++i)
+ {
+ if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
+ {
+ size_t j = nexti[i];
+ ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]);
+
+ if(mindist < 0 || res_line.sqr_distance < mindist)
+ {
+ mindist = res_line.sqr_distance;
+ res.encode = static_cast<size_t>(((res_line.encode&1)?1<<i:0) + ((res_line.encode&2)?1<<j:0));
+ res.parameterization[i] = res_line.parameterization[0];
+ res.parameterization[j] = res_line.parameterization[1];
+ res.parameterization[nexti[j]] = 0;
+ }
+ }
+ }
+
+ if(mindist < 0) // the origin project is within the triangle
+ {
+ FCL_REAL d = a.dot(n);
+ FCL_REAL s = sqrt(l);
+ Vec3f o_to_project = n * (d / l);
+ mindist = o_to_project.sqrLength();
+ res.encode = 7; // m = 0x111
+ res.parameterization[0] = dl[1].cross(b - o_to_project).length() / s;
+ res.parameterization[1] = dl[2].cross(c - o_to_project).length() / s;
+ res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1];
+ }
+
+ res.sqr_distance = mindist;
+ }
+
+ return res;
+
+}
+
+Project::ProjectResult Project::projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& 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};
+ FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
+ bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0;
+ if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
+ {
+ FCL_REAL mindist = -1;
+
+ for(size_t i = 0; i < 3; ++i)
+ {
+ size_t j = nexti[i];
+ FCL_REAL s = vl * d.dot(dl[i].cross(dl[j]));
+ if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
+ {
+ ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d);
+ if(mindist < 0 || res_triangle.sqr_distance < mindist)
+ {
+ mindist = res_triangle.sqr_distance;
+ res.encode = static_cast<size_t>( (res_triangle.encode&1?1<<i:0) + (res_triangle.encode&2?1<<j:0) + (res_triangle.encode&4?8:0) );
+ res.parameterization[i] = res_triangle.parameterization[0];
+ res.parameterization[j] = res_triangle.parameterization[1];
+ res.parameterization[nexti[j]] = 0;
+ res.parameterization[3] = res_triangle.parameterization[2];
+ }
+ }
+ }
+
+ if(mindist < 0)
+ {
+ mindist = 0;
+ res.encode = 15;
+ res.parameterization[0] = triple(c, b, d) / vl;
+ res.parameterization[1] = triple(a, c, d) / vl;
+ res.parameterization[2] = triple(b, a, d) / vl;
+ res.parameterization[3] = 1 - (res.parameterization[0] + res.parameterization[1] + res.parameterization[2]);
+ }
+
+ res.sqr_distance = mindist;
+ }
+ else if(!ng)
+ {
+ res = projectTriangleOrigin(a, b, c);
+ res.parameterization[3] = 0;
+ }
+ return res;
+}
+
+
}
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 1df32317..8be837f1 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -35,6 +35,7 @@
/** \author Jia Pan */
#include "fcl/narrowphase/gjk.h"
+#include "fcl/intersect.h"
namespace fcl
{
@@ -48,7 +49,7 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
{
case GEOM_TRIANGLE:
{
- const Triangle2* triangle = static_cast<const Triangle2*>(shape);
+ const TriangleP* triangle = static_cast<const TriangleP*>(shape);
FCL_REAL dota = dir.dot(triangle->a);
FCL_REAL dotb = dir.dot(triangle->b);
FCL_REAL dotc = dir.dot(triangle->c);
@@ -154,10 +155,7 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
}
break;
case GEOM_PLANE:
- {
- return Vec3f(0, 0, 0);
- }
- break;
+ break;
default:
; // nothing
}
@@ -165,116 +163,6 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
return Vec3f(0, 0, 0);
}
-
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m)
-{
- const Vec3f d = b - a;
- const FCL_REAL l = d.sqrLength();
- if(l > 0)
- {
- const FCL_REAL t(l > 0 ? - a.dot(d) / l : 0);
- if(t >= 1) { w[0] = 0; w[1] = 1; m = 2; return b.sqrLength(); } // m = 0x10
- else if(t <= 0) { w[0] = 1; w[1] = 0; m = 1; return a.sqrLength(); } // m = 0x01
- else { w[0] = 1 - (w[1] = t); m = 3; return (a + d * t).sqrLength(); } // m = 0x11
- }
-
- return -1;
-}
-
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m)
-{
- 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 FCL_REAL l = n.sqrLength();
-
- if(l > 0)
- {
- FCL_REAL mindist = -1;
- FCL_REAL subw[2] = {0, 0};
- size_t subm = 0;
- for(size_t i = 0; i < 3; ++i)
- {
- if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
- {
- size_t j = nexti[i];
- FCL_REAL subd = projectOrigin(*vt[i], *vt[j], subw, subm);
- if(mindist < 0 || subd < mindist)
- {
- mindist = subd;
- m = static_cast<size_t>(((subm&1)?1<<i:0) + ((subm&2)?1<<j:0));
- w[i] = subw[0];
- w[j] = subw[1];
- w[nexti[j]] = 0;
- }
- }
- }
-
- if(mindist < 0) // the origin project is within the triangle
- {
- FCL_REAL d = a.dot(n);
- FCL_REAL s = sqrt(l);
- Vec3f p = n * (d / l);
- mindist = p.sqrLength();
- m = 7; // m = 0x111
- w[0] = dl[1].cross(b-p).length() / s;
- w[1] = dl[2].cross(c-p).length() / s;
- w[2] = 1 - (w[0] + w[1]);
- }
-
- return mindist;
- }
- return -1;
-}
-
-FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m)
-{
- static const size_t nexti[] = {1, 2, 0};
- const Vec3f* vt[] = {&a, &b, &c, &d};
- const Vec3f dl[3] = {a-d, b-d, c-d};
- FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
- bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0;
- if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
- {
- FCL_REAL mindist = -1;
- FCL_REAL subw[3] = {0, 0, 0};
- size_t subm = 0;
- for(size_t i = 0; i < 3; ++i)
- {
- size_t j = nexti[i];
- FCL_REAL s = vl * d.dot(dl[i].cross(dl[j]));
- if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
- {
- FCL_REAL subd = projectOrigin(*vt[i], *vt[j], d, subw, subm);
- if(mindist < 0 || subd < mindist)
- {
- mindist = subd;
- m = static_cast<size_t>( (subm&1?1<<i:0) + (subm&2?1<<j:0) + (subm&4?8:0) );
- w[i] = subw[0];
- w[j] = subw[1];
- w[nexti[j]] = 0;
- w[3] = subw[2];
- }
- }
- }
-
- if(mindist < 0)
- {
- mindist = 0;
- m = 15;
- w[0] = triple(c, b, d) / vl;
- w[1] = triple(a, c, d) / vl;
- w[2] = triple(b, a, d) / vl;
- w[3] = 1 - (w[0] + w[1] + w[2]);
- }
-
- return mindist;
- }
- return -1;
-}
-
-
void GJK::initialize()
{
ray = Vec3f();
@@ -282,12 +170,19 @@ void GJK::initialize()
status = Failed;
current = 0;
distance = 0.0;
+ simplex = NULL;
}
+
+Vec3f GJK::getGuessFromSimplex() const
+{
+ return ray;
+}
+
+
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
{
size_t iterations = 0;
- FCL_REAL sqdist = 0;
FCL_REAL alpha = 0;
Vec3f lastw[4];
size_t clastw = 0;
@@ -304,29 +199,29 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
distance = 0.0;
simplices[0].rank = 0;
ray = guess;
-
- FCL_REAL sqrl = ray.sqrLength();
- appendVertex(simplices[0], (sqrl>0) ? -ray : Vec3f(1, 0, 0));
+
+ appendVertex(simplices[0], (ray.sqrLength() > 0) ? -ray : Vec3f(1, 0, 0));
simplices[0].p[0] = 1;
ray = simplices[0].c[0]->w;
- sqdist = sqrl;
- lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray;
+ lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points
do
{
size_t next = 1 - current;
Simplex& curr_simplex = simplices[current];
Simplex& next_simplex = simplices[next];
-
- FCL_REAL rl = ray.sqrLength();
- if(rl < tolerance) // means origin is near the face of original simplex, return touch
+
+ // check A: when origin is near the existing simplex, stop
+ FCL_REAL rl = ray.length();
+ if(rl < tolerance) // mean origin is near the face of original simplex, return touch
{
status = Inside;
break;
}
appendVertex(curr_simplex, -ray); // see below, ray points away from origin
-
+
+ // check B: when the new support point is close to previous support points, stop (as the new simplex is degenerated)
Vec3f& w = curr_simplex.c[curr_simplex.rank - 1]->w;
bool found = false;
for(size_t i = 0; i < 4; ++i)
@@ -347,7 +242,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
lastw[clastw = (clastw+1)&3] = w;
}
- // check for termination, from bullet
+ // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated)
FCL_REAL omega = ray.dot(w) / rl;
alpha = std::max(alpha, omega);
if((rl - alpha) - tolerance * rl <= 0)
@@ -356,36 +251,34 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
break;
}
- // reduce simplex and decide the extend direction
- FCL_REAL weights[4];
- size_t mask = 0; // decide the simplex vertices that compose the minimal distance
+ Project::ProjectResult project_res;
switch(curr_simplex.rank)
{
case 2:
- sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, weights, mask); break;
+ project_res = Project::projectLineOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w); break;
case 3:
- sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, weights, mask); break;
+ project_res = Project::projectTriangleOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w); break;
case 4:
- sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w, weights, mask); break;
+ project_res = Project::projectTetrahedraOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w); break;
}
- if(sqdist >= 0)
+ if(project_res.sqr_distance >= 0)
{
next_simplex.rank = 0;
ray = Vec3f();
current = next;
for(size_t i = 0; i < curr_simplex.rank; ++i)
{
- if(mask & (1 << i))
+ if(project_res.encode & (1 << i))
{
next_simplex.c[next_simplex.rank] = curr_simplex.c[i];
- next_simplex.p[next_simplex.rank++] = weights[i];
- ray += curr_simplex.c[i]->w * weights[i];
+ next_simplex.p[next_simplex.rank++] = project_res.parameterization[i]; // weights[i];
+ ray += curr_simplex.c[i]->w * project_res.parameterization[i]; // weights[i];
}
else
free_v[nfree++] = curr_simplex.c[i];
}
- if(mask == 15) status = Inside; // the origin is within the 4-simplex, collision
+ if(project_res.encode == 15) status = Inside; // the origin is within the 4-simplex, collision
}
else
{
@@ -413,6 +306,12 @@ void GJK::getSupport(const Vec3f& d, SimplexV& sv) const
sv.w = shape.support(sv.d);
}
+void GJK::getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const
+{
+ sv.d = normalize(d);
+ sv.w = shape.support(sv.d, v);
+}
+
void GJK::removeVertex(Simplex& simplex)
{
free_v[nfree++] = simplex.c[--simplex.rank];
diff --git a/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp
index 2b18391c..d46689aa 100644
--- a/src/narrowphase/gjk_libccd.cpp
+++ b/src/narrowphase/gjk_libccd.cpp
@@ -38,6 +38,7 @@
#include "fcl/narrowphase/gjk_libccd.h"
#include <ccd/simplex.h>
#include <ccd/vec3.h>
+#include <ccd/ccd.h>
namespace fcl
{
@@ -87,16 +88,53 @@ struct ccd_triangle_t : public ccd_obj_t
ccd_vec3_t c;
};
-/*
-static void tripleCross(const ccd_vec3_t* a, const ccd_vec3_t* b,
- const ccd_vec3_t* c, ccd_vec3_t* d)
+namespace libccd_extension
+{
+
+
+static ccd_real_t simplexReduceToTriangle(ccd_simplex_t *simplex,
+ ccd_real_t dist,
+ ccd_vec3_t *best_witness)
+{
+ ccd_real_t newdist;
+ ccd_vec3_t witness;
+ int best = -1;
+ int i;
+
+ // try the fourth point in all three positions
+ for (i = 0; i < 3; i++){
+ newdist = ccdVec3PointTriDist2(ccd_vec3_origin,
+ &ccdSimplexPoint(simplex, (i == 0 ? 3 : 0))->v,
+ &ccdSimplexPoint(simplex, (i == 1 ? 3 : 1))->v,
+ &ccdSimplexPoint(simplex, (i == 2 ? 3 : 2))->v,
+ &witness);
+ newdist = CCD_SQRT(newdist);
+
+ // record the best triangle
+ if (newdist < dist){
+ dist = newdist;
+ best = i;
+ ccdVec3Copy(best_witness, &witness);
+ }
+ }
+
+ if (best >= 0){
+ ccdSimplexSet(simplex, best, ccdSimplexPoint(simplex, 3));
+ }
+ ccdSimplexSetSize(simplex, 3);
+
+ return dist;
+}
+
+_ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
+ const ccd_vec3_t *c, ccd_vec3_t *d)
{
ccd_vec3_t e;
ccdVec3Cross(&e, a, b);
ccdVec3Cross(d, &e, c);
}
-static int doSimplex2Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B;
ccd_vec3_t AB, AO, tmp;
@@ -112,28 +150,22 @@ static int doSimplex2Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
ccdVec3Copy(&AO, &A->v);
ccdVec3Scale(&AO, -CCD_ONE);
- // dot product AB . -AO
+ // dot product AB . AO
dot = ccdVec3Dot(&AB, &AO);
// check if origin doesn't lie on AB segment
ccdVec3Cross(&tmp, &AB, &AO);
- if(ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO)
- {
+ if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
return 1;
}
- *dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, &A->v, &B->v, NULL);
-
// check if origin is in area where AB segment is
- if(ccdIsZero(dot) || dot < CCD_ZERO)
- {
+ if (ccdIsZero(dot) || dot < CCD_ZERO){
// origin is in outside are of A
ccdSimplexSet(simplex, 0, A);
ccdSimplexSetSize(simplex, 1);
ccdVec3Copy(dir, &AO);
- }
- else
- {
+ }else{
// origin is in area where AB segment is
// keep simplex untouched and set direction to
@@ -144,11 +176,11 @@ static int doSimplex2Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
return 0;
}
-static int doSimplex3Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B, *C;
ccd_vec3_t AO, AB, AC, ABC, tmp;
- ccd_real_t dot;
+ ccd_real_t dot, dist;
// get last added as A
A = ccdSimplexLast(simplex);
@@ -157,16 +189,14 @@ static int doSimplex3Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
C = ccdSimplexPoint(simplex, 0);
// check touching contact
- *dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
- if(ccdIsZero(*dist))
- {
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
+ if (ccdIsZero(dist)){
return 1;
}
// check if triangle is really triangle (has area > 0)
// if not simplex can't be expanded and thus no itersection is found
- if(ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v))
- {
+ if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){
return -1;
}
@@ -181,52 +211,37 @@ static int doSimplex3Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
ccdVec3Cross(&tmp, &ABC, &AC);
dot = ccdVec3Dot(&tmp, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
dot = ccdVec3Dot(&AC, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
// C is already in place
ccdSimplexSet(simplex, 1, A);
ccdSimplexSetSize(simplex, 2);
tripleCross(&AC, &AO, &AC, dir);
- }
- else
- {
+ }else{
ccd_do_simplex3_45:
dot = ccdVec3Dot(&AB, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
ccdSimplexSet(simplex, 0, B);
ccdSimplexSet(simplex, 1, A);
ccdSimplexSetSize(simplex, 2);
tripleCross(&AB, &AO, &AB, dir);
- }
- else
- {
+ }else{
ccdSimplexSet(simplex, 0, A);
ccdSimplexSetSize(simplex, 1);
ccdVec3Copy(dir, &AO);
}
}
- }
- else
- {
+ }else{
ccdVec3Cross(&tmp, &AB, &ABC);
dot = ccdVec3Dot(&tmp, &AO);
- if(ccdIsZero(dot) || dot > CCD_ZERO)
- {
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
goto ccd_do_simplex3_45;
- }
- else
- {
+ }else{
dot = ccdVec3Dot(&ABC, &AO);
- if (ccdIsZero(dot) || dot > CCD_ZERO)
- {
+ if (ccdIsZero(dot) || dot > CCD_ZERO){
ccdVec3Copy(dir, &ABC);
- }
- else
- {
+ }else{
ccd_support_t Ctmp;
ccdSupportCopy(&Ctmp, C);
ccdSimplexSet(simplex, 0, B);
@@ -238,16 +253,16 @@ static int doSimplex3Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
}
}
-
return 0;
}
-static int doSimplex4Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B, *C, *D;
ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
int B_on_ACD, C_on_ADB, D_on_ABC;
int AB_O, AC_O, AD_O;
+ ccd_real_t dist;
// get last added as A
A = ccdSimplexLast(simplex);
@@ -259,25 +274,24 @@ static int doSimplex4Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
// check if tetrahedron is really tetrahedron (has volume > 0)
// if it is not simplex can't be expanded and thus no intersection is
// found
- ccd_real_t volume = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
- if(ccdIsZero(volume))
- {
+ dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
+ if (ccdIsZero(dist)){
return -1;
}
// check if origin lies on some of tetrahedron's face - if so objects
// intersect
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
- if(ccdIsZero(volume))
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
+ if (ccdIsZero(dist))
return 1;
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
- if(ccdIsZero(volume))
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
+ if (ccdIsZero(dist))
return 1;
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
- if(ccdIsZero(volume))
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
+ if (ccdIsZero(dist))
return 1;
- volume = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
- if(ccdIsZero(volume))
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
+ if (ccdIsZero(dist))
return 1;
// compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
@@ -302,15 +316,12 @@ static int doSimplex4Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
- if(AB_O && AC_O && AD_O)
- {
+ if (AB_O && AC_O && AD_O){
// origin is in tetrahedron
return 1;
- // rearrange simplex to triangle and call doSimplex3Dist()
- }
- else if(!AB_O)
- {
+ // rearrange simplex to triangle and call doSimplex3()
+ }else if (!AB_O){
// B is farthest from the origin among all of the tetrahedron's
// points, so remove it from the list and go on with the triangle
// case
@@ -318,60 +329,44 @@ static int doSimplex4Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* d
// D and C are in place
ccdSimplexSet(simplex, 2, A);
ccdSimplexSetSize(simplex, 3);
- }
- else if(!AC_O)
- {
+ }else if (!AC_O){
// C is farthest
ccdSimplexSet(simplex, 1, D);
ccdSimplexSet(simplex, 0, B);
ccdSimplexSet(simplex, 2, A);
ccdSimplexSetSize(simplex, 3);
- }
- else
- {
- // (!AD_O)
+ }else{ // (!AD_O)
ccdSimplexSet(simplex, 0, C);
ccdSimplexSet(simplex, 1, B);
ccdSimplexSet(simplex, 2, A);
ccdSimplexSetSize(simplex, 3);
}
- return doSimplex3Dist(simplex, dir, dist);
+ return doSimplex3(simplex, dir);
}
-
-static int doSimplexDist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
- if(ccdSimplexSize(simplex) == 2)
- {
+ if (ccdSimplexSize(simplex) == 2){
// simplex contains segment only one segment
- return doSimplex2Dist(simplex, dir, dist);
- }
- else if(ccdSimplexSize(simplex) == 3)
- {
+ return doSimplex2(simplex, dir);
+ }else if (ccdSimplexSize(simplex) == 3){
// simplex contains triangle
- return doSimplex3Dist(simplex, dir, dist);
- }
- else
- {
- // ccdSimplexSize(simplex) == 4
+ return doSimplex3(simplex, dir);
+ }else{ // ccdSimplexSize(simplex) == 4
// tetrahedron - this is the only shape which can encapsule origin
// so doSimplex4() also contains test on it
- return doSimplex4Dist(simplex, dir, dist);
+ return doSimplex4(simplex, dir);
}
}
-
-
-static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
- const ccd_t *ccd, ccd_simplex_t *simplex,
- ccd_real_t tolerance)
+static int __ccdGJK(const void *obj1, const void *obj2,
+ const ccd_t *ccd, ccd_simplex_t *simplex)
{
unsigned long iterations;
ccd_vec3_t dir; // direction vector
ccd_support_t last; // last support point
int do_simplex_res;
- ccd_real_t min_dist = -1;
// initialize simplex struct
ccdSimplexInit(simplex);
@@ -387,23 +382,16 @@ static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
ccdVec3Copy(&dir, &last.v);
ccdVec3Scale(&dir, -CCD_ONE);
- bool collision_free = false;
-
// start iterations
- for(iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
- {
+ for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
// obtain support point
__ccdSupport(obj1, obj2, &dir, ccd, &last);
// check if farthest point in Minkowski difference in direction dir
// isn't somewhere before origin (the test on negative dot product)
// - because if it is, objects are not intersecting at all.
- if(ccdVec3Dot(&last.v, &dir) < CCD_ZERO)
- {
- collision_free = true;
- ccd_real_t dist = ccdVec3Len2(&last.v);
- if(min_dist < 0) min_dist = dist;
- else min_dist = std::min(min_dist, dist);
+ if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){
+ return -1; // intersection not found
}
// add last support vector to simplex
@@ -411,34 +399,108 @@ static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
// if doSimplex returns 1 if objects intersect, -1 if objects don't
// intersect and 0 if algorithm should continue
- ccd_real_t dist;
- do_simplex_res = doSimplexDist(simplex, &dir, &dist);
+ do_simplex_res = doSimplex(simplex, &dir);
+ if (do_simplex_res == 1){
+ return 0; // intersection found
+ }else if (do_simplex_res == -1){
+ return -1; // intersection not found
+ }
- if((do_simplex_res == 1) && (!collision_free))
- {
- return -1; // intersection found
+ if (ccdIsZero(ccdVec3Len2(&dir))){
+ return -1; // intersection not found
+ }
+ }
+
+ // intersection wasn't found
+ return -1;
+}
+
+/// change the libccd distance to add two closest points
+ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t* p1, ccd_vec3_t* p2)
+{
+ unsigned long iterations;
+ ccd_simplex_t simplex;
+ ccd_support_t last; // last support point
+ ccd_vec3_t dir; // direction vector
+ ccd_real_t dist, last_dist;
+
+ // first find an intersection
+ if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0)
+ return -CCD_ONE;
+
+ last_dist = CCD_REAL_MAX;
+
+ for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
+ // get a next direction vector
+ // we are trying to find out a point on the minkowski difference
+ // that is nearest to the origin, so we obtain a point on the
+ // simplex that is nearest and try to exapand the simplex towards
+ // the origin
+ if (ccdSimplexSize(&simplex) == 1){
+ ccdVec3Copy(&dir, &ccdSimplexPoint(&simplex, 0)->v);
+ dist = ccdVec3Len2(&ccdSimplexPoint(&simplex, 0)->v);
+ dist = CCD_SQRT(dist);
+ }else if (ccdSimplexSize(&simplex) == 2){
+ dist = ccdVec3PointSegmentDist2(ccd_vec3_origin,
+ &ccdSimplexPoint(&simplex, 0)->v,
+ &ccdSimplexPoint(&simplex, 1)->v,
+ &dir);
+ dist = CCD_SQRT(dist);
+ }else if(ccdSimplexSize(&simplex) == 3){
+ dist = ccdVec3PointTriDist2(ccd_vec3_origin,
+ &ccdSimplexPoint(&simplex, 0)->v,
+ &ccdSimplexPoint(&simplex, 1)->v,
+ &ccdSimplexPoint(&simplex, 2)->v,
+ &dir);
+ dist = CCD_SQRT(dist);
+ }else{ // ccdSimplexSize(&simplex) == 4
+ dist = simplexReduceToTriangle(&simplex, last_dist, &dir);
}
- else if(do_simplex_res == -1) collision_free = true;
- if(ccdIsZero(ccdVec3Len2(&dir)))
- collision_free = true;
-
+ // touching contact -- do we really need this?
+ // maybe __ccdGJK() solve this alredy.
+ if (ccdIsZero(dist))
+ return -CCD_ONE;
- if(min_dist > 0)
+ // check whether we improved for at least a minimum tolerance
+ if ((last_dist - dist) < ccd->dist_tolerance)
{
- ccd_real_t old_min_dist = min_dist;
- min_dist = std::min(min_dist, dist);
+ if(p1) *p1 = last.v1;
+ if(p2) *p2 = last.v2;
+ return dist;
+ }
+
+ // point direction towards the origin
+ ccdVec3Scale(&dir, -CCD_ONE);
+ ccdVec3Normalize(&dir);
- if((fabs(min_dist - old_min_dist) < tolerance) && (iterations > 0))
- break;
+ // find out support point
+ __ccdSupport(obj1, obj2, &dir, ccd, &last);
+
+ // record last distance
+ last_dist = dist;
+
+ // check whether we improved for at least a minimum tolerance
+ // this is here probably only for a degenerate cases when we got a
+ // point that is already in the simplex
+ dist = ccdVec3Len2(&last.v);
+ dist = CCD_SQRT(dist);
+ if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
+ {
+ if(p1) *p1 = last.v1;
+ if(p2) *p2 = last.v2;
+ return last_dist;
}
- else min_dist = dist;
+
+ // add a point to simplex
+ ccdSimplexAdd(&simplex, &last);
}
- // intersection wasn't found
- return min_dist;
+ return -CCD_REAL(1.);
}
-*/
+
+}
+
/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
@@ -499,8 +561,8 @@ static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &o->rot_inv);
ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
- ccdSign(ccdVec3Y(&dir)) * o->dim[1],
- ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
+ ccdSign(ccdVec3Y(&dir)) * o->dim[1],
+ ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
ccdQuatRotVec(v, &o->rot);
ccdVec3Add(v, &o->pos);
}
@@ -549,8 +611,8 @@ static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
rad = cyl->radius / zdist;
ccdVec3Set(v, rad * ccdVec3X(&dir),
- rad * ccdVec3Y(&dir),
- ccdSign(ccdVec3Z(&dir)) * cyl->height);
+ rad * ccdVec3Y(&dir),
+ ccdSign(ccdVec3Z(&dir)) * cyl->height);
}
// transform support vertex
@@ -668,7 +730,7 @@ static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t*
static void centerShape(const void* obj, ccd_vec3_t* c)
{
const ccd_obj_t *o = static_cast<const ccd_obj_t*>(obj);
- ccdVec3Copy(c, &o->pos);
+ ccdVec3Copy(c, &o->pos);
}
static void centerConvex(const void* obj, ccd_vec3_t* c)
@@ -682,9 +744,9 @@ static void centerConvex(const void* obj, ccd_vec3_t* c)
static void centerTriangle(const void* obj, ccd_vec3_t* c)
{
const ccd_triangle_t *o = static_cast<const ccd_triangle_t*>(obj);
- ccdVec3Copy(c, &o->c);
- ccdQuatRotVec(c, &o->rot);
- ccdVec3Add(c, &o->pos);
+ ccdVec3Copy(c, &o->c);
+ ccdQuatRotVec(c, &o->rot);
+ ccdVec3Add(c, &o->pos);
}
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
@@ -712,6 +774,7 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
}
+ /// libccd returns dir and pos in world space and dir is pointing from object 2 to object 1
res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
if(res == 0)
{
@@ -725,39 +788,12 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
return false;
}
-/*
-bool GJKDistance(void* obj1, ccd_support_fn supp1,
- void* obj2, ccd_support_fn supp2,
- unsigned int max_iterations, FCL_REAL tolerance_,
- FCL_REAL* res)
-{
- ccd_t ccd;
- ccd_real_t dist;
- CCD_INIT(&ccd);
- ccd.support1 = supp1;
- ccd.support2 = supp2;
-
- ccd.max_iterations = max_iterations;
- ccd_real_t tolerance = tolerance_;
-
-
- ccd_simplex_t simplex;
- dist = __ccdGJKDist(obj1, obj2, &ccd, &simplex, tolerance);
-
- if(dist > 0)
- {
- *res = std::sqrt(dist);
- return true;
- }
- else
- return false;
-}
-*/
+/// p1 and p2 are in global coordinate, so needs transform in the narrowphase.h functions
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
unsigned int max_iterations, FCL_REAL tolerance,
- FCL_REAL* res)
+ FCL_REAL* res, Vec3f* p1, Vec3f* p2)
{
ccd_t ccd;
ccd_real_t dist;
@@ -768,12 +804,16 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1,
ccd.max_iterations = max_iterations;
ccd.dist_tolerance = tolerance;
- dist = ccdGJKDist(obj1, obj2, &ccd);
- *res = dist;
+ ccd_vec3_t p1_, p2_;
+ dist = libccd_extension::ccdGJKDist2(obj1, obj2, &ccd, &p1_, &p2_);
+ if(p1) p1->setValue(ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_));
+ if(p2) p2->setValue(ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_));
+ if(res) *res = dist;
if(dist < 0) return false;
else return true;
}
+
GJKSupportFunction GJKInitializer<Cylinder>::getSupportFunction()
{
return &supportCyl;
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index adcda2fc..2a1fc513 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -36,6 +36,7 @@
#include "fcl/narrowphase/narrowphase.h"
#include "fcl/shape/geometric_shapes_utility.h"
+#include "fcl/intersect.h"
#include <boost/math/constants/constants.hpp>
#include <vector>
@@ -50,84 +51,92 @@ namespace details
// 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;
+ Vec3f v = s2 - s1;
+ Vec3f w = p - s1;
- FCL_REAL c1 = w.dot(v);
- FCL_REAL c2 = v.dot(v);
+ FCL_REAL c1 = w.dot(v);
+ FCL_REAL c2 = v.dot(v);
- if (c1 <= 0) {
- sp = s1;
- } else if (c2 <= c1) {
- sp = s2;
- } else {
- FCL_REAL b = c1/c2;
- Vec3f Pb = s1 + v * b;
- sp = Pb;
- }
+ if (c1 <= 0) {
+ sp = s1;
+ } else if (c2 <= c1) {
+ sp = s2;
+ } else {
+ FCL_REAL b = c1/c2;
+ Vec3f Pb = s1 + v * b;
+ sp = Pb;
+ }
}
bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
{
- Transform3f tf2_inv (tf2);
- tf2_inv.inverse();
+ Transform3f tf2_inv (tf2);
+ tf2_inv.inverse();
- Vec3f pos1 (0., 0., 0.5 * s2.lz);
- Vec3f pos2 (0., 0., -0.5 * s2.lz);
- Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f()));
+ Vec3f pos1 (0., 0., 0.5 * s2.lz);
+ Vec3f pos2 (0., 0., -0.5 * s2.lz);
+ Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f()));
- Vec3f segment_point;
+ Vec3f segment_point;
- lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
- Vec3f diff = s_c - segment_point;
+ lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
+ Vec3f diff = s_c - segment_point;
- FCL_REAL distance = diff.length() - s1.radius - s2.radius;
+ FCL_REAL distance = diff.length() - s1.radius - s2.radius;
- if (distance > 0)
- return false;
+ if (distance > 0)
+ return false;
- Vec3f normal = diff.normalize() * - FCL_REAL(1);
+ Vec3f normal = diff.normalize() * - FCL_REAL(1);
- if (distance < 0 && penetration_depth)
- *penetration_depth = -distance;
+ if (distance < 0 && penetration_depth)
+ *penetration_depth = -distance;
- if (normal_)
- *normal_ = tf2.getQuatRotation().transform(normal);
+ if (normal_)
+ *normal_ = tf2.getQuatRotation().transform(normal);
- if (contact_points) {
- *contact_points = tf2.transform(segment_point + normal * distance);
- }
+ if (contact_points) {
+ *contact_points = tf2.transform(segment_point + normal * distance);
+ }
- return true;
+ return true;
}
bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist)
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
{
- Transform3f tf2_inv (tf2);
- tf2_inv.inverse();
+ Transform3f tf2_inv(tf2);
+ tf2_inv.inverse();
- Vec3f pos1 (0., 0., 0.5 * s2.lz);
- Vec3f pos2 (0., 0., -0.5 * s2.lz);
- Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f()));
+ Vec3f pos1(0., 0., 0.5 * s2.lz);
+ Vec3f pos2(0., 0., -0.5 * s2.lz);
+ Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f()));
- Vec3f segment_point;
+ Vec3f segment_point;
- lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
- Vec3f diff = s_c - segment_point;
+ lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
+ Vec3f diff = s_c - segment_point;
- FCL_REAL distance = diff.length() - s1.radius - s2.radius;
+ FCL_REAL distance = diff.length() - s1.radius - s2.radius;
- if (distance <= 0)
- return false;
+ if(distance <= 0)
+ return false;
- if (dist)
- *dist = distance;
+ if(dist) *dist = distance;
- return true;
+ if(p1 || p2) diff.normalize();
+ if(p1)
+ {
+ *p1 = s_c - diff * s1.radius;
+ *p1 = inverse(tf1).transform(tf2.transform(*p1));
+ }
+
+ if(p2) *p2 = segment_point + diff * s1.radius;
+
+ return true;
}
bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
@@ -158,17 +167,21 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
- FCL_REAL* dist)
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
{
- Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
+ Vec3f o1 = tf1.getTranslation();
+ Vec3f o2 = tf2.getTranslation();
+ Vec3f diff = o1 - o2;
FCL_REAL len = diff.length();
if(len > s1.radius + s2.radius)
{
- *dist = len - (s1.radius + s2.radius);
+ if(dist) *dist = len - (s1.radius + s2.radius);
+ if(p1) *p1 = inverse(tf1).transform(o1 - diff * (s1.radius / len));
+ if(p2) *p2 = inverse(tf2).transform(o2 + diff * (s2.radius / len));
return true;
}
- *dist = -1;
+ if(dist) *dist = -1;
return false;
}
@@ -200,7 +213,7 @@ FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, V
return diff.dot(diff);
}
-/** \brief Whether a point's projection is in a triangle */
+/// @brief Whether a point's projection is in a triangle
bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& normal, const Vec3f& p)
{
Vec3f edge1(p2 - p1);
@@ -310,6 +323,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
return false;
}
+
bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist)
@@ -541,18 +555,58 @@ bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
if(sqr_dist > radius * radius)
{
- *dist = std::sqrt(sqr_dist) - radius;
+ if(dist) *dist = std::sqrt(sqr_dist) - radius;
return true;
}
else
{
- *dist = -1;
+ if(dist) *dist = -1;
return false;
}
}
+bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
+{
+ if(p1 || p2)
+ {
+ Vec3f o = tf.getTranslation();
+ Project::ProjectResult result;
+ result = Project::projectTriangle(P1, P2, P3, o);
+ if(result.sqr_distance > sp.radius * sp.radius)
+ {
+ if(dist) *dist = std::sqrt(result.sqr_distance);
+ Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
+ Vec3f dir = o - project_p;
+ dir.normalize();
+ if(p1) { *p1 = o - dir * sp.radius; *p1 = inverse(tf).transform(*p1); }
+ if(p2) *p2 = project_p;
+ return true;
+ }
+ else
+ return false;
+ }
+ else
+ {
+ return sphereTriangleDistance(sp, tf, P1, P2, P3, dist);
+ }
+}
+
+
+bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
+{
+ bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P2), dist, p1, p2);
+ if(p2) *p2 = inverse(tf2).transform(*p2);
+
+ return res;
+}
+
+
struct ContactPoint
{
Vec3f normal;
@@ -610,36 +664,36 @@ static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
nr = 0;
for(int i = nq; i > 0; --i)
{
- // go through all points in q and all lines between adjacent points
- if(sign * pq[dir] < h[dir])
+ // go through all points in q and all lines between adjacent points
+ if(sign * pq[dir] < h[dir])
{
- // this point is inside the chopping line
- pr[0] = pq[0];
- pr[1] = pq[1];
- pr += 2;
- nr++;
- if(nr & 8)
+ // this point is inside the chopping line
+ pr[0] = pq[0];
+ pr[1] = pq[1];
+ pr += 2;
+ nr++;
+ if(nr & 8)
{
- q = r;
- goto done;
- }
- }
- FCL_REAL* nextq = (i > 1) ? pq+2 : q;
- if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
+ q = r;
+ goto done;
+ }
+ }
+ FCL_REAL* nextq = (i > 1) ? pq+2 : q;
+ if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
{
- // this line crosses the chopping line
- pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
- (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
- pr[dir] = sign*h[dir];
- pr += 2;
- nr++;
- if(nr & 8)
+ // this line crosses the chopping line
+ pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
+ (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
+ pr[dir] = sign*h[dir];
+ pr += 2;
+ nr++;
+ if(nr & 8)
{
- q = r;
- goto done;
- }
- }
- pq += 2;
+ q = r;
+ goto done;
+ }
+ }
+ pq += 2;
}
q = r;
r = (q == ret) ? buffer : ret;
@@ -718,13 +772,13 @@ static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
{
if(avail[i])
{
- diff = std::abs(A[i]-a);
- if(diff > pi) diff = 2*pi - diff;
- if(diff < maxdiff)
+ diff = std::abs(A[i]-a);
+ if(diff > pi) diff = 2*pi - diff;
+ if(diff < maxdiff)
{
- maxdiff = diff;
- *iret = i;
- }
+ maxdiff = diff;
+ *iret = i;
+ }
}
}
avail[*iret] = 0;
@@ -1268,8 +1322,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
if(dep[i] > maxdepth)
{
- maxdepth = dep[i];
- i1 = i;
+ maxdepth = dep[i];
+ i1 = i;
}
}
@@ -2380,7 +2434,7 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const T
const Capsule &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
template<>
@@ -2558,34 +2612,34 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist) const
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist);
+ return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
+ return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
+ return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
}
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
+ return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
}
@@ -2594,10 +2648,10 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Tran
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
- const Capsule &s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+ const Capsule &s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ return details::sphereCapsuleIntersect (s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
template<>
@@ -2773,34 +2827,34 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f&
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist) const
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist);
+ return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
+ return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
+ return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2);
}
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist) const
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
{
- return details::sphereTriangleDistance(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist);
+ return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2);
}
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index 3450ce57..ce537dfc 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -186,7 +186,7 @@ void Plane::computeLocalAABB()
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
-void Triangle2::computeLocalAABB()
+void TriangleP::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index bbe7423d..6c25ab5e 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -203,7 +203,7 @@ std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf)
return result;
}
-std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f& tf)
+std::vector<Vec3f> getBoundVertices(const TriangleP& triangle, const Transform3f& tf)
{
std::vector<Vec3f> result(3);
result[0] = tf.transform(triangle.a);
@@ -333,7 +333,7 @@ void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv)
}
template<>
-void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv)
+void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf, AABB& bv)
{
bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
}
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index fd91349f..7e47e121 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -465,14 +465,14 @@ bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c,
{
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
- n = data2.P2 - data2.P1;
+ n = data2.P2 - data2.P1; n.normalize();
c1 = data2.c1;
c2 = data2.c2;
stack[stack.size() - 2] = stack[stack.size() - 1];
}
else
{
- n = data.P2 - data.P1;
+ n = data.P2 - data.P1; n.normalize();
c1 = data.c1;
c2 = data.c2;
}
@@ -572,14 +572,14 @@ bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
{
const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
- n = data2.P2 - data2.P1;
+ n = data2.P2 - data2.P1; n.normalize();
c1 = data2.c1;
c2 = data2.c2;
stack[stack.size() - 2] = stack[stack.size() - 1];
}
else
{
- n = data.P2 - data.P1;
+ n = data.P2 - data.P1; n.normalize();
c1 = data.c1;
c2 = data.c2;
}
@@ -591,9 +591,9 @@ bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
getBVAxis(model1->getBV(c1).bv, 1) * n[2] +
getBVAxis(model1->getBV(c1).bv, 2) * n[2];
- Matrix3f R0;
+ Quaternion3f R0;
motion1->getCurrentRotation(R0);
- n_transformed = R0 * n_transformed;
+ n_transformed = R0.transform(n_transformed);
n_transformed.normalize();
TBVMotionBoundVisitor<BV> mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, -n_transformed);
@@ -682,10 +682,10 @@ void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2,
/// n is the local frame of object 1, pointing from object 1 to object2
Vec3f n = P2 - P1;
/// turn n into the global frame, pointing from object 1 to object 2
- Matrix3f R0;
+ Quaternion3f R0;
motion1->getCurrentRotation(R0);
- Vec3f n_transformed = R0 * n;
- n_transformed.normalize();
+ Vec3f n_transformed = R0.transform(n);
+ n_transformed.normalize(); // normalized here
TriangleMotionBoundVisitor mb_visitor1(t11, t12, t13, n_transformed), mb_visitor2(t21, t22, t23, -n_transformed);
FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1);
@@ -732,7 +732,7 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
motion1, motion2,
enable_statistics,
min_distance,
- p1, p2,
+ closest_p1, closest_p2,
last_tri_id1, last_tri_id2,
delta_t,
num_leaf_tests);
@@ -780,7 +780,7 @@ void MeshConservativeAdvancementTraversalNodeOBBRSS::leafTesting(int b1, int b2)
motion1, motion2,
enable_statistics,
min_distance,
- p1, p2,
+ closest_p1, closest_p2,
last_tri_id1, last_tri_id2,
delta_t,
num_leaf_tests);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6638b19e..1e06285c 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,6 +28,7 @@ add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
+add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index dff6ffa0..0d45432b 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -41,6 +41,7 @@
#include "fcl/narrowphase/narrowphase.h"
#include "fcl/collision.h"
#include "test_fcl_utility.h"
+#include "fcl/ccd/motion.h"
#include <iostream>
using namespace fcl;
@@ -52,6 +53,66 @@ GJKSolver_indep solver2;
#define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
+BOOST_AUTO_TEST_CASE(gjkcache)
+{
+ Cylinder s1(5, 10);
+ Cone s2(5, 10);
+
+ CollisionRequest request;
+ request.enable_cached_gjk_guess = true;
+ request.gjk_solver_type = GST_INDEP;
+
+ TranslationMotion motion(Transform3f(Vec3f(-20.0, -20.0, -20.0)), Transform3f(Vec3f(20.0, 20.0, 20.0)));
+
+ int N = 1000;
+ FCL_REAL dt = 1.0 / (N - 1);
+
+ /// test exploiting spatial coherence
+ Timer timer1;
+ timer1.start();
+ std::vector<bool> result1(N);
+ for(int i = 0; i < N; ++i)
+ {
+ motion.integrate(dt * i);
+ Transform3f tf;
+ motion.getCurrentTransform(tf);
+
+ CollisionResult result;
+
+ collide(&s1, Transform3f(), &s2, tf, request, result);
+ result1[i] = result.isCollision();
+ request.cached_gjk_guess = result.cached_gjk_guess; // use cached guess
+ }
+
+ timer1.stop();
+
+ /// test without exploiting spatial coherence
+ Timer timer2;
+ timer2.start();
+ std::vector<bool> result2(N);
+ request.enable_cached_gjk_guess = false;
+ for(int i = 0; i < N; ++i)
+ {
+ motion.integrate(dt * i);
+ Transform3f tf;
+ motion.getCurrentTransform(tf);
+
+ CollisionResult result;
+
+ collide(&s1, Transform3f(), &s2, tf, request, result);
+ result2[i] = result.isCollision();
+ }
+
+ timer2.stop();
+
+ std::cout << timer1.getElapsedTime() << " " << timer2.getElapsedTime() << std::endl;
+
+ for(std::size_t i = 0; i < result1.size(); ++i)
+ {
+ BOOST_CHECK(result1[i] == result2[i]);
+ }
+}
+
BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
{
Sphere s1(20);
@@ -68,73 +129,73 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform, request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -155,25 +216,25 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform, request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(15, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(15, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(15.01, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
Quaternion3f q;
@@ -181,13 +242,13 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(q), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(q), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(q), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(q), request, result) > 0);
BOOST_CHECK(res);
}
@@ -208,38 +269,38 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox)
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform, request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.5, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.5, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.501, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.501, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.501, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.4, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.4, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
}
@@ -260,37 +321,37 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder)
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform, request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -311,49 +372,49 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecone)
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform, request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10.001, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.001, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.001, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
BOOST_CHECK(res);
}
@@ -374,61 +435,61 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_conecylinder)
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform, request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), request, result) > 0);
BOOST_CHECK(res);
res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 10.01)), &solver1, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 10.01)), request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -1805,7 +1866,7 @@ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
bool res;
FCL_REAL dist = -1;
-
+ Vec3f closest_p1, closest_p2;
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 10) < 0.001);
BOOST_CHECK(res);
@@ -2057,12 +2118,14 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
Vec3f normal;
bool res;
+ request.gjk_solver_type = GST_INDEP; // use indep GJK solver
+
res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
BOOST_CHECK_FALSE(res);
res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
@@ -2070,7 +2133,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
@@ -2079,7 +2142,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
@@ -2088,7 +2151,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
@@ -2097,7 +2160,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
@@ -2106,7 +2169,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
@@ -2115,7 +2178,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
@@ -2124,7 +2187,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform, &solver2, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform, request, result) > 0);
BOOST_CHECK(res);
@@ -2133,7 +2196,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
@@ -2143,7 +2206,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
@@ -2152,7 +2215,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), request, result) > 0);
BOOST_CHECK(res);
res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
@@ -2160,7 +2223,7 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &contact, &penetration_depth, &normal);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &solver2, request, result) > 0);
+ res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), request, result) > 0);
BOOST_CHECK_FALSE(res);
}
diff --git a/test/test_fcl_shape_mesh_consistency.cpp b/test/test_fcl_shape_mesh_consistency.cpp
index 5119b08e..cc15cad4 100644
--- a/test/test_fcl_shape_mesh_consistency.cpp
+++ b/test/test_fcl_shape_mesh_consistency.cpp
@@ -48,9 +48,6 @@ using namespace fcl;
#define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
-GJKSolver_libccd solver1;
-GJKSolver_indep solver2;
-
FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
@@ -71,16 +68,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
pose.setTranslation(Vec3f(50, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
for(std::size_t i = 0; i < 10; ++i)
@@ -93,31 +90,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
res.clear(); res1.clear();
distance(&s1, pose1, &s2, pose2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
}
pose.setTranslation(Vec3f(40.1, 0, 0));
res.clear(), res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
@@ -130,16 +127,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver1, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
}
}
@@ -163,16 +160,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd)
pose.setTranslation(Vec3f(50, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
for(std::size_t i = 0; i < 10; ++i)
@@ -184,32 +181,32 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver1, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
}
pose.setTranslation(Vec3f(15.1, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
for(std::size_t i = 0; i < 10; ++i)
@@ -221,16 +218,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver1, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
}
}
@@ -254,16 +251,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_libccd)
pose.setTranslation(Vec3f(20, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
for(std::size_t i = 0; i < 10; ++i)
@@ -275,32 +272,32 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_libccd)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver1, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
}
pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :(
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
for(std::size_t i = 0; i < 10; ++i)
@@ -312,16 +309,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_libccd)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver1, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
}
}
@@ -345,15 +342,15 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_libccd)
pose.setTranslation(Vec3f(20, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
for(std::size_t i = 0; i < 10; ++i)
@@ -365,32 +362,32 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_libccd)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver1, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
}
pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :(
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
for(std::size_t i = 0; i < 10; ++i)
@@ -402,16 +399,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_libccd)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver1, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
}
}
@@ -428,6 +425,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK)
generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
DistanceRequest request;
+ request.gjk_solver_type = GST_INDEP;
DistanceResult res, res1;
Transform3f pose;
@@ -435,16 +433,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK)
pose.setTranslation(Vec3f(50, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
@@ -458,31 +456,31 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK)
res.clear(); res1.clear();
distance(&s1, pose1, &s2, pose2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
}
pose.setTranslation(Vec3f(40.1, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
@@ -495,16 +493,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
}
}
@@ -528,16 +526,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK)
pose.setTranslation(Vec3f(50, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
for(std::size_t i = 0; i < 10; ++i)
@@ -549,32 +547,32 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
}
pose.setTranslation(Vec3f(15.1, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
for(std::size_t i = 0; i < 10; ++i)
@@ -586,16 +584,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
}
}
@@ -619,16 +617,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK)
pose.setTranslation(Vec3f(20, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
for(std::size_t i = 0; i < 10; ++i)
@@ -640,22 +638,22 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
else
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
else
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
else
@@ -665,16 +663,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK)
pose.setTranslation(Vec3f(10.1, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
for(std::size_t i = 0; i < 10; ++i)
@@ -686,16 +684,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
}
}
@@ -719,16 +717,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK)
pose.setTranslation(Vec3f(20, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
for(std::size_t i = 0; i < 10; ++i)
@@ -740,32 +738,32 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
}
pose.setTranslation(Vec3f(10.1, 0, 0));
res.clear(); res1.clear();
- distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
- distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2, pose, request, res);
+ distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+ distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+ distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
for(std::size_t i = 0; i < 10; ++i)
@@ -777,16 +775,16 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK)
Transform3f pose2 = t * pose;
res.clear(); res1.clear();
- distance(&s1, pose1, &s2, pose2, &solver2, request, res);
- distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2, pose2, request, res);
+ distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+ distance(&s1, pose1, &s2_rss, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
res1.clear();
- distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+ distance(&s1_rss, pose1, &s2, pose2, request, res1);
BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
}
}
@@ -821,31 +819,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
// s1 is mesh, s2 is shape --> collision free
// all are reasonable
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(40, 0, 0));
@@ -853,31 +851,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
pose_obb.setTranslation(Vec3f(40, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(30, 0, 0));
@@ -885,31 +883,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
pose_obb.setTranslation(Vec3f(30, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(29.9, 0, 0));
@@ -917,31 +915,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
@@ -950,31 +948,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(-30, 0, 0));
@@ -982,31 +980,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
pose_obb.setTranslation(Vec3f(-30, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -1038,31 +1036,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
// s1 is mesh, s2 is shape --> collision free
// all are reasonable
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(15.01, 0, 0));
@@ -1070,31 +1068,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
pose_obb.setTranslation(Vec3f(15.01, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(14.99, 0, 0));
@@ -1102,31 +1100,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
pose_obb.setTranslation(Vec3f(14.99, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
}
@@ -1158,31 +1156,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
// s1 is mesh, s2 is shape --> collision free
// all are reasonable
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(22.4, 0, 0));
@@ -1190,31 +1188,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
pose_obb.setTranslation(Vec3f(22.4, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(22.51, 0, 0));
@@ -1222,31 +1220,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
pose_obb.setTranslation(Vec3f(22.51, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -1277,31 +1275,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_libccd)
pose_obb.setTranslation(Vec3f(9.99, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(10.01, 0, 0));
@@ -1309,31 +1307,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_libccd)
pose_obb.setTranslation(Vec3f(10.01, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -1364,31 +1362,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd)
pose_obb.setTranslation(Vec3f(9.9, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(10.1, 0, 0));
@@ -1396,31 +1394,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd)
pose_obb.setTranslation(Vec3f(10.1, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(0, 0, 9.9));
@@ -1428,31 +1426,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd)
pose_obb.setTranslation(Vec3f(0, 0, 9.9));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(0, 0, 10.1));
@@ -1460,31 +1458,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd)
pose_obb.setTranslation(Vec3f(0, 0, 10.1));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -1506,6 +1504,8 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
+ request.gjk_solver_type = GST_INDEP;
+
CollisionResult result;
bool res;
@@ -1519,31 +1519,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
// s1 is mesh, s2 is shape --> collision free
// all are reasonable
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(40, 0, 0));
@@ -1551,31 +1551,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
pose_obb.setTranslation(Vec3f(40, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(30, 0, 0));
@@ -1583,31 +1583,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
pose_obb.setTranslation(Vec3f(30, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(29.9, 0, 0));
@@ -1615,31 +1615,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
@@ -1648,31 +1648,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(-30, 0, 0));
@@ -1680,31 +1680,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
pose_obb.setTranslation(Vec3f(-30, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -1724,6 +1724,8 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK)
generateBVHModel(s2_obb, s2, Transform3f());
CollisionRequest request;
+ request.gjk_solver_type = GST_INDEP;
+
CollisionResult result;
bool res;
@@ -1736,31 +1738,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK)
// s1 is mesh, s2 is shape --> collision free
// all are reasonable
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(15.01, 0, 0));
@@ -1768,31 +1770,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK)
pose_obb.setTranslation(Vec3f(15.01, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(14.99, 0, 0));
@@ -1800,31 +1802,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK)
pose_obb.setTranslation(Vec3f(14.99, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
}
@@ -1844,6 +1846,8 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK)
generateBVHModel(s2_obb, s2, Transform3f());
CollisionRequest request;
+ request.gjk_solver_type = GST_INDEP;
+
CollisionResult result;
bool res;
@@ -1856,31 +1860,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK)
// s1 is mesh, s2 is shape --> collision free
// all are reasonable
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(22.4, 0, 0));
@@ -1888,31 +1892,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK)
pose_obb.setTranslation(Vec3f(22.4, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(22.51, 0, 0));
@@ -1920,31 +1924,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK)
pose_obb.setTranslation(Vec3f(22.51, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -1964,6 +1968,8 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK)
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
+ request.gjk_solver_type = GST_INDEP;
+
CollisionResult result;
bool res;
@@ -1975,31 +1981,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK)
pose_obb.setTranslation(Vec3f(9.99, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(10.01, 0, 0));
@@ -2007,31 +2013,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK)
pose_obb.setTranslation(Vec3f(10.01, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
@@ -2051,6 +2057,8 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK)
generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
CollisionRequest request;
+ request.gjk_solver_type = GST_INDEP;
+
CollisionResult result;
bool res;
@@ -2062,31 +2070,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK)
pose_obb.setTranslation(Vec3f(9.9, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(10.1, 0, 0));
@@ -2094,31 +2102,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK)
pose_obb.setTranslation(Vec3f(10.1, 0, 0));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
pose.setTranslation(Vec3f(0, 0, 9.9));
@@ -2126,31 +2134,31 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK)
pose_obb.setTranslation(Vec3f(0, 0, 9.9));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK(res);
pose.setTranslation(Vec3f(0, 0, 10.1));
@@ -2158,30 +2166,30 @@ BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK)
pose_obb.setTranslation(Vec3f(0, 0, 10.1));
result.clear();
- res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+ res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+ res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
result.clear();
- res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+ res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
BOOST_CHECK_FALSE(res);
}
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index 300c69ad..e5c0404b 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -1,5 +1,6 @@
#include "test_fcl_utility.h"
#include "fcl/collision.h"
+#include "fcl/continuous_collision.h"
#include "fcl/distance.h"
#include <cstdio>
#include <cstddef>
@@ -359,9 +360,9 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cda
bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_)
{
- CollisionData* cdata = static_cast<CollisionData*>(cdata_);
- const CollisionRequest& request = cdata->request;
- CollisionResult& result = cdata->result;
+ ContinuousCollisionData* cdata = static_cast<ContinuousCollisionData*>(cdata_);
+ const ContinuousCollisionRequest& request = cdata->request;
+ ContinuousCollisionResult& result = cdata->result;
if(cdata->done) return true;
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 173cbafd..f3341504 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -145,6 +145,24 @@ struct DistanceData
};
+/// @brief Continuous collision data stores the continuous collision request and result given the continuous collision algorithm.
+struct ContinuousCollisionData
+{
+ ContinuousCollisionData()
+ {
+ done = false;
+ }
+
+ /// @brief Continuous collision request
+ ContinuousCollisionRequest request;
+
+ /// @brief Continuous collision result
+ ContinuousCollisionResult result;
+
+ /// @brief Whether the continuous collision iteration can stop
+ bool done;
+};
+
/// @brief Default collision callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now.
--
GitLab