diff --git a/CMakeLists.txt b/CMakeLists.txt
index aff3da82302e70a73b76c5c19294631db608ad76..6fdcca6bbeba4a671815c115860fcb2fd11efc4f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -126,16 +126,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/math/matrix_3f.h
include/hpp/fcl/math/vec_3f.h
include/hpp/fcl/math/types.h
- include/hpp/fcl/math/tools.h
include/hpp/fcl/math/transform.h
- include/hpp/fcl/traversal/details/traversal.h
- include/hpp/fcl/traversal/traversal_node_shapes.h
- include/hpp/fcl/traversal/traversal_node_setup.h
- include/hpp/fcl/traversal/traversal_recurse.h
- include/hpp/fcl/traversal/traversal_node_octree.h
- include/hpp/fcl/traversal/traversal_node_bvhs.h
- include/hpp/fcl/traversal/traversal_node_bvh_shape.h
- include/hpp/fcl/traversal/traversal_node_base.h
include/hpp/fcl/data_types.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index c720f37a616f53625392fdc1ba04dde4cf9eb069..05859d25b1f359ebe000ecfa9d3ac9768af4e932 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -81,6 +81,30 @@ public:
{
}
+
+
+
+
+
+
+
+
+
+//start API in common test
+ /// @name Bounding volume API
+ /// Common API to BVs.
+ /// @{
+
+ /// @brief Check whether the AABB contains a point
+ inline bool contain(const Vec3f& p) const
+ {
+ if(p[0] < min_[0] || p[0] > max_[0]) return false;
+ if(p[1] < min_[1] || p[1] > max_[1]) return false;
+ if(p[2] < min_[2] || p[2] > max_[2]) return false;
+
+ return true;
+ }
+
/// @brief Check whether two AABB are overlap
inline bool overlap(const AABB& other) const
{
@@ -99,35 +123,11 @@ public:
bool overlap(const AABB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const;
- /// @brief Check whether the AABB contains another AABB
- inline bool contain(const AABB& other) const
- {
- return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
- }
-
- /// @brief Check whether two AABB are overlap and return the overlap part
- inline bool overlap(const AABB& other, AABB& overlap_part) const
- {
- if(!overlap(other))
- {
- return false;
- }
-
- overlap_part.min_ = min_.cwiseMax(other.min_);
- overlap_part.max_ = max_.cwiseMin(other.max_);
- return true;
- }
-
-
- /// @brief Check whether the AABB contains a point
- inline bool contain(const Vec3f& p) const
- {
- if(p[0] < min_[0] || p[0] > max_[0]) return false;
- if(p[1] < min_[1] || p[1] > max_[1]) return false;
- if(p[2] < min_[2] || p[2] > max_[2]) return false;
+ /// @brief Distance between two AABBs
+ FCL_REAL distance(const AABB& other) const;
- return true;
- }
+ /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points
+ FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
/// @brief Merge the AABB and a point
inline AABB& operator += (const Vec3f& p)
@@ -152,6 +152,18 @@ public:
return res += other;
}
+ /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
+ inline FCL_REAL size() const
+ {
+ return (max_ - min_).squaredNorm();
+ }
+
+ /// @brief Center of the AABB
+ inline Vec3f center() const
+ {
+ return (min_ + max_) * 0.5;
+ }
+
/// @brief Width of the AABB
inline FCL_REAL width() const
{
@@ -174,26 +186,42 @@ public:
inline FCL_REAL volume() const
{
return width() * height() * depth();
- }
+ }
- /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
- inline FCL_REAL size() const
+ /// @}
+//End API in common test
+
+
+
+
+
+
+
+
+
+
+
+
+
+ /// @brief Check whether the AABB contains another AABB
+ inline bool contain(const AABB& other) const
{
- return (max_ - min_).squaredNorm();
+ return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
}
- /// @brief Center of the AABB
- inline Vec3f center() const
+ /// @brief Check whether two AABB are overlap and return the overlap part
+ inline bool overlap(const AABB& other, AABB& overlap_part) const
{
- return (min_ + max_) * 0.5;
+ if(!overlap(other))
+ {
+ return false;
+ }
+
+ overlap_part.min_ = min_.cwiseMax(other.min_);
+ overlap_part.max_ = max_.cwiseMin(other.max_);
+ return true;
}
- /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points
- FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
-
- /// @brief Distance between two AABBs
- FCL_REAL distance(const AABB& other) const;
-
/// @brief expand the half size of the AABB by delta, and keep the center unchanged.
inline AABB& expand(const Vec3f& delta)
{
@@ -241,7 +269,6 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
-
}
} // namespace hpp
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index 437f65957ff952380157b893a705782accd70999..3b1cb3f5dc09785c116e233256760c6234a50cbb 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -60,6 +60,12 @@ public:
/// @brief Half dimensions of OBB
Vec3f extent;
+
+
+
+ /// @brief Check whether the OBB contains a point.
+ bool contain(const Vec3f& p) const;
+
/// Check collision between two OBB
/// \return true if collision happens.
bool overlap(const OBB& other) const;
@@ -71,8 +77,8 @@ public:
bool overlap(const OBB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const;
- /// @brief Check whether the OBB contains a point.
- bool contain(const Vec3f& p) const;
+ /// @brief Distance between two OBBs, not implemented.
+ FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief A simple way to merge the OBB and a point (the result is not compact).
OBB& operator += (const Vec3f& p);
@@ -87,6 +93,18 @@ public:
/// @brief Return the merged OBB of current OBB and the other one (the result is not compact).
OBB operator + (const OBB& other) const;
+ /// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
+ inline FCL_REAL size() const
+ {
+ return extent.squaredNorm();
+ }
+
+ /// @brief Center of the OBB
+ inline const Vec3f& center() const
+ {
+ return To;
+ }
+
/// @brief Width of the OBB.
inline FCL_REAL width() const
{
@@ -110,22 +128,6 @@ public:
{
return width() * height() * depth();
}
-
- /// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
- inline FCL_REAL size() const
- {
- return extent.squaredNorm();
- }
-
- /// @brief Center of the OBB
- inline const Vec3f& center() const
- {
- return To;
- }
-
-
- /// @brief Distance between two OBBs, not implemented.
- FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
};
diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h
index 74fb3bffaaa60a8600bd8475594605d20f6d60f7..cb6ed77bd9307edb6b5173546feff655c6e0b516 100644
--- a/include/hpp/fcl/BV/OBBRSS.h
+++ b/include/hpp/fcl/BV/OBBRSS.h
@@ -59,6 +59,16 @@ public:
/// @brief RSS member, for distance
RSS rss;
+
+
+
+
+/// @brief Check whether the OBBRSS contains a point
+ inline bool contain(const Vec3f& p) const
+ {
+ return obb.contain(p);
+ }
+
/// @brief Check collision between two OBBRSS
bool overlap(const OBBRSS& other) const
{
@@ -74,10 +84,10 @@ public:
return obb.overlap(other.obb, request, sqrDistLowerBound);
}
-/// @brief Check whether the OBBRSS contains a point
- inline bool contain(const Vec3f& p) const
+ /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
+ FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
{
- return obb.contain(p);
+ return rss.distance(other.rss, P, Q);
}
/// @brief Merge the OBBRSS and a point
@@ -104,6 +114,18 @@ public:
return result;
}
+ /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
+ inline FCL_REAL size() const
+ {
+ return obb.size();
+ }
+
+ /// @brief Center of the OBBRSS
+ inline const Vec3f& center() const
+ {
+ return obb.center();
+ }
+
/// @brief Width of the OBRSS
inline FCL_REAL width() const
{
@@ -127,25 +149,12 @@ public:
{
return obb.volume();
}
+};
+
+
- /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
- inline FCL_REAL size() const
- {
- return obb.size();
- }
- /// @brief Center of the OBBRSS
- inline const Vec3f& center() const
- {
- return obb.center();
- }
- /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
- FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
- {
- return rss.distance(other.rss, P, Q);
- }
-};
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index a2c0111da817b431e877aeb690a2f0533678d8b3..e158d5cc7fd80cb7a482d4b579959101f9652cb7 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -65,6 +65,10 @@ public:
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL r;
+
+ /// @brief Check whether the RSS contains a point
+ inline bool contain(const Vec3f& p) const;
+
/// @brief Check collision between two RSS
bool overlap(const RSS& other) const;
@@ -76,15 +80,8 @@ public:
return overlap (other);
}
- /// @brief Check collision between two RSS and return the overlap part.
- /// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
- bool overlap(const RSS& other, RSS& /*overlap_part*/) const
- {
- return overlap(other);
- }
-
- /// @brief Check whether the RSS contains a point
- inline bool contain(const Vec3f& p) const;
+ /// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points
+ FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief A simple way to merge the RSS and a point, not compact.
/// @todo This function may have some bug.
@@ -100,6 +97,19 @@ public:
/// @brief Return the merged RSS of current RSS and the other one
RSS operator + (const RSS& other) const;
+
+ /// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
+ inline FCL_REAL size() const
+ {
+ return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
+ }
+
+ /// @brief The RSS center
+ inline const Vec3f& center() const
+ {
+ return Tr;
+ }
+
/// @brief Width of the RSS
inline FCL_REAL width() const
{
@@ -124,21 +134,12 @@ public:
return (l[0] * l[1] * 2 * r + 4 * boost::math::constants::pi<FCL_REAL>() * r * r * r);
}
- /// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
- inline FCL_REAL size() const
- {
- return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
- }
-
- /// @brief The RSS center
- inline const Vec3f& center() const
+ /// @brief Check collision between two RSS and return the overlap part.
+ /// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
+ bool overlap(const RSS& other, RSS& /*overlap_part*/) const
{
- return Tr;
+ return overlap(other);
}
-
- /// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points
- FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-
};
/// @brief distance between two RSS bounding volumes
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 43c6db0972d0a52e58b45bee08b82be8a1765c5b..00af0f813c477e9780c23c49be5b0061bd6062ab 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -111,8 +111,8 @@ public:
return overlap (other);
}
- //// @brief Check whether one point is inside the KDOP
- bool inside(const Vec3f& p) const;
+ /// @brief The distance between two KDOP<N>. Not implemented.
+ FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief Merge the point and the KDOP
KDOP<N>& operator += (const Vec3f& p);
@@ -123,6 +123,19 @@ public:
/// @brief Create a KDOP by mergin two KDOPs
KDOP<N> operator + (const KDOP<N>& other) const;
+ /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
+ inline FCL_REAL size() const
+ {
+ return width() * width() + height() * height() + depth() * depth();
+ }
+
+ /// @brief The (AABB) center
+ inline Vec3f center() const
+ {
+ return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
+ }
+
+
/// @brief The (AABB) width
inline FCL_REAL width() const
{
@@ -147,21 +160,6 @@ public:
return width() * height() * depth();
}
- /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
- inline FCL_REAL size() const
- {
- return width() * width() + height() * height() + depth() * depth();
- }
-
- /// @brief The (AABB) center
- inline Vec3f center() const
- {
- return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
- }
-
- /// @brief The distance between two KDOP<N>. Not implemented.
- FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-
inline FCL_REAL dist(std::size_t i) const
{
return dist_[i];
@@ -172,6 +170,8 @@ public:
return dist_[i];
}
+ //// @brief Check whether one point is inside the KDOP
+ bool inside(const Vec3f& p) const;
};
diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h
index 77ab3e45124ef9ce6e0b39f23490786ba3ee5d11..2227c9754a9b07707534cfaba04ab898bf7f56bb 100644
--- a/include/hpp/fcl/BV/kIOS.h
+++ b/include/hpp/fcl/BV/kIOS.h
@@ -93,6 +93,9 @@ public:
/// @ OBB related with kIOS
OBB obb;
+ /// @brief Check whether the kIOS contains a point
+ inline bool contain(const Vec3f& p) const;
+
/// @brief Check collision between two kIOS
bool overlap(const kIOS& other) const;
@@ -100,8 +103,8 @@ public:
bool overlap(const kIOS& other, const CollisionRequest&,
FCL_REAL& sqrDistLowerBound) const;
- /// @brief Check whether the kIOS contains a point
- inline bool contain(const Vec3f& p) const;
+ /// @brief The distance between two kIOS
+ FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
/// @brief A simple way to merge the kIOS and a point
kIOS& operator += (const Vec3f& p);
@@ -116,6 +119,9 @@ public:
/// @brief Return the merged kIOS of current kIOS and the other one
kIOS operator + (const kIOS& other) const;
+ /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
+ FCL_REAL size() const;
+
/// @brief Center of the kIOS
const Vec3f& center() const
{
@@ -133,12 +139,6 @@ public:
/// @brief Volume of the kIOS
FCL_REAL volume() const;
-
- /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
- FCL_REAL size() const;
-
- /// @brief The distance between two kIOS
- FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
};
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 2a7be848e4d625c65c71e6270d153afcbcbb64e3..7d7f00c8f041dd63d2d6b91352e84eccb4559cf4 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -77,10 +77,10 @@ public:
BVHBuildState build_state;
/// @brief Split rule to split one BV node into two children
- boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
+ boost::shared_ptr<BVSplitter<BV> > bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
- boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
+ boost::shared_ptr<BVFitterTpl<BV> > bv_fitter;
/// @brief Model type described by the instance
BVHModelType getModelType() const
diff --git a/include/hpp/fcl/BVH/BV_fitter.h b/include/hpp/fcl/BVH/BV_fitter.h
index 97a27f9da198a6d88890e6278224c31a32e6b693..d8070d695129b820c00a979007416b3e8cf5e70e 100644
--- a/include/hpp/fcl/BVH/BV_fitter.h
+++ b/include/hpp/fcl/BVH/BV_fitter.h
@@ -74,27 +74,9 @@ void fit<OBBRSS>(Vec3f* ps, int n, OBBRSS& bv);
template<>
void fit<AABB>(Vec3f* ps, int n, AABB& bv);
-/// @brief Interface for fitting a bv given the triangles or points inside it.
-template<typename BV>
-class BVFitterBase
-{
-public:
- /// @brief Set the primitives to be processed by the fitter
- virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
- /// @brief Set the primitives to be processed by the fitter, for deformable mesh.
- virtual void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
- /// @brief Compute the fitting BV
- virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0;
-
- /// @brief clear the temporary data generated.
- virtual void clear() = 0;
-};
-
/// @brief The class for the default algorithm fitting a bounding volume to a set of points
template<typename BV>
-class BVFitterTpl : public BVFitterBase<BV>
+class BVFitterTpl
{
public:
/// @brief default deconstructor
@@ -118,6 +100,9 @@ public:
type = type_;
}
+ /// @brief Compute the fitting BV
+ virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0;
+
/// @brief Clear the geometry primitive data
void clear()
{
diff --git a/include/hpp/fcl/BVH/BV_splitter.h b/include/hpp/fcl/BVH/BV_splitter.h
index 7ad7bd996b0d6fef5b96318256579f03f0f6cd1b..7be8ca7490f7ae6fe3e268e4ac3a3c6b8d3e0e52 100644
--- a/include/hpp/fcl/BVH/BV_splitter.h
+++ b/include/hpp/fcl/BVH/BV_splitter.h
@@ -49,32 +49,13 @@ namespace hpp
namespace fcl
{
-/// @brief Base interface for BV splitting algorithm
-template<typename BV>
-class BVSplitterBase
-{
-public:
- /// @brief Set the geometry data needed by the split rule
- virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
-
- /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node
- virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0;
-
- /// @brief Apply the split rule on a given point
- virtual bool apply(const Vec3f& q) const = 0;
-
- /// @brief Clear the geometry data set before
- virtual void clear() = 0;
-};
-
-
/// @brief Three types of split algorithms are provided in FCL as default
enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER};
/// @brief A class describing the split rule that splits each BV node
template<typename BV>
-class BVSplitter : public BVSplitterBase<BV>
+class BVSplitter
{
public:
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 1106c965d68b9e89559aa80a3cdbc39e99878530..d089a77bad653a27a575db85411aaa67f20af01c 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -39,7 +39,7 @@
#include <hpp/fcl/BVH/BVH_utility.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/math/tools.h>
+#include "../math/tools.h"
#include <iostream>
#include <limits>
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 75dd554d812c12a1864d1816315c924f678b58ea..ff3563cc29bf3ab591cbcdd65d5bfedd508a56d8 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -38,7 +38,7 @@
#include <hpp/fcl/BV/RSS.h>
#include <hpp/fcl/BVH/BVH_utility.h>
#include <iostream>
-#include <hpp/fcl/math/tools.h>
+#include "../math/tools.h"
namespace hpp
{
namespace fcl
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index 3ef215079e236f369b65522293f76fbc1eaf4c91..d04481fb84f451b065881f26c5a4d9060fc83249 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -38,7 +38,7 @@
#include <hpp/fcl/BVH/BV_fitter.h>
#include <hpp/fcl/BVH/BVH_utility.h>
#include <limits>
-#include <hpp/fcl/math/tools.h>
+#include "../math/tools.h"
namespace hpp
{
diff --git a/src/collision.cpp b/src/collision.cpp
index c5405037d4e9cd41d46acf2596467c5d48efcd01..b6a45e8301499083696e1cb48ff2e4aa51671f86 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -54,9 +54,9 @@ CollisionFunctionMatrix<GJKSolver>& getCollisionFunctionLookTable()
return table;
}
-template<typename NarrowPhaseSolver>
+template<typename GJKSolver>
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result)
{
@@ -81,18 +81,18 @@ void invertResults(CollisionResult& result)
}
}
-template<typename NarrowPhaseSolver>
+template<typename GJKSolver>
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver_,
+ const GJKSolver* nsolver_,
const CollisionRequest& request,
CollisionResult& result)
{
- const NarrowPhaseSolver* nsolver = nsolver_;
+ const GJKSolver* nsolver = nsolver_;
if(!nsolver_)
- nsolver = new NarrowPhaseSolver();
+ nsolver = new GJKSolver();
- const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
+ const CollisionFunctionMatrix<GJKSolver>& looktable = getCollisionFunctionLookTable<GJKSolver>();
result.distance_lower_bound = -1;
std::size_t res;
if(request.num_max_contacts == 0)
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 69b01a281455a764e4fedbdb3414c52405b22313..5dbcc9181a6988fb65898c55fcc03d66ba42e2d1 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -38,7 +38,7 @@
#include <hpp/fcl/collision_func_matrix.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
+#include "traversal/traversal_node_setup.h"
#include <../src/collision_node.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include "distance_func_matrix.h"
@@ -49,17 +49,17 @@ namespace fcl
{
#ifdef HPP_FCL_HAVE_OCTOMAP
-template<typename T_SH, typename NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
- ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node (request);
+ ShapeOcTreeCollisionTraversalNode<T_SH, GJKSolver> node (request);
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
@@ -67,17 +67,17 @@ std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& t
return result.numContacts();
}
-template<typename T_SH, typename NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node (request);
+ OcTreeShapeCollisionTraversalNode<T_SH, GJKSolver> node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
@@ -85,17 +85,17 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& t
return result.numContacts();
}
-template<typename NarrowPhaseSolver>
+template<typename GJKSolver>
std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeCollisionTraversalNode<NarrowPhaseSolver> node (request);
+ OcTreeCollisionTraversalNode<GJKSolver> node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
@@ -103,34 +103,34 @@ std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, c
return result.numContacts();
}
-template<typename T_BVH, typename NarrowPhaseSolver>
+template<typename T_BVH, typename GJKSolver>
std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node (request);
+ OcTreeMeshCollisionTraversalNode<T_BVH, GJKSolver> node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
}
-template<typename T_BVH, typename NarrowPhaseSolver>
+template<typename T_BVH, typename GJKSolver>
std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
- MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node (request);
+ MeshOcTreeCollisionTraversalNode<T_BVH, GJKSolver> node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
@@ -139,16 +139,16 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1
#endif
-template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+template<typename T_SH1, typename T_SH2, typename GJKSolver>
std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
DistanceResult distanceResult;
DistanceRequest distanceRequest (request.enable_contact);
- FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, NarrowPhaseSolver>
+ FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, GJKSolver>
(o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
if (distance <= 0) {
@@ -179,16 +179,16 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
return 0;
}
-template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
+template<typename T_BVH, typename T_SH, typename GJKSolver>
struct BVHShapeCollider
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
- MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node (request);
+ MeshShapeCollisionTraversalNode<T_BVH, T_SH, GJKSolver> node (request);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
@@ -205,9 +205,9 @@ struct BVHShapeCollider
namespace details
{
-template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
+template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH, typename GJKSolver>
std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
@@ -224,50 +224,50 @@ std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform
}
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
+struct BVHShapeCollider<OBB, T_SH, GJKSolver>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver>, OBB, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBB<T_SH, GJKSolver>, OBB, T_SH, GJKSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
+struct BVHShapeCollider<RSS, T_SH, GJKSolver>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver>, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeRSS<T_SH, GJKSolver>, RSS, T_SH, GJKSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
+struct BVHShapeCollider<kIOS, T_SH, GJKSolver>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver>, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodekIOS<T_SH, GJKSolver>, kIOS, T_SH, GJKSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
+struct BVHShapeCollider<OBBRSS, T_SH, GJKSolver>
{
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
+ return details::orientedBVHShapeCollide<MeshShapeCollisionTraversalNodeOBBRSS<T_SH, GJKSolver>, OBBRSS, T_SH, GJKSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -333,17 +333,17 @@ std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1
}
-template<typename T_BVH, typename NarrowPhaseSolver>
+template<typename T_BVH, typename GJKSolver>
std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* /*nsolver*/,
+ const GJKSolver* /*nsolver*/,
const CollisionRequest& request, CollisionResult& result)
{
return BVHCollide<T_BVH>(o1, tf1, o2, tf2, request, result);
}
-template<typename NarrowPhaseSolver>
-CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
+template<typename GJKSolver>
+CollisionFunctionMatrix<GJKSolver>::CollisionFunctionMatrix()
{
for(int i = 0; i < NODE_COUNT; ++i)
{
@@ -351,197 +351,197 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
collision_matrix[i][j] = NULL;
}
- collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide<Box, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide<Capsule, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide<Cone, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide<Cylinder, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<Convex, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide<Plane, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide<Halfspace, Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide<Halfspace, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide<Halfspace, Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider<AABB, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider<OBB, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider<RSS, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<16>, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<18>, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<24>, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider<kIOS, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS, Capsule, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS, Cone, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, Convex, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane, NarrowPhaseSolver>::collide;
- collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider<OBBRSS, Halfspace, NarrowPhaseSolver>::collide;
-
- collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB, NarrowPhaseSolver>;
- collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB, NarrowPhaseSolver>;
- collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS, NarrowPhaseSolver>;
- collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16>, NarrowPhaseSolver>;
- collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18>, NarrowPhaseSolver>;
- collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24>, NarrowPhaseSolver>;
- collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS, NarrowPhaseSolver>;
- collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box, GJKSolver>;
+ collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere, GJKSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule, GJKSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone, GJKSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex, GJKSolver>;
+ collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane, GJKSolver>;
+ collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide<Box, Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere, Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide<Capsule, Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide<Cone, Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide<Cylinder, Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex, Capsule, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex, Cone, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<Convex, Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide<Plane, Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace, Box, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace, Sphere, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace, Capsule, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace, Cone, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace, Cylinder, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide<Halfspace, Convex, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide<Halfspace, Plane, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide<Halfspace, Halfspace, GJKSolver>;
+
+ collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box, GJKSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone, GJKSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex, GJKSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane, GJKSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider<AABB, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box, GJKSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone, GJKSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex, GJKSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane, GJKSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider<OBB, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box, GJKSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone, GJKSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex, GJKSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane, GJKSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider<RSS, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box, GJKSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone, GJKSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex, GJKSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane, GJKSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<16>, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box, GJKSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone, GJKSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex, GJKSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane, GJKSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<18>, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box, GJKSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone, GJKSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex, GJKSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane, GJKSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<24>, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box, GJKSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone, GJKSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex, GJKSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane, GJKSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider<kIOS, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box, GJKSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere, GJKSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS, Capsule, GJKSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS, Cone, GJKSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder, GJKSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, Convex, GJKSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane, GJKSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider<OBBRSS, Halfspace, GJKSolver>::collide;
+
+ collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB, GJKSolver>;
+ collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB, GJKSolver>;
+ collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS, GJKSolver>;
+ collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16>, GJKSolver>;
+ collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18>, GJKSolver>;
+ collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24>, GJKSolver>;
+ collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS, GJKSolver>;
+ collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS, GJKSolver>;
#ifdef HPP_FCL_HAVE_OCTOMAP
- collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex, NarrowPhaseSolver>;
- collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace, NarrowPhaseSolver>;
-
- collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<NarrowPhaseSolver>;
-
- collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<16>, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<18>, NarrowPhaseSolver>;
- collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<24>, NarrowPhaseSolver>;
-
- collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB, NarrowPhaseSolver>;
- collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB, NarrowPhaseSolver>;
- collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS, NarrowPhaseSolver>;
- collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS, NarrowPhaseSolver>;
- collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS, NarrowPhaseSolver>;
- collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16>, NarrowPhaseSolver>;
- collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18>, NarrowPhaseSolver>;
- collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24>, NarrowPhaseSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box, GJKSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere, GJKSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule, GJKSolver>;
+ collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone, GJKSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder, GJKSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex, GJKSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane, GJKSolver>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace, GJKSolver>;
+
+ collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<GJKSolver>;
+
+ collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<16>, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<18>, GJKSolver>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<24>, GJKSolver>;
+
+ collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB, GJKSolver>;
+ collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB, GJKSolver>;
+ collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS, GJKSolver>;
+ collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS, GJKSolver>;
+ collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS, GJKSolver>;
+ collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16>, GJKSolver>;
+ collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18>, GJKSolver>;
+ collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24>, GJKSolver>;
#endif
}
template struct CollisionFunctionMatrix<GJKSolver>;
diff --git a/src/collision_node.cpp b/src/collision_node.cpp
index c69457030e0ea3d387bcb42cfea06a2ba9822c72..2091362e39dcae030efb77dc67fe938d1ed31570 100644
--- a/src/collision_node.cpp
+++ b/src/collision_node.cpp
@@ -37,7 +37,7 @@
#include <../src/collision_node.h>
-#include <hpp/fcl/traversal/traversal_recurse.h>
+#include "traversal/traversal_recurse.h"
namespace hpp
{
diff --git a/src/collision_node.h b/src/collision_node.h
index 2f17fec589163edb1a7e65451d322a511113909e..10a89dba69e01fb1e04001004e69b28d7bab37f0 100644
--- a/src/collision_node.h
+++ b/src/collision_node.h
@@ -39,8 +39,8 @@
#ifndef HPP_FCL_COLLISION_NODE_H
#define HPP_FCL_COLLISION_NODE_H
-#include <hpp/fcl/traversal/traversal_node_base.h>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
+#include "traversal/traversal_node_base.h"
+#include "traversal/traversal_node_bvhs.h"
#include <hpp/fcl/BVH/BVH_front.h>
diff --git a/src/distance.cpp b/src/distance.cpp
index b0acef0bedbbd4349e7f2ff56de4440530a162dd..41ae36ee8a122137f89d5857cbadcee2f648fbaa 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -53,25 +53,25 @@ DistanceFunctionMatrix<GJKSolver>& getDistanceFunctionLookTable()
return table;
}
-template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
+template<typename GJKSolver>
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
- return distance<NarrowPhaseSolver>(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver,
+ return distance<GJKSolver>(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver,
request, result);
}
-template<typename NarrowPhaseSolver>
+template<typename GJKSolver>
FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver_,
+ const GJKSolver* nsolver_,
const DistanceRequest& request, DistanceResult& result)
{
- const NarrowPhaseSolver* nsolver = nsolver_;
+ const GJKSolver* nsolver = nsolver_;
if(!nsolver_)
- nsolver = new NarrowPhaseSolver();
+ nsolver = new GJKSolver();
- const DistanceFunctionMatrix<NarrowPhaseSolver>& looktable = getDistanceFunctionLookTable<NarrowPhaseSolver>();
+ const DistanceFunctionMatrix<GJKSolver>& looktable = getDistanceFunctionLookTable<GJKSolver>();
OBJECT_TYPE object_type1 = o1->getObjectType();
NODE_TYPE node_type1 = o1->getNodeType();
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index a2637a7a82f42f2b2eb0e5164b94dc6e79e5990f..126dbefc90c1cb78d6f7b9ea59e3b1d6da5eb884 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -38,7 +38,7 @@
#include <hpp/fcl/distance_func_matrix.h>
#include <../src/collision_node.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
+#include "traversal/traversal_node_setup.h"
#include <hpp/fcl/narrowphase/narrowphase.h>
namespace hpp
@@ -47,15 +47,15 @@ namespace fcl
{
#ifdef HPP_FCL_HAVE_OCTOMAP
-template<typename T_SH, typename NarrowPhaseSolver>
-FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+template<typename T_SH, typename GJKSolver>
+FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- ShapeOcTreeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
+ ShapeOcTreeDistanceTraversalNode<T_SH, GJKSolver> node;
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -63,15 +63,15 @@ FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1
return result.min_distance;
}
-template<typename T_SH, typename NarrowPhaseSolver>
-FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+template<typename T_SH, typename GJKSolver>
+FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- OcTreeShapeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
+ OcTreeShapeDistanceTraversalNode<T_SH, GJKSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -79,15 +79,15 @@ FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1
return result.min_distance;
}
-template<typename NarrowPhaseSolver>
-FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+template<typename GJKSolver>
+FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
+ OcTreeDistanceTraversalNode<GJKSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -95,15 +95,15 @@ FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, con
return result.min_distance;
}
-template<typename T_BVH, typename NarrowPhaseSolver>
-FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+template<typename T_BVH, typename GJKSolver>
+FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- MeshOcTreeDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ MeshOcTreeDistanceTraversalNode<T_BVH, GJKSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -111,15 +111,15 @@ FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
return result.min_distance;
}
-template<typename T_BVH, typename NarrowPhaseSolver>
-FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+template<typename T_BVH, typename GJKSolver>
+FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- OcTreeMeshDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
+ OcTreeMeshDistanceTraversalNode<T_BVH, GJKSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver<GJKSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -129,12 +129,12 @@ FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
#endif
-template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
-FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+template<typename T_SH1, typename T_SH2, typename GJKSolver>
+FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- ShapeDistanceTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
+ ShapeDistanceTraversalNode<T_SH1, T_SH2, GJKSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
@@ -144,14 +144,14 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
return result.min_distance;
}
-template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
+template<typename T_BVH, typename T_SH, typename GJKSolver>
struct BVHShapeDistancer
{
- static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
- MeshShapeDistanceTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
+ MeshShapeDistanceTraversalNode<T_BVH, T_SH, GJKSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
@@ -168,8 +168,8 @@ struct BVHShapeDistancer
namespace details
{
-template<typename OrientedMeshShapeDistanceTraversalNode, typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
-FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+template<typename OrientedMeshShapeDistanceTraversalNode, typename T_BVH, typename T_SH, typename GJKSolver>
+FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
if(request.isSatisfied(result)) return result.min_distance;
@@ -185,34 +185,34 @@ FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3f
}
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
+struct BVHShapeDistancer<RSS, T_SH, GJKSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
- return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver>, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
+ return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodeRSS<T_SH, GJKSolver>, RSS, T_SH, GJKSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
+struct BVHShapeDistancer<kIOS, T_SH, GJKSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
- return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver>, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
+ return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodekIOS<T_SH, GJKSolver>, kIOS, T_SH, GJKSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
+template<typename T_SH, typename GJKSolver>
+struct BVHShapeDistancer<OBBRSS, T_SH, GJKSolver>
{
- static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+ static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
- return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver>, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result);
+ return details::orientedBVHShapeDistance<MeshShapeDistanceTraversalNodeOBBRSS<T_SH, GJKSolver>, OBBRSS, T_SH, GJKSolver>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -280,16 +280,16 @@ FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1
}
-template<typename T_BVH, typename NarrowPhaseSolver>
+template<typename T_BVH, typename GJKSolver>
FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* /*nsolver*/,
+ const GJKSolver* /*nsolver*/,
const DistanceRequest& request, DistanceResult& result)
{
return BVHDistance<T_BVH>(o1, tf1, o2, tf2, request, result);
}
-template<typename NarrowPhaseSolver>
-DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
+template<typename GJKSolver>
+DistanceFunctionMatrix<GJKSolver>::DistanceFunctionMatrix()
{
for(int i = 0; i < NODE_COUNT; ++i)
{
@@ -297,200 +297,200 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[i][j] = NULL;
}
- distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance<Box, Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere, Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance<Capsule, Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance<Cone, Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance<Cylinder, Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<Convex, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<Convex, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<Convex, Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance<Plane, Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance<Halfspace, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance<Halfspace, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance<Halfspace, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance<Halfspace, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance<Halfspace, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance<Halfspace, Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance<Halfspace, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance<Halfspace, Halfspace, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box, Box, GJKSolver>;
+ distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere, GJKSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule, GJKSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone, GJKSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, Convex, GJKSolver>;
+ distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane, GJKSolver>;
+ distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance<Box, Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere, Capsule, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere, Convex, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere, Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule, Capsule, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule, Convex, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule, Plane, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance<Capsule, Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone, Convex, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance<Cone, Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder, Capsule, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder, Cone, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder, Convex, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder, Plane, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance<Cylinder, Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex, Box, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex, Sphere, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<Convex, Capsule, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<Convex, Cone, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex, Convex, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex, Plane, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<Convex, Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane, Capsule, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, Convex, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance<Plane, Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance<Halfspace, Box, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance<Halfspace, Sphere, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance<Halfspace, Capsule, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance<Halfspace, Cone, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance<Halfspace, Cylinder, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance<Halfspace, Convex, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance<Halfspace, Plane, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance<Halfspace, Halfspace, GJKSolver>;
/* AABB distance not implemented */
/*
- distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box, GJKSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB, Cone, GJKSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, GJKSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, GJKSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, GJKSolver>::distance;
*/
- distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
-
- distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer<RSS, Halfspace, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, GJKSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone, GJKSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, GJKSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, GJKSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, GJKSolver>::distance;
+
+ distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, GJKSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone, GJKSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, Convex, GJKSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane, GJKSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer<RSS, Halfspace, GJKSolver>::distance;
/* KDOP distance not implemented */
/*
- distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<16>, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<16>, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<16>, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<16>, Halfspace, NarrowPhaseSolver>::distance;
-
- distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<18>, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<18>, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<18>, Halfspace, NarrowPhaseSolver>::distance;
-
- distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<24>, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<24>, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<24>, Halfspace, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>, Box, GJKSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<16>, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<16>, Cone, GJKSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<16>, Convex, GJKSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>, Plane, GJKSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<16>, Halfspace, GJKSolver>::distance;
+
+ distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box, GJKSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<18>, Cone, GJKSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<18>, Convex, GJKSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>, Plane, GJKSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<18>, Halfspace, GJKSolver>::distance;
+
+ distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box, GJKSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<24>, Cone, GJKSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<24>, Convex, GJKSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>, Plane, GJKSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<24>, Halfspace, GJKSolver>::distance;
*/
- distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer<kIOS, Halfspace, NarrowPhaseSolver>::distance;
-
- distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS, Capsule, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS, Cone, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS, Convex, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS, Plane, NarrowPhaseSolver>::distance;
- distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
-
- distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
- distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB, NarrowPhaseSolver>;
- distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
- distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
- distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
+ distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box, GJKSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS, Cone, GJKSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS, Convex, GJKSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS, Plane, GJKSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer<kIOS, Halfspace, GJKSolver>::distance;
+
+ distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box, GJKSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere, GJKSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS, Capsule, GJKSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS, Cone, GJKSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder, GJKSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS, Convex, GJKSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS, Plane, GJKSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, GJKSolver>::distance;
+
+ distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, GJKSolver>;
+ distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB, GJKSolver>;
+ distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, GJKSolver>;
+ distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, GJKSolver>;
+ distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, GJKSolver>;
#ifdef HPP_FCL_HAVE_OCTOMAP
- distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex, NarrowPhaseSolver>;
- distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace, NarrowPhaseSolver>;
-
- distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<NarrowPhaseSolver>;
-
- distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<16>, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<18>, NarrowPhaseSolver>;
- distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<24>, NarrowPhaseSolver>;
-
- distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB, NarrowPhaseSolver>;
- distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB, NarrowPhaseSolver>;
- distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS, NarrowPhaseSolver>;
- distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS, NarrowPhaseSolver>;
- distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS, NarrowPhaseSolver>;
- distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<16>, NarrowPhaseSolver>;
- distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<18>, NarrowPhaseSolver>;
- distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<24>, NarrowPhaseSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box, GJKSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere, GJKSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule, GJKSolver>;
+ distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone, GJKSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder, GJKSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex, GJKSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane, GJKSolver>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace, GJKSolver>;
+
+ distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<GJKSolver>;
+
+ distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<16>, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<18>, GJKSolver>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<24>, GJKSolver>;
+
+ distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB, GJKSolver>;
+ distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB, GJKSolver>;
+ distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS, GJKSolver>;
+ distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS, GJKSolver>;
+ distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS, GJKSolver>;
+ distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<16>, GJKSolver>;
+ distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<18>, GJKSolver>;
+ distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<24>, GJKSolver>;
#endif
diff --git a/src/distance_func_matrix.h b/src/distance_func_matrix.h
index 5cb194384daf982f891d858de1297038277673ba..97e90780a3ff7dee186b91928b6a609e71fbcfea 100644
--- a/src/distance_func_matrix.h
+++ b/src/distance_func_matrix.h
@@ -39,18 +39,18 @@
namespace hpp
{
namespace fcl {
- template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+ template<typename T_SH1, typename T_SH2, typename GJKSolver>
FCL_REAL ShapeShapeDistance
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver, const DistanceRequest& request,
+ const GJKSolver* nsolver, const DistanceRequest& request,
DistanceResult& result);
- template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+ template<typename T_SH1, typename T_SH2, typename GJKSolver>
std::size_t ShapeShapeCollide
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver, const CollisionRequest& request,
+ const GJKSolver* nsolver, const CollisionRequest& request,
CollisionResult& result);
}
diff --git a/src/intersect.cpp b/src/intersect.cpp
index a69343de864f491a98cb51a43639f7af61e1ac0a..15df43fc3432f1245e75ac21d84e5291b4bd72fa 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -40,7 +40,7 @@
#include <limits>
#include <vector>
#include <boost/math/special_functions/fpclassify.hpp>
-#include <hpp/fcl/math/tools.h>
+#include "math/tools.h"
namespace hpp
{
diff --git a/include/hpp/fcl/math/tools.h b/src/math/tools.h
similarity index 100%
rename from include/hpp/fcl/math/tools.h
rename to src/math/tools.h
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index b8cfeb0f863fda002107c3d11304efb72589646b..75b40d66c4647b87170444a11ae81dff622370c0 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -38,7 +38,7 @@
#ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H
# define HPP_FCL_SRC_NARROWPHASE_DETAILS_H
-#include <hpp/fcl/traversal/traversal_node_setup.h>
+#include "../traversal/traversal_node_setup.h"
#include <hpp/fcl/narrowphase/narrowphase.h>
namespace hpp
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 9db577c471f7237298e08ede37046659b96bf51a..4d167e985d0dec64ad339a7422444d9a0f5dff59 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -37,7 +37,7 @@
#include <hpp/fcl/narrowphase/gjk.h>
#include <hpp/fcl/intersect.h>
-#include <hpp/fcl/math/tools.h>
+#include "../math/tools.h"
namespace hpp
{
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index d0f1825c670082c990a0ec7265209852e1e7cad1..70f26152604a39f76e543f1aa4f8fb0b1900201f 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -38,7 +38,7 @@
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/BVH/BV_fitter.h>
-#include <hpp/fcl/math/tools.h>
+#include "../math/tools.h"
namespace hpp
{
diff --git a/include/hpp/fcl/traversal/details/traversal.h b/src/traversal/details/traversal.h
similarity index 100%
rename from include/hpp/fcl/traversal/details/traversal.h
rename to src/traversal/details/traversal.h
diff --git a/src/traversal/traversal_node_base.cpp b/src/traversal/traversal_node_base.cpp
index a4bc4eac9dedfa7ecef2ede85ec0ed51ce8f6f68..f43b4770319cc9c0b333e7dc3ef956d17110a5a2 100644
--- a/src/traversal/traversal_node_base.cpp
+++ b/src/traversal/traversal_node_base.cpp
@@ -36,7 +36,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/traversal/traversal_node_base.h>
+#include "traversal_node_base.h"
#include <limits>
namespace hpp
@@ -97,7 +97,7 @@ DistanceTraversalNodeBase::~DistanceTraversalNodeBase()
{
}
- FCL_REAL DistanceTraversalNodeBase::BVTesting(int /*b1*/, int /*b2*/) const
+ FCL_REAL DistanceTraversalNodeBase::BVDistanceLowerBound(int /*b1*/, int /*b2*/) const
{
return std::numeric_limits<FCL_REAL>::max();
}
diff --git a/include/hpp/fcl/traversal/traversal_node_base.h b/src/traversal/traversal_node_base.h
similarity index 93%
rename from include/hpp/fcl/traversal/traversal_node_base.h
rename to src/traversal/traversal_node_base.h
index 918505de5e55d4f0678e8dad64603c96378559ba..2e5419598df8c78ae1d4dcfb6656350b11b3c027 100644
--- a/include/hpp/fcl/traversal/traversal_node_base.h
+++ b/src/traversal/traversal_node_base.h
@@ -98,16 +98,16 @@ public:
virtual ~CollisionTraversalNodeBase();
/// @brief BV test between b1 and b2
- virtual bool BVTesting(int b1, int b2) const = 0;
+ virtual bool BVDisjoints(int b1, int b2) const = 0;
/// BV test between b1 and b2
/// \param b1, b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
- virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0;
+ virtual bool BVDisjoints(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0;
/// @brief Leaf test between node b1 and b2, if they are both leafs
- virtual void leafTesting(int /*b1*/, int /*b2*/, FCL_REAL& /*sqrDistLowerBound*/) const
+ virtual void leafCollides(int /*b1*/, int /*b2*/, FCL_REAL& /*sqrDistLowerBound*/) const
{
throw std::runtime_error ("Not implemented");
}
@@ -139,10 +139,10 @@ public:
/// @brief BV test between b1 and b2
/// \return a lower bound of the distance between the two BV.
/// \note except for OBB, this method returns the distance.
- virtual FCL_REAL BVTesting(int b1, int b2) const;
+ virtual FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs
- virtual void leafTesting(int b1, int b2) const = 0;
+ virtual void leafComputeDistance(int b1, int b2) const = 0;
/// @brief Check whether the traversal can stop
virtual bool canStop(FCL_REAL c) const;
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/src/traversal/traversal_node_bvh_shape.h
similarity index 91%
rename from include/hpp/fcl/traversal/traversal_node_bvh_shape.h
rename to src/traversal/traversal_node_bvh_shape.h
index 0ae2c53c5f3a307c5f51e58cf268c01eee16557d..2ed8460b79f1f97f2c11403d87fc9228c74ea9a9 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/src/traversal/traversal_node_bvh_shape.h
@@ -42,8 +42,8 @@
#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/traversal/traversal_node_base.h>
-#include <hpp/fcl/traversal/details/traversal.h>
+#include "traversal_node_base.h"
+#include "details/traversal.h"
#include <hpp/fcl/BVH/BVH_model.h>
@@ -166,7 +166,7 @@ public:
}
/// @brief BV culling test in one BVTT node
- bool BVTesting(int b1, int /*b2*/) const
+ bool BVDisjoints(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
@@ -180,7 +180,7 @@ public:
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
/// @brief BV culling test in one BVTT node
- bool BVTesting(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
+ bool BVDisjoints(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_bv_tests++;
bool res;
@@ -195,7 +195,7 @@ public:
}
/// @brief Intersection testing between leaves (one triangle and one shape)
- void leafTesting(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
+ void leafCollides(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model1->getBV(b1);
@@ -327,7 +327,7 @@ public:
/// BV test between b1 and b2
/// \param b2 Bounding volumes to test,
- bool BVTesting(int /*b1*/, int b2) const
+ bool BVDisjoints(int /*b1*/, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
@@ -340,7 +340,7 @@ public:
/// \param b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
- bool BVTesting(int /*b1*/, int b2, FCL_REAL& sqrDistLowerBound) const
+ bool BVDisjoints(int /*b1*/, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_bv_tests++;
bool res;
@@ -355,7 +355,7 @@ public:
}
/// @brief Intersection testing between leaves (one shape and one triangle)
- void leafTesting(int /*b1*/, int b2, FCL_REAL& sqrDistLowerBound) const
+ void leafCollides(int /*b1*/, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node = this->model2->getBV(b2);
@@ -493,7 +493,7 @@ public:
}
/// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int b1, int /*b2*/) const
+ FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
{
return model1->getBV(b1).bv.distance(model2_bv);
}
@@ -541,7 +541,7 @@ public:
}
/// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const
{
return model1_bv.distance(model2->getBV(b2).bv);
}
@@ -573,7 +573,7 @@ public:
}
/// @brief Distance testing between leaves (one triangle and one shape)
- void leafTesting(int b1, int /*b2*/) const
+ void leafComputeDistance(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -620,7 +620,7 @@ namespace details
{
template<typename BV, typename S, typename GJKSolver>
-void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */,
+void meshShapeDistanceOrientedNodeleafComputeDistance(int b1, int /* b2 */,
const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices,
const Transform3f& tf1,
@@ -700,15 +700,15 @@ public:
{
}
- FCL_REAL BVTesting(int b1, int /*b2*/) const
+ FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
- details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
@@ -732,25 +732,25 @@ public:
{
}
- FCL_REAL BVTesting(int b1, int /*b2*/) const
+ FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
- details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
-template<typename S, typename NarrowPhaseSolver>
-class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
+class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S, GJKSolver>
{
public:
- MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
+ MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, GJKSolver>()
{
}
@@ -765,22 +765,22 @@ public:
}
- FCL_REAL BVTesting(int b1, int /*b2*/) const
+ FCL_REAL BVDistanceLowerBound(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
- details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ details::meshShapeDistanceOrientedNodeleafComputeDistance(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
/// @brief Traversal node for distance between shape and mesh
-template<typename S, typename BV, typename NarrowPhaseSolver>
+template<typename S, typename BV, typename GJKSolver>
class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
{
public:
@@ -796,7 +796,7 @@ public:
}
/// @brief Distance testing between leaves (one shape and one triangle)
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -835,14 +835,14 @@ public:
FCL_REAL rel_err;
FCL_REAL abs_err;
- const NarrowPhaseSolver* nsolver;
+ const GJKSolver* nsolver;
};
-template<typename S, typename NarrowPhaseSolver>
-class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
+class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS, GJKSolver>
{
public:
- ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>()
+ ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, GJKSolver>()
{
}
@@ -856,25 +856,25 @@ public:
{
}
- FCL_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
- details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
-template<typename S, typename NarrowPhaseSolver>
-class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
+class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS, GJKSolver>
{
public:
- ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>()
+ ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, GJKSolver>()
{
}
@@ -888,25 +888,25 @@ public:
{
}
- FCL_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
- details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
};
-template<typename S, typename NarrowPhaseSolver>
-class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
+class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS, GJKSolver>
{
public:
- ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
+ ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, GJKSolver>()
{
}
@@ -920,15 +920,15 @@ public:
{
}
- FCL_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
- details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ details::meshShapeDistanceOrientedNodeleafComputeDistance(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result));
}
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index 5fe97fa5a0eaf8b892e77971b24ca2854e8b02d2..c4e3fbf63d1b5a1899ac3f96696ee9d6b693035f 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -36,7 +36,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
+#include "traversal_node_bvhs.h"
namespace hpp
{
@@ -46,7 +46,7 @@ namespace fcl
namespace details
{
template<typename BV>
-static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
+static inline void meshDistanceOrientedNodeleafComputeDistance(int b1, int b2,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
Vec3f* vertices1, Vec3f* vertices2,
Triangle* tri_indices1, Triangle* tri_indices2,
@@ -152,15 +152,15 @@ void MeshDistanceTraversalNodeRSS::postprocess()
details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
-FCL_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
+FCL_REAL MeshDistanceTraversalNodeRSS::BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
-void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const
+void MeshDistanceTraversalNodeRSS::leafComputeDistance(int b1, int b2) const
{
- details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
request, *result);
}
@@ -180,15 +180,15 @@ void MeshDistanceTraversalNodekIOS::postprocess()
details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
-FCL_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
+FCL_REAL MeshDistanceTraversalNodekIOS::BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
-void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const
+void MeshDistanceTraversalNodekIOS::leafComputeDistance(int b1, int b2) const
{
- details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
request, *result);
}
@@ -208,15 +208,15 @@ void MeshDistanceTraversalNodeOBBRSS::postprocess()
details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result);
}
-FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
+FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
}
-void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
+void MeshDistanceTraversalNodeOBBRSS::leafComputeDistance(int b1, int b2) const
{
- details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ details::meshDistanceOrientedNodeleafComputeDistance(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
R, T, enable_statistics, num_leaf_tests,
request, *result);
}
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/src/traversal/traversal_node_bvhs.h
similarity index 93%
rename from include/hpp/fcl/traversal/traversal_node_bvhs.h
rename to src/traversal/traversal_node_bvhs.h
index 57e8a3f3576f8c705bf0669453811b6bbbe2afb0..f0ca5d16bcfa147f61cf389698eef3a20ab356e9 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/src/traversal/traversal_node_bvhs.h
@@ -40,14 +40,14 @@
#define HPP_FCL_TRAVERSAL_NODE_MESHES_H
#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/traversal/traversal_node_base.h>
+#include "traversal_node_base.h"
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/intersect.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
-#include <hpp/fcl/traversal/details/traversal.h>
+#include "details/traversal.h"
#include <boost/shared_array.hpp>
#include <boost/shared_ptr.hpp>
@@ -157,7 +157,7 @@ public:
}
/// @brief BV culling test in one BVTT node
- bool BVTesting(int b1, int b2) const
+ bool BVDisjoints(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
@@ -171,7 +171,7 @@ public:
/// \param b1, b2 Bounding volumes to test,
/// \retval sqrDistLowerBound square of a lower bound of the minimal
/// distance between bounding volumes.
- bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ bool BVDisjoints(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_bv_tests++;
if (RTIsIdentity)
@@ -200,7 +200,7 @@ public:
/// \note If the distance between objects is less than the security margin,
/// and the object are not colliding, the penetration depth is
/// negative.
- void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ void leafCollides(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -271,7 +271,7 @@ typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
namespace details
{
- template<typename BV> struct DistanceTraversalBVTesting_impl
+ template<typename BV> struct DistanceTraversalBVDistanceLowerBound_impl
{
static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
{
@@ -279,7 +279,7 @@ namespace details
}
};
- template<> struct DistanceTraversalBVTesting_impl<OBB>
+ template<> struct DistanceTraversalBVDistanceLowerBound_impl<OBB>
{
static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
{
@@ -361,10 +361,10 @@ public:
}
/// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
- return details::DistanceTraversalBVTesting_impl<BV>
+ return details::DistanceTraversalBVDistanceLowerBound_impl<BV>
::run (model1->getBV(b1), model2->getBV(b2));
}
@@ -397,7 +397,7 @@ public:
}
/// @brief Distance testing between leaves (two triangles)
- void leafTesting(int b1, int b2) const
+ void leafComputeDistance(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -457,9 +457,9 @@ public:
void postprocess();
- FCL_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
- void leafTesting(int b1, int b2) const;
+ void leafComputeDistance(int b1, int b2) const;
Matrix3f R;
Vec3f T;
@@ -475,9 +475,9 @@ public:
void postprocess();
- FCL_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
- void leafTesting(int b1, int b2) const;
+ void leafComputeDistance(int b1, int b2) const;
Matrix3f R;
Vec3f T;
@@ -492,11 +492,11 @@ public:
void postprocess();
- FCL_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVDistanceLowerBound(int b1, int b2) const;
- FCL_REAL BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
+ FCL_REAL BVDistanceLowerBound(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
- void leafTesting(int b1, int b2) const;
+ void leafComputeDistance(int b1, int b2) const;
Matrix3f R;
Vec3f T;
diff --git a/include/hpp/fcl/traversal/traversal_node_octree.h b/src/traversal/traversal_node_octree.h
similarity index 93%
rename from include/hpp/fcl/traversal/traversal_node_octree.h
rename to src/traversal/traversal_node_octree.h
index b9c47c07afb8f37ac7a8be75cb4a53e95e33a7e8..270fb06d8432f99d1dc06918c673aeb400976d2e 100644
--- a/include/hpp/fcl/traversal/traversal_node_octree.h
+++ b/src/traversal/traversal_node_octree.h
@@ -39,7 +39,7 @@
#define HPP_FCL_TRAVERSAL_NODE_OCTREE_H
#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/traversal/traversal_node_base.h>
+#include "traversal_node_base.h"
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/octree.h>
@@ -51,11 +51,11 @@ namespace fcl
{
/// @brief Algorithms for collision related with octree
-template<typename NarrowPhaseSolver>
+template<typename GJKSolver>
class OcTreeSolver
{
private:
- const NarrowPhaseSolver* solver;
+ const GJKSolver* solver;
mutable const CollisionRequest* crequest;
mutable const DistanceRequest* drequest;
@@ -64,7 +64,7 @@ private:
mutable DistanceResult* dresult;
public:
- OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_),
+ OcTreeSolver(const GJKSolver* solver_) : solver(solver_),
crequest(NULL),
drequest(NULL),
cresult(NULL),
@@ -890,7 +890,7 @@ private:
/// @brief Traversal node for octree collision
-template<typename NarrowPhaseSolver>
+template<typename GJKSolver>
class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -903,17 +903,17 @@ public:
otsolver = NULL;
}
- bool BVTesting(int, int) const
+ bool BVDisjoints(int, int) const
{
return false;
}
- bool BVTesting(int, int, FCL_REAL&) const
+ bool BVDisjoints(int, int, FCL_REAL&) const
{
return false;
}
- void leafTesting(int, int, FCL_REAL&) const
+ void leafCollides(int, int, FCL_REAL&) const
{
otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
}
@@ -923,11 +923,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for octree distance
-template<typename NarrowPhaseSolver>
+template<typename GJKSolver>
class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -940,17 +940,17 @@ public:
}
- FCL_REAL BVTesting(int, int) const
+ FCL_REAL BVDistanceLowerBound(int, int) const
{
return -1;
}
- bool BVTesting(int, int, FCL_REAL&) const
+ bool BVDistanceLowerBound(int, int, FCL_REAL&) const
{
return false;
}
- void leafTesting(int, int) const
+ void leafComputeDistance(int, int) const
{
otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
}
@@ -958,11 +958,11 @@ public:
const OcTree* model1;
const OcTree* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for shape-octree collision
-template<typename S, typename NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -975,17 +975,17 @@ public:
otsolver = NULL;
}
- bool BVTesting(int, int) const
+ bool BVDisjoints(int, int) const
{
return false;
}
- bool BVTesting(int, int, FCL_REAL&) const
+ bool BVDisjoints(int, int, FCL_REAL&) const
{
return false;
}
- void leafTesting(int, int, FCL_REAL&) const
+ void leafCollides(int, int, FCL_REAL&) const
{
otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
}
@@ -995,11 +995,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for octree-shape collision
-template<typename S, typename NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -1012,17 +1012,17 @@ public:
otsolver = NULL;
}
- bool BVTesting(int, int) const
+ bool BVDisjoints(int, int) const
{
return false;
}
- bool BVTesting(int, int, fcl::FCL_REAL&) const
+ bool BVDisjoints(int, int, fcl::FCL_REAL&) const
{
return false;
}
- void leafTesting(int, int, FCL_REAL&) const
+ void leafCollides(int, int, FCL_REAL&) const
{
otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
}
@@ -1032,11 +1032,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for shape-octree distance
-template<typename S, typename NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1048,12 +1048,12 @@ public:
otsolver = NULL;
}
- FCL_REAL BVTesting(int, int) const
+ FCL_REAL BVDistanceLowerBound(int, int) const
{
return -1;
}
- void leafTesting(int, int) const
+ void leafComputeDistance(int, int) const
{
otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
}
@@ -1061,11 +1061,11 @@ public:
const S* model1;
const OcTree* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for octree-shape distance
-template<typename S, typename NarrowPhaseSolver>
+template<typename S, typename GJKSolver>
class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1077,12 +1077,12 @@ public:
otsolver = NULL;
}
- FCL_REAL BVTesting(int, int) const
+ FCL_REAL BVDistanceLowerBound(int, int) const
{
return -1;
}
- void leafTesting(int, int) const
+ void leafComputeDistance(int, int) const
{
otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
}
@@ -1090,11 +1090,11 @@ public:
const OcTree* model1;
const S* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for mesh-octree collision
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV, typename GJKSolver>
class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -1107,17 +1107,17 @@ public:
otsolver = NULL;
}
- bool BVTesting(int, int) const
+ bool BVDisjoints(int, int) const
{
return false;
}
- bool BVTesting(int, int, FCL_REAL&) const
+ bool BVDisjoints(int, int, FCL_REAL&) const
{
return false;
}
- void leafTesting(int, int, FCL_REAL&) const
+ void leafCollides(int, int, FCL_REAL&) const
{
otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
}
@@ -1127,11 +1127,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for octree-mesh collision
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV, typename GJKSolver>
class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -1144,17 +1144,17 @@ public:
otsolver = NULL;
}
- bool BVTesting(int, int) const
+ bool BVDisjoints(int, int) const
{
return false;
}
- bool BVTesting(int, int, FCL_REAL&) const
+ bool BVDisjoints(int, int, FCL_REAL&) const
{
return false;
}
- void leafTesting(int, int, FCL_REAL&) const
+ void leafCollides(int, int, FCL_REAL&) const
{
otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
}
@@ -1164,11 +1164,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for mesh-octree distance
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV, typename GJKSolver>
class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1180,12 +1180,12 @@ public:
otsolver = NULL;
}
- FCL_REAL BVTesting(int, int) const
+ FCL_REAL BVDistanceLowerBound(int, int) const
{
return -1;
}
- void leafTesting(int, int) const
+ void leafComputeDistance(int, int) const
{
otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
}
@@ -1193,12 +1193,12 @@ public:
const BVHModel<BV>* model1;
const OcTree* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
/// @brief Traversal node for octree-mesh distance
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV, typename GJKSolver>
class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1210,12 +1210,12 @@ public:
otsolver = NULL;
}
- FCL_REAL BVTesting(int, int) const
+ FCL_REAL BVDistanceLowerBound(int, int) const
{
return -1;
}
- void leafTesting(int, int) const
+ void leafComputeDistance(int, int) const
{
otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
}
@@ -1223,7 +1223,7 @@ public:
const OcTree* model1;
const BVHModel<BV>* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver<GJKSolver>* otsolver;
};
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 9f236985d0501ee4f93748369c0c870ac4b31f92..58682c9f2531161eeb69a9b4b619313fbcaa716a 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -36,8 +36,8 @@
/** \author Jia Pan */
-#include <hpp/fcl/traversal/traversal_node_setup.h>
-#include <hpp/fcl/math/tools.h>
+#include "traversal_node_setup.h"
+#include "../math/tools.h"
namespace hpp
{
diff --git a/include/hpp/fcl/traversal/traversal_node_setup.h b/src/traversal/traversal_node_setup.h
similarity index 72%
rename from include/hpp/fcl/traversal/traversal_node_setup.h
rename to src/traversal/traversal_node_setup.h
index 0912c231d15a567b242911d9e8ee60780ff5fc87..d6d447fe429659863d64e771eeef10b5e18f880f 100644
--- a/include/hpp/fcl/traversal/traversal_node_setup.h
+++ b/src/traversal/traversal_node_setup.h
@@ -39,12 +39,12 @@
#ifndef HPP_FCL_TRAVERSAL_NODE_SETUP_H
#define HPP_FCL_TRAVERSAL_NODE_SETUP_H
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
-#include <hpp/fcl/traversal/traversal_node_shapes.h>
-#include <hpp/fcl/traversal/traversal_node_bvh_shape.h>
+#include "traversal_node_bvhs.h"
+#include "traversal_node_shapes.h"
+#include "traversal_node_bvh_shape.h"
#ifdef HPP_FCL_HAVE_OCTOMAP
-#include <hpp/fcl/traversal/traversal_node_octree.h>
+#include "traversal_node_octree.h"
#endif
#include <hpp/fcl/BVH/BVH_utility.h>
@@ -56,11 +56,11 @@ namespace fcl
#ifdef HPP_FCL_HAVE_OCTOMAP
/// @brief Initialize traversal node for collision between two octrees, given current object transform
-template<typename NarrowPhaseSolver>
-bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
+template<typename GJKSolver>
+bool initialize(OcTreeCollisionTraversalNode<GJKSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -77,11 +77,11 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance between two octrees, given current object transform
-template<typename NarrowPhaseSolver>
-bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
+template<typename GJKSolver>
+bool initialize(OcTreeDistanceTraversalNode<GJKSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -100,11 +100,11 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for collision between one shape and one octree, given current object transform
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(ShapeOcTreeCollisionTraversalNode<S, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -121,11 +121,11 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for collision between one octree and one shape, given current object transform
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(OcTreeShapeCollisionTraversalNode<S, GJKSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -142,11 +142,11 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance between one shape and one octree, given current object transform
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(ShapeOcTreeDistanceTraversalNode<S, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -165,11 +165,11 @@ bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance between one octree and one shape, given current object transform
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(OcTreeShapeDistanceTraversalNode<S, GJKSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -188,11 +188,11 @@ bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for collision between one mesh and one octree, given current object transform
-template<typename BV, typename NarrowPhaseSolver>
-bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
+template<typename BV, typename GJKSolver>
+bool initialize(MeshOcTreeCollisionTraversalNode<BV, GJKSolver>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -209,11 +209,11 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform
-template<typename BV, typename NarrowPhaseSolver>
-bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
+template<typename BV, typename GJKSolver>
+bool initialize(OcTreeMeshCollisionTraversalNode<BV, GJKSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -230,11 +230,11 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance between one mesh and one octree, given current object transform
-template<typename BV, typename NarrowPhaseSolver>
-bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
+template<typename BV, typename GJKSolver>
+bool initialize(MeshOcTreeDistanceTraversalNode<BV, GJKSolver>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -253,11 +253,11 @@ bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform
-template<typename BV, typename NarrowPhaseSolver>
-bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
+template<typename BV, typename GJKSolver>
+bool initialize(OcTreeMeshDistanceTraversalNode<BV, GJKSolver>& node,
const OcTree& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver<GJKSolver>* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -279,11 +279,11 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
/// @brief Initialize traversal node for collision between two geometric shapes, given current object transform
-template<typename S1, typename S2, typename NarrowPhaseSolver>
-bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
+template<typename S1, typename S2, typename GJKSolver>
+bool initialize(ShapeCollisionTraversalNode<S1, S2, GJKSolver>& node,
const S1& shape1, const Transform3f& tf1,
const S2& shape2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result)
{
node.model1 = &shape1;
@@ -298,11 +298,11 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform
-template<typename BV, typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
+template<typename BV, typename S, typename GJKSolver>
+bool initialize(MeshShapeCollisionTraversalNode<BV, S, GJKSolver>& node,
BVHModel<BV>& model1, Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result,
bool use_refit = false, bool refit_bottomup = false)
{
@@ -342,61 +342,15 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
return true;
}
-
-/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform
-template<typename S, typename BV, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- BVHModel<BV>& model2, Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- CollisionResult& result,
- bool use_refit = false, bool refit_bottomup = false)
-{
- if(model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- if(!tf2.isIdentity())
- {
- 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);
-
- tf2.setIdentity();
- }
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model1, tf1, node.model1_bv);
-
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
-
- node.result = &result;
-
- return true;
-}
-
/// @cond IGNORE
namespace details
{
-template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
-static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+template<typename BV, typename S, typename GJKSolver, template<typename, typename> class OrientedNode>
+static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, GJKSolver>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
@@ -424,44 +378,44 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(MeshShapeCollisionTraversalNodeOBB<S, GJKSolver>& node,
const BVHModel<OBB>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(MeshShapeCollisionTraversalNodeRSS<S, GJKSolver>& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(MeshShapeCollisionTraversalNodekIOS<S, GJKSolver>& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
}
/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, GJKSolver>& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result)
{
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
@@ -471,11 +425,11 @@ bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
/// @cond IGNORE
namespace details
{
-template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
-static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+template<typename S, typename BV, typename GJKSolver, template<typename, typename> class OrientedNode>
+static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
CollisionResult& result)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -499,52 +453,6 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
}
/// @endcond
-/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- const BVHModel<OBB>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- CollisionResult& result)
-{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
-}
-
-/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- CollisionResult& result)
-{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
-}
-
-/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- const BVHModel<kIOS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- CollisionResult& result)
-{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
-}
-
-/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const Transform3f& tf1,
- const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- CollisionResult& result)
-{
- return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, result);
-}
-
-
-
/// @brief Initialize traversal node for collision between two meshes, given the current transforms
template<typename BV>
@@ -636,11 +544,11 @@ bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
}
/// @brief Initialize traversal node for distance between two geometric shapes
-template<typename S1, typename S2, typename NarrowPhaseSolver>
-bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
+template<typename S1, typename S2, typename GJKSolver>
+bool initialize(ShapeDistanceTraversalNode<S1, S2, GJKSolver>& node,
const S1& shape1, const Transform3f& tf1,
const S2& shape2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -742,11 +650,11 @@ bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
DistanceResult& result);
/// @brief Initialize traversal node for distance computation between one mesh and one shape, given the current transforms
-template<typename BV, typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
+template<typename BV, typename S, typename GJKSolver>
+bool initialize(MeshShapeDistanceTraversalNode<BV, S, GJKSolver>& node,
BVHModel<BV>& model1, Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result,
bool use_refit = false, bool refit_bottomup = false)
@@ -789,11 +697,11 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance computation between one shape and one mesh, given the current transforms
-template<typename S, typename BV, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
+template<typename S, typename BV, typename GJKSolver>
+bool initialize(ShapeMeshDistanceTraversalNode<S, BV, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
BVHModel<BV>& model2, Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result,
bool use_refit = false, bool refit_bottomup = false)
@@ -839,11 +747,11 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
namespace details
{
-template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
-static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+template<typename BV, typename S, typename GJKSolver, template<typename, typename> class OrientedNode>
+static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, GJKSolver>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -870,11 +778,11 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
/// @endcond
/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for RSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(MeshShapeDistanceTraversalNodeRSS<S, GJKSolver>& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -882,11 +790,11 @@ bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(MeshShapeDistanceTraversalNodekIOS<S, GJKSolver>& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -894,11 +802,11 @@ bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, GJKSolver>& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -908,11 +816,11 @@ bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
namespace details
{
-template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
-static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+template<typename S, typename BV, typename GJKSolver, template<typename, typename> class OrientedNode>
+static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -941,11 +849,11 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhas
/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -953,11 +861,11 @@ bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<kIOS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -965,11 +873,11 @@ bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
}
/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+template<typename S, typename GJKSolver>
+bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, GJKSolver>& node,
const S& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const GJKSolver* nsolver,
const DistanceRequest& request,
DistanceResult& result)
{
diff --git a/include/hpp/fcl/traversal/traversal_node_shapes.h b/src/traversal/traversal_node_shapes.h
similarity index 90%
rename from include/hpp/fcl/traversal/traversal_node_shapes.h
rename to src/traversal/traversal_node_shapes.h
index 5180e9dcb43fe097e3be7730be259ee6e2feea8a..17be1e73f8b19d74ef01dd5709e81140cffd7970 100644
--- a/include/hpp/fcl/traversal/traversal_node_shapes.h
+++ b/src/traversal/traversal_node_shapes.h
@@ -40,7 +40,7 @@
#define HPP_FCL_TRAVERSAL_NODE_SHAPES_H
#include <hpp/fcl/collision_data.h>
-#include <hpp/fcl/traversal/traversal_node_base.h>
+#include "traversal_node_base.h"
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/BV/BV.h>
@@ -53,7 +53,7 @@ namespace fcl
/// @brief Traversal node for collision between two shapes
-template<typename S1, typename S2, typename NarrowPhaseSolver>
+template<typename S1, typename S2, typename GJKSolver>
class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -67,19 +67,19 @@ public:
}
/// @brief BV culling test in one BVTT node
- bool BVTesting(int, int) const
+ bool BVDisjoints(int, int) const
{
return false;
}
/// @brief BV culling test in one BVTT node
- bool BVTesting(int, int, FCL_REAL&) const
+ bool BVDisjoints(int, int, FCL_REAL&) const
{
throw std::runtime_error ("Not implemented");
}
/// @brief Intersection testing between leaves (two shapes)
- void leafTesting(int, int, FCL_REAL&) const
+ void leafCollides(int, int, FCL_REAL&) const
{
bool is_collision = false;
if(request.enable_contact)
@@ -111,11 +111,11 @@ public:
const S1* model1;
const S2* model2;
- const NarrowPhaseSolver* nsolver;
+ const GJKSolver* nsolver;
};
/// @brief Traversal node for distance between two shapes
-template<typename S1, typename S2, typename NarrowPhaseSolver>
+template<typename S1, typename S2, typename GJKSolver>
class ShapeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -128,13 +128,13 @@ public:
}
/// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int, int) const
+ FCL_REAL BVDistanceLowerBound(int, int) const
{
return -1; // should not be used
}
/// @brief Distance testing between leaves (two shapes)
- void leafTesting(int, int) const
+ void leafComputeDistance(int, int) const
{
FCL_REAL distance;
Vec3f closest_p1, closest_p2, normal;
@@ -147,7 +147,7 @@ public:
const S1* model1;
const S2* model2;
- const NarrowPhaseSolver* nsolver;
+ const GJKSolver* nsolver;
};
}
diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
index d041c158c787f9db447323eec11fb5d60f02e3db..dd2e8d2857a93d271297fea05e939b92b1e29c81 100644
--- a/src/traversal/traversal_recurse.cpp
+++ b/src/traversal/traversal_recurse.cpp
@@ -36,7 +36,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/traversal/traversal_recurse.h>
+#include "traversal_recurse.h"
#include <vector>
@@ -54,12 +54,12 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
{
updateFrontList(front_list, b1, b2);
- if(node->BVTesting(b1, b2, sqrDistLowerBound)) return;
- node->leafTesting(b1, b2, sqrDistLowerBound);
+ if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return;
+ node->leafCollides(b1, b2, sqrDistLowerBound);
return;
}
- if(node->BVTesting(b1, b2, sqrDistLowerBound)) {
+ if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
updateFrontList(front_list, b1, b2);
return;
}
@@ -118,11 +118,11 @@ void collisionNonRecurse(CollisionTraversalNodeBase* node,
updateFrontList(front_list, a, b);
// TODO should we test the BVs ?
- //if(node->BVTesting(a, b, sdlb)) {
+ //if(node->BVDijsoints(a, b, sdlb)) {
//if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
//continue;
//}
- node->leafTesting(a, b, sdlb);
+ node->leafCollides(a, b, sdlb);
if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
if (node->canStop() && !front_list) return;
continue;
@@ -134,7 +134,7 @@ void collisionNonRecurse(CollisionTraversalNodeBase* node,
// }
// Check the BV
- if(node->BVTesting(a, b, sdlb)) {
+ if(node->BVDisjoints(a, b, sdlb)) {
if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
updateFrontList(front_list, a, b);
continue;
@@ -169,7 +169,7 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontLi
{
updateFrontList(front_list, b1, b2);
- node->leafTesting(b1, b2);
+ node->leafComputeDistance(b1, b2);
return;
}
@@ -190,8 +190,8 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontLi
c2 = node->getSecondRightChild(b2);
}
- FCL_REAL d1 = node->BVTesting(a1, a2);
- FCL_REAL d2 = node->BVTesting(c1, c2);
+ FCL_REAL d1 = node->BVDistanceLowerBound(a1, a2);
+ FCL_REAL d2 = node->BVDistanceLowerBound(c1, c2);
if(d2 < d1)
{
@@ -298,7 +298,7 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFr
{
updateFrontList(front_list, min_test.b1, min_test.b2);
- node->leafTesting(min_test.b1, min_test.b2);
+ node->leafComputeDistance(min_test.b1, min_test.b2);
}
else if(bvtq.full())
{
@@ -317,11 +317,11 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFr
int c2 = node->getFirstRightChild(min_test.b1);
bvt1.b1 = c1;
bvt1.b2 = min_test.b2;
- bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2);
+ bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
bvt2.b1 = c2;
bvt2.b2 = min_test.b2;
- bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2);
+ bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
}
else
{
@@ -329,11 +329,11 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFr
int c2 = node->getSecondRightChild(min_test.b2);
bvt1.b1 = min_test.b1;
bvt1.b2 = c1;
- bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2);
+ bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
bvt2.b1 = min_test.b1;
bvt2.b2 = c2;
- bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2);
+ bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
}
bvtq.push(bvt1);
@@ -378,7 +378,7 @@ void propagateBVHFrontListCollisionRecurse
}
else
{
- if(!node->BVTesting(b1, b2, sqrDistLowerBound)) {
+ if(!node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
front_iter->valid = false;
if(node->firstOverSecond(b1, b2)) {
int c1 = node->getFirstLeftChild(b1);
diff --git a/include/hpp/fcl/traversal/traversal_recurse.h b/src/traversal/traversal_recurse.h
similarity index 96%
rename from include/hpp/fcl/traversal/traversal_recurse.h
rename to src/traversal/traversal_recurse.h
index b93fd55af9da9cc6b2f35bce84de79d8c84424a1..f5a4e58e90f835286497ad81102aff23a3fe01af 100644
--- a/include/hpp/fcl/traversal/traversal_recurse.h
+++ b/src/traversal/traversal_recurse.h
@@ -39,8 +39,8 @@
#ifndef HPP_FCL_TRAVERSAL_RECURSE_H
#define HPP_FCL_TRAVERSAL_RECURSE_H
-#include <hpp/fcl/traversal/traversal_node_base.h>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
+#include "traversal_node_base.h"
+#include "traversal_node_bvhs.h"
#include <hpp/fcl/BVH/BVH_front.h>
#include <queue>
diff --git a/test/benchmark.cpp b/test/benchmark.cpp
index f545b7df91511b5b976176993beb8b068482d7ad..f3a16334fcee0cec289ae72ec9075c927943fec3 100644
--- a/test/benchmark.cpp
+++ b/test/benchmark.cpp
@@ -14,8 +14,8 @@
// received a copy of the GNU Lesser General Public License along with
// hpp-fcl. If not, see <http://www.gnu.org/licenses/>.
-#include <hpp/fcl/traversal/traversal_node_setup.h>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
+#include "../src/traversal/traversal_node_setup.h"
+#include "../src/traversal/traversal_node_bvhs.h"
#include <../src/collision_node.h>
#include "utility.h"
#include "fcl_resources/config.h"
diff --git a/test/box_box_distance.cpp b/test/box_box_distance.cpp
index df295a9fadb4ac9aff7eba9f9175fe9611137140..90d090e30f2c0853ef2bc0449536cf039a3d87ba 100644
--- a/test/box_box_distance.cpp
+++ b/test/box_box_distance.cpp
@@ -42,6 +42,7 @@
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include <cmath>
+#include <iostream>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision.h>
diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp
index ee2a063aab2098e2ebdbe2c62da3419fd0743aa6..7dda5376b31576e3fefee5dadbe8c2232d810841 100644
--- a/test/capsule_capsule.cpp
+++ b/test/capsule_capsule.cpp
@@ -43,6 +43,7 @@
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include <cmath>
+#include <iostream>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision.h>
diff --git a/test/collision.cpp b/test/collision.cpp
index e2e791d83765c316eb45ebbb9c28da69560c5e51..d28452d9eb2afaa7e7b2aa6326dac7da466e0404 100644
--- a/test/collision.cpp
+++ b/test/collision.cpp
@@ -46,8 +46,8 @@
#include <boost/assign/list_of.hpp>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
+#include "../src/traversal/traversal_node_bvhs.h"
+#include "../src/traversal/traversal_node_setup.h"
#include <../src/collision_node.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/BV/BV.h>
diff --git a/test/distance.cpp b/test/distance.cpp
index 94922236c5f892456d757fba37542df0cd81e436..a7457eb6c6329486c181c00843488b6b3147a1aa 100644
--- a/test/distance.cpp
+++ b/test/distance.cpp
@@ -40,8 +40,8 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
+#include "../src/traversal/traversal_node_bvhs.h"
+#include "../src/traversal/traversal_node_setup.h"
#include <../src/collision_node.h>
#include "utility.h"
#include <boost/timer.hpp>
diff --git a/test/frontlist.cpp b/test/frontlist.cpp
index 799b0c4a1c241d283b929450addc4bf9c2f43202..b2e9f30039864fdf7312afed9a91df5e05ad7d03 100644
--- a/test/frontlist.cpp
+++ b/test/frontlist.cpp
@@ -41,8 +41,8 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include <hpp/fcl/traversal/traversal_node_bvhs.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
+#include "../src/traversal/traversal_node_bvhs.h"
+#include "../src/traversal/traversal_node_setup.h"
#include <../src/collision_node.h>
#include "utility.h"
diff --git a/test/geometric_shapes.cpp b/test/geometric_shapes.cpp
index 3d48154deab50f68d82ddfdda9be962d54c02aba..1c621f6d8b35c4c404dae9185c671a067d802079 100644
--- a/test/geometric_shapes.cpp
+++ b/test/geometric_shapes.cpp
@@ -46,7 +46,7 @@
#include <hpp/fcl/distance.h>
#include "utility.h"
#include <iostream>
-#include <hpp/fcl/math/tools.h>
+#include "../src/math/tools.h"
using namespace hpp::fcl;
diff --git a/test/gjk.cpp b/test/gjk.cpp
index 9ae852de0587454b24d7a968fe1532b7fd9bcc69..b02436d16a301d9df43b52e28c60333cfc931165 100644
--- a/test/gjk.cpp
+++ b/test/gjk.cpp
@@ -43,7 +43,7 @@
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes.h>
-#include <hpp/fcl/math/tools.h>
+#include "../src/math/tools.h"
using hpp::fcl::GJKSolver;
using hpp::fcl::TriangleP;
diff --git a/test/math.cpp b/test/math.cpp
index ef7d49c2b42b5aca845b5a15fb3858b15c961e1b..33845b0a73196805496f3e4591e3ad6dc84752b4 100644
--- a/test/math.cpp
+++ b/test/math.cpp
@@ -43,7 +43,7 @@
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/intersect.h>
-#include <hpp/fcl/math/tools.h>
+#include "../src/math/tools.h"
using namespace hpp::fcl;
diff --git a/test/utility.cpp b/test/utility.cpp
index e5e54baefff8ba545b6c08626e04fe312fb81efa..e3e0082b9ef45c4fd021e252182f5f413bd4dcbd 100644
--- a/test/utility.cpp
+++ b/test/utility.cpp
@@ -4,6 +4,7 @@
#include <cstdio>
#include <cstddef>
#include <fstream>
+#include <iostream>
namespace hpp
{