diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9ed03b283ed9dbd0959dd1b21efe1bb3629ca66e..4100a81826c6cc209314953baae9794f36e428a1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -132,24 +132,12 @@ 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
include/hpp/fcl/BVH/BVH_model.h
- include/hpp/fcl/BVH/BV_fitter.h
include/hpp/fcl/BVH/BVH_front.h
include/hpp/fcl/BVH/BVH_utility.h
- include/hpp/fcl/intersect.h
include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h
include/hpp/fcl/octree.h
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index c720f37a616f53625392fdc1ba04dde4cf9eb069..72954a64436a08f7d2bf41a19e7d6b3ba6664a67 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -39,7 +39,7 @@
#define HPP_FCL_AABB_H
#include <stdexcept>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
namespace hpp
{
@@ -81,6 +81,20 @@ public:
{
}
+ /// @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 +113,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 +142,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 +176,29 @@ 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
+ /// @}
+
+ /// @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 +246,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/BV.h b/include/hpp/fcl/BV/BV.h
index bb42f7fbcd6999b6d8c1bdbd3e4d7bcc834728da..3f03df66c96bf4e68105ecf237e00bc1aff3b66a 100644
--- a/include/hpp/fcl/BV/BV.h
+++ b/include/hpp/fcl/BV/BV.h
@@ -125,7 +125,7 @@ class Converter<RSS, OBB>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
- bv2.extent.noalias() = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
+ bv2.extent.noalias() = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
@@ -168,9 +168,9 @@ public:
bv2.Tr = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
- bv2.r = bv1.extent[2];
- bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
- bv2.l[1] = 2 * (bv1.extent[1] - bv2.r);
+ bv2.radius = bv1.extent[2];
+ bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
+ bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
}
};
@@ -183,9 +183,9 @@ public:
bv2.Tr.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
- bv2.r = bv1.r;
- bv2.l[0] = bv1.l[0];
- bv2.l[1] = bv1.l[1];
+ bv2.radius = bv1.radius;
+ bv2.length[0] = bv1.length[0];
+ bv2.length[1] = bv1.length[1];
}
};
@@ -232,9 +232,9 @@ public:
}
Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
- bv2.r = extent[id[2]];
- bv2.l[0] = (extent[id[0]] - bv2.r) * 2;
- bv2.l[1] = (extent[id[1]] - bv2.r) * 2;
+ bv2.radius = extent[id[2]];
+ bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
+ bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 08dbbf8bd1ba82f1f0ed1208d7e9bcbc277c1912..e7fc637418af63b040efc47c7ff1427c77dda8ed 100644
--- a/include/hpp/fcl/BV/BV_node.h
+++ b/include/hpp/fcl/BV/BV_node.h
@@ -39,7 +39,7 @@
#ifndef HPP_FCL_BV_NODE_H
#define HPP_FCL_BV_NODE_H
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <hpp/fcl/BV/BV.h>
#include <iostream>
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index 437f65957ff952380157b893a705782accd70999..5dee5cb1824ab8dd648b07fd13a6062523c884af 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -38,7 +38,7 @@
#ifndef HPP_FCL_OBB_H
#define HPP_FCL_OBB_H
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
namespace hpp
{
@@ -60,6 +60,9 @@ 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 +74,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 +90,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 +125,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..36c97e6143d73c530f31e31f124ebdf7b27d6277 100644
--- a/include/hpp/fcl/BV/OBBRSS.h
+++ b/include/hpp/fcl/BV/OBBRSS.h
@@ -59,6 +59,12 @@ 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 +80,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 +110,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,24 +145,6 @@ 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
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index a2c0111da817b431e877aeb690a2f0533678d8b3..b626e27edf8561a1c14211d2bffbc9be73d1a62c 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -39,7 +39,7 @@
#define HPP_FCL_RSS_H
#include <stdexcept>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <boost/math/constants/constants.hpp>
namespace hpp
@@ -60,10 +60,14 @@ public:
Vec3f Tr;
/// @brief Side lengths of rectangle
- FCL_REAL l[2];
+ FCL_REAL length[2];
/// @brief Radius of sphere summed with rectangle to form RSS
- FCL_REAL r;
+ FCL_REAL radius;
+
+
+ /// @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,45 +97,49 @@ 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(length[0] * length[0] + length[1] * length[1]) + 2 * radius);
+ }
+
+ /// @brief The RSS center
+ inline const Vec3f& center() const
+ {
+ return Tr;
+ }
+
/// @brief Width of the RSS
inline FCL_REAL width() const
{
- return l[0] + 2 * r;
+ return length[0] + 2 * radius;
}
/// @brief Height of the RSS
inline FCL_REAL height() const
{
- return l[1] + 2 * r;
+ return length[1] + 2 * radius;
}
/// @brief Depth of the RSS
inline FCL_REAL depth() const
{
- return 2 * r;
+ return 2 * radius;
}
/// @brief Volume of the RSS
inline FCL_REAL volume() const
{
- return (l[0] * l[1] * 2 * r + 4 * boost::math::constants::pi<FCL_REAL>() * r * r * r);
+ return (length[0] * length[1] * 2 * radius + 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius);
}
- /// @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
@@ -160,4 +161,4 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
} // namespace hpp
-#endif
+#endif
\ No newline at end of file
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 43c6db0972d0a52e58b45bee08b82be8a1765c5b..61e43d90244dec4fab97de79c62992af18364ee6 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
namespace hpp
{
@@ -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 40624e8b3c5229df16481313e37add583398f8d3..66005904e48a6548a2c5345213c6c5e5929e5dd8 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -41,8 +41,8 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/BV_node.h>
-#include <hpp/fcl/BVH/BV_splitter.h>
-#include <hpp/fcl/BVH/BV_fitter.h>
+#include "../../src/BVH/BV_splitter.h"
+#include "../../src/BVH/BV_fitter.h"
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
@@ -252,10 +252,10 @@ class BVHModel : public BVHModelBase
public:
/// @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<BVFitter<BV> > bv_fitter;
/// @brief Constructing an empty BVH
BVHModel() : BVHModelBase (),
diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h
index 700e3234233199340514c8e2a90de0d5396ad8b8..c17759919ae445a4e69df4b7b75c12ce6c83a8a4 100644
--- a/include/hpp/fcl/collision.h
+++ b/include/hpp/fcl/collision.h
@@ -39,7 +39,7 @@
#ifndef HPP_FCL_COLLISION_H
#define HPP_FCL_COLLISION_H
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision_data.h>
@@ -52,15 +52,13 @@ namespace fcl
/// returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
/// performs the collision between them.
/// Return value is the number of contacts generated between the two objects.
-
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
- const CollisionRequest& request,
- CollisionResult& result);
+ const CollisionRequest& request, CollisionResult& result);
+/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&)
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
- const CollisionRequest& request,
- CollisionResult& result);
+ const CollisionRequest& request, CollisionResult& result);
}
} // namespace hpp
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index ded180506448b294d203d18664626df094d95b79..d6278c2e07acd5e531eb9611e83834b0395922c9 100644
--- a/include/hpp/fcl/collision_data.h
+++ b/include/hpp/fcl/collision_data.h
@@ -41,7 +41,7 @@
#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <vector>
#include <set>
#include <limits>
diff --git a/include/hpp/fcl/collision_func_matrix.h b/include/hpp/fcl/collision_func_matrix.h
index af0355bd5849bf2ddb3044f44c7119c5247e75d8..892ea4013d870af3a8be510593454934f3931f6f 100644
--- a/include/hpp/fcl/collision_func_matrix.h
+++ b/include/hpp/fcl/collision_func_matrix.h
@@ -42,6 +42,7 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision_data.h>
+#include <hpp/fcl/narrowphase/narrowphase.h>
namespace hpp
{
@@ -49,7 +50,7 @@ namespace fcl
{
/// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface
-template<typename GJKSolver>
+
struct CollisionFunctionMatrix
{
/// @brief the uniform call interface for collision: for collision, we need know
diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h
index 7ef4d29412092be70cf8286af30a1e8676f734ec..3d3c3beab99fbb660b46e55b09fa37d4e47f2616 100644
--- a/include/hpp/fcl/data_types.h
+++ b/include/hpp/fcl/data_types.h
@@ -41,16 +41,37 @@
#include <cstddef>
#include <boost/cstdint.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+
+namespace hpp
+{
+
+#ifdef HPP_FCL_HAVE_OCTOMAP
+ #define OCTOMAP_VERSION_AT_LEAST(x,y,z) \
+ (OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \
+ (OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \
+ OCTOMAP_PATCH_VERSION >= z))))
+
+ #define OCTOMAP_VERSION_AT_MOST(x,y,z) \
+ (OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \
+ (OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \
+ OCTOMAP_PATCH_VERSION <= z))))
+#endif // HPP_FCL_HAVE_OCTOMAP
+}
+
namespace hpp
{
namespace fcl
{
-
typedef double FCL_REAL;
typedef boost::uint64_t FCL_INT64;
typedef boost::int64_t FCL_UINT64;
typedef boost::uint32_t FCL_UINT32;
typedef boost::int32_t FCL_INT32;
+typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
+typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
/// @brief Triangle with 3 indices for points
class Triangle
@@ -90,4 +111,5 @@ private:
} // namespace hpp
+
#endif
diff --git a/include/hpp/fcl/distance.h b/include/hpp/fcl/distance.h
index 87216ce6df8a1621f7bf885f94fd243ece3c64e0..d04d2c8224abfdfc1cc25f33d33fb7dbf4929780 100644
--- a/include/hpp/fcl/distance.h
+++ b/include/hpp/fcl/distance.h
@@ -48,14 +48,12 @@ namespace fcl
/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them.
/// Return value is the minimum distance generated between the two objects.
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result);
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
- const DistanceRequest& request, DistanceResult& result);
-
+/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&)
FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result);
-
}
} // namespace hpp
diff --git a/include/hpp/fcl/distance_func_matrix.h b/include/hpp/fcl/distance_func_matrix.h
index 89ee2d549135e2e62320847a4a75e0ae5af0619b..1f0ea6d26e573f065a6a97cc6552fff40ba31a20 100644
--- a/include/hpp/fcl/distance_func_matrix.h
+++ b/include/hpp/fcl/distance_func_matrix.h
@@ -40,6 +40,7 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision_data.h>
+#include <hpp/fcl/narrowphase/narrowphase.h>
namespace hpp
{
@@ -47,7 +48,6 @@ namespace fcl
{
/// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface
-template<typename GJKSolver>
struct DistanceFunctionMatrix
{
/// @brief the uniform call interface for distance: for distance, we need know
diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index ef409169f59c8011c755f8c28a327d71b31b81e1..dd6e9f4cbaeb197d17742d7bfef4adc520b712e7 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -39,7 +39,7 @@
#ifndef HPP_FCL_TRANSFORM_H
#define HPP_FCL_TRANSFORM_H
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <boost/thread/mutex.hpp>
namespace hpp
diff --git a/include/hpp/fcl/math/types.h b/include/hpp/fcl/math/types.h
index f6788e46aa0dd0df8dcfba057d3201c74ceb01a8..ec431e7a92b59d788dc4582b7b84d25e3815696f 100644
--- a/include/hpp/fcl/math/types.h
+++ b/include/hpp/fcl/math/types.h
@@ -38,50 +38,9 @@
#ifndef HPP_FCL_MATH_TYPES_H
#define HPP_FCL_MATH_TYPES_H
-#include <hpp/fcl/data_types.h>
+# warning "This file is deprecated. Include <hpp/fcl/data_types.h> instead."
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
-namespace hpp
-{
-
-#ifdef HPP_FCL_HAVE_OCTOMAP
- #define OCTOMAP_VERSION_AT_LEAST(x,y,z) \
- (OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \
- (OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \
- OCTOMAP_PATCH_VERSION >= z))))
-
- #define OCTOMAP_VERSION_AT_MOST(x,y,z) \
- (OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \
- (OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \
- OCTOMAP_PATCH_VERSION <= z))))
-#endif // HPP_FCL_HAVE_OCTOMAP
-}
-
-namespace hpp
-{
-namespace fcl
-{
- typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
- typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
-
-/// @brief Class for variance matrix in 3d
-class Variance3f
-{
-public:
- /// @brief Variation matrix
- Matrix3f Sigma;
-
- /// @brief Variations along the eign axes
- Matrix3f::Scalar sigma[3];
-
- /// @brief Eigen axes of the variation matrix
- Vec3f axis[3];
-
-};
-
-}
-} // namespace hpp
+// List of equivalent includes.
+# include <hpp/fcl/data_types.h>
#endif
\ No newline at end of file
diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/hpp/fcl/mesh_loader/loader.h
index df4c6f1da075dab267f4bcb09e66a8736fb325a0..d72e5c6f48a7a5bb7e0d151655902bc2b1234d98 100644
--- a/include/hpp/fcl/mesh_loader/loader.h
+++ b/include/hpp/fcl/mesh_loader/loader.h
@@ -40,7 +40,7 @@
#include <boost/shared_ptr.hpp>
#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/config.hh>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <hpp/fcl/collision_object.h>
namespace hpp
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index cb70da9f4d07c22e7b981d09d16b30b2f52c9b32..4e75a2c8471b4f88207281281379fb0bf391ddf0 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -42,7 +42,7 @@
#include <boost/math/constants/constants.hpp>
#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <string.h>
namespace hpp
@@ -149,6 +149,8 @@ class Capsule : public ShapeBase
public:
Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
+ lz = lz_;
+ HalfLength = lz_/2;
}
/// @brief Radius of capsule
@@ -157,6 +159,9 @@ public:
/// @brief Length along z axis
FCL_REAL lz;
+ /// @brief Half Length along z axis
+ FCL_REAL HalfLength;
+
/// @brief Compute AABB
void computeLocalAABB();
@@ -189,6 +194,8 @@ class Cone : public ShapeBase
public:
Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
+ lz = lz_;
+ HalfLength = lz_/2;
}
/// @brief Radius of the cone
@@ -197,6 +204,9 @@ public:
/// @brief Length along z axis
FCL_REAL lz;
+ /// @brief Half Length along z axis
+ FCL_REAL HalfLength;
+
/// @brief Compute AABB
void computeLocalAABB();
@@ -231,8 +241,9 @@ class Cylinder : public ShapeBase
public:
Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
{
+ lz = lz_;
+ HalfLength = lz_/2;
}
-
/// @brief Radius of the cylinder
FCL_REAL radius;
@@ -240,6 +251,9 @@ public:
/// @brief Length along z axis
FCL_REAL lz;
+ /// @brief Half Length along z axis
+ FCL_REAL HalfLength;
+
/// @brief Compute AABB
void computeLocalAABB();
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..ccd4ba8734e6160860d8f7b97387ac957c6d711e 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
@@ -844,8 +844,8 @@ bool RSS::overlap(const RSS& other) const
/// Now compute R1'R2
Matrix3f R (axes.transpose() * other.axes);
- FCL_REAL dist = rectDistance(R, T, l, other.l);
- return (dist <= (r + other.r));
+ FCL_REAL dist = rectDistance(R, T, length, other.length);
+ return (dist <= (radius + other.radius));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
@@ -859,8 +859,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
- FCL_REAL dist = rectDistance(R, T, b1.l, b2.l);
- return (dist <= (b1.r + b2.r));
+ FCL_REAL dist = rectDistance(R, T, b1.length, b2.length);
+ return (dist <= (b1.radius + b2.radius));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
@@ -876,7 +876,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
- FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
+ FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius;
if (dist <= 0) return true;
sqrDistLowerBound = dist * dist;
return false;
@@ -893,28 +893,28 @@ bool RSS::contain(const Vec3f& p) const
Vec3f proj(proj0, proj1, proj2);
/// projection is within the rectangle
- if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
+ if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0))
{
- return (abs_proj2 < r);
+ return (abs_proj2 < radius);
}
- else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
+ else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1])))
{
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(proj0, y, 0);
- return ((proj - v).squaredNorm() < r * r);
+ return ((proj - v).squaredNorm() < radius * radius);
}
- else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
+ else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0])))
{
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL x = (proj0 > 0) ? length[0] : 0;
Vec3f v(x, proj1, 0);
- return ((proj - v).squaredNorm() < r * r);
+ return ((proj - v).squaredNorm() < radius * radius);
}
else
{
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL x = (proj0 > 0) ? length[0] : 0;
+ FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(x, y, 0);
- return ((proj - v).squaredNorm() < r * r);
+ return ((proj - v).squaredNorm() < radius * radius);
}
}
@@ -928,99 +928,99 @@ RSS& RSS::operator += (const Vec3f& p)
Vec3f proj(proj0, proj1, proj2);
// projection is within the rectangle
- if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
+ if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0))
{
- if(abs_proj2 < r)
+ if(abs_proj2 < radius)
; // do nothing
else
{
- r = 0.5 * (r + abs_proj2); // enlarge the r
+ radius = 0.5 * (radius + abs_proj2); // enlarge the r
// change RSS origin position
if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
+ Tr[2] += 0.5 * (abs_proj2 - radius);
else
- Tr[2] -= 0.5 * (abs_proj2 - r);
+ Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
- else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
+ else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1])))
{
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(proj0, y, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
- if(new_r_sqr < r * r)
+ if(new_r_sqr < radius * radius)
; // do nothing
else
{
- if(abs_proj2 < r)
+ if(abs_proj2 < radius)
{
- FCL_REAL delta_y = - std::sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
- l[1] += delta_y;
+ FCL_REAL delta_y = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y);
+ length[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
}
else
{
FCL_REAL delta_y = fabs(proj1 - y);
- l[1] += delta_y;
+ length[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
+ Tr[2] += 0.5 * (abs_proj2 - radius);
else
- Tr[2] -= 0.5 * (abs_proj2 - r);
+ Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
}
- else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
+ else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0])))
{
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL x = (proj0 > 0) ? length[0] : 0;
Vec3f v(x, proj1, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
- if(new_r_sqr < r * r)
+ if(new_r_sqr < radius * radius)
; // do nothing
else
{
- if(abs_proj2 < r)
+ if(abs_proj2 < radius)
{
- FCL_REAL delta_x = - std::sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
- l[0] += delta_x;
+ FCL_REAL delta_x = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x);
+ length[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
}
else
{
FCL_REAL delta_x = fabs(proj0 - x);
- l[0] += delta_x;
+ length[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
+ Tr[2] += 0.5 * (abs_proj2 - radius);
else
- Tr[2] -= 0.5 * (abs_proj2 - r);
+ Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
}
else
{
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL x = (proj0 > 0) ? length[0] : 0;
+ FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(x, y, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
- if(new_r_sqr < r * r)
+ if(new_r_sqr < radius * radius)
; // do nothing
else
{
- if(abs_proj2 < r)
+ if(abs_proj2 < radius)
{
FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2);
- FCL_REAL delta_diag = - std::sqrt(r * r - proj2 * proj2) + diag;
+ FCL_REAL delta_diag = - std::sqrt(radius * radius - proj2 * proj2) + diag;
FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
- l[0] += delta_x;
- l[1] += delta_y;
+ length[0] += delta_x;
+ length[1] += delta_y;
if(proj0 < 0 && proj1 < 0)
{
@@ -1033,8 +1033,8 @@ RSS& RSS::operator += (const Vec3f& p)
FCL_REAL delta_x = fabs(proj0 - x);
FCL_REAL delta_y = fabs(proj1 - y);
- l[0] += delta_x;
- l[1] += delta_y;
+ length[0] += delta_x;
+ length[1] += delta_y;
if(proj0 < 0 && proj1 < 0)
{
@@ -1043,9 +1043,9 @@ RSS& RSS::operator += (const Vec3f& p)
}
if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
+ Tr[2] += 0.5 * (abs_proj2 - radius);
else
- Tr[2] -= 0.5 * (abs_proj2 - r);
+ Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
}
@@ -1058,12 +1058,12 @@ RSS RSS::operator + (const RSS& other) const
RSS bv;
Vec3f v[16];
- Vec3f d0_pos (other.axes.col(0) * (other.l[0] + other.r));
- Vec3f d1_pos (other.axes.col(1) * (other.l[1] + other.r));
- Vec3f d0_neg (other.axes.col(0) * (-other.r));
- Vec3f d1_neg (other.axes.col(1) * (-other.r));
- Vec3f d2_pos (other.axes.col(2) * other.r);
- Vec3f d2_neg (other.axes.col(2) * (-other.r));
+ Vec3f d0_pos (other.axes.col(0) * (other.length[0] + other.radius));
+ Vec3f d1_pos (other.axes.col(1) * (other.length[1] + other.radius));
+ Vec3f d0_neg (other.axes.col(0) * (-other.radius));
+ Vec3f d1_neg (other.axes.col(1) * (-other.radius));
+ Vec3f d2_pos (other.axes.col(2) * other.radius);
+ Vec3f d2_neg (other.axes.col(2) * (-other.radius));
v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos;
v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg;
@@ -1074,12 +1074,12 @@ RSS RSS::operator + (const RSS& other) const
v[6].noalias() = other.Tr + d0_neg + d1_neg + d2_pos;
v[7].noalias() = other.Tr + d0_neg + d1_neg + d2_neg;
- d0_pos.noalias() = axes.col(0) * (l[0] + r);
- d1_pos.noalias() = axes.col(1) * (l[1] + r);
- d0_neg.noalias() = axes.col(0) * (-r);
- d1_neg.noalias() = axes.col(1) * (-r);
- d2_pos.noalias() = axes.col(2) * r;
- d2_neg.noalias() = axes.col(2) * (-r);
+ d0_pos.noalias() = axes.col(0) * (length[0] + radius);
+ d1_pos.noalias() = axes.col(1) * (length[1] + radius);
+ d0_neg.noalias() = axes.col(0) * (-radius);
+ d1_neg.noalias() = axes.col(1) * (-radius);
+ d2_pos.noalias() = axes.col(2) * radius;
+ d2_neg.noalias() = axes.col(2) * (-radius);
v[ 8].noalias() = Tr + d0_pos + d1_pos + d2_pos;
v[ 9].noalias() = Tr + d0_pos + d1_pos + d2_neg;
@@ -1113,7 +1113,7 @@ RSS RSS::operator + (const RSS& other) const
E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.length, bv.radius);
return bv;
}
@@ -1126,8 +1126,8 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
Matrix3f R (axes.transpose() * other.axes);
Vec3f T (axes.transpose() * (other.Tr - Tr));
- FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
- dist -= (r + other.r);
+ FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q);
+ dist -= (radius + other.radius);
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
@@ -1138,8 +1138,8 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
Vec3f T(b1.axes.transpose() * Ttemp);
- FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q);
- dist -= (b1.r + b2.r);
+ FCL_REAL dist = rectDistance(R, T, b1.length, b2.length, P, Q);
+ dist -= (b1.radius + b2.radius);
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index 901d5cf9e5020b0a5b66c54fd697a828d19d5b20..b1f2f7bef3ad94d6b1ca9879857c95aa4634a668 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -45,68 +45,6 @@ namespace hpp
namespace fcl
{
-void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
-{
- for(int i = 0; i < model.getNumBVs(); ++i)
- {
- BVNode<OBB>& bvnode = model.getBV(i);
-
- Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
-
- for(int j = 0; j < bvnode.num_primitives; ++j)
- {
- int v_id = bvnode.first_primitive + j;
- const Variance3f& uc = ucs[v_id];
-
- Vec3f&v = model.vertices[bvnode.first_primitive + j];
-
- for(int k = 0; k < 3; ++k)
- {
- vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
- vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
- }
- }
-
- OBB bv;
- fit(vs, bvnode.num_primitives * 6, bv);
-
- delete [] vs;
-
- bvnode.bv = bv;
- }
-}
-
-void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
-{
- for(int i = 0; i < model.getNumBVs(); ++i)
- {
- BVNode<RSS>& bvnode = model.getBV(i);
-
- Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
-
- for(int j = 0; j < bvnode.num_primitives; ++j)
- {
- int v_id = bvnode.first_primitive + j;
- const Variance3f& uc = ucs[v_id];
-
- Vec3f&v = model.vertices[bvnode.first_primitive + j];
-
- for(int k = 0; k < 3; ++k)
- {
- vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
- vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
- }
- }
-
- RSS bv;
- fit(vs, bvnode.num_primitives * 6, bv);
-
- delete [] vs;
-
- bvnode.bv = bv;
- }
-}
-
template<typename BV>
BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, const AABB& _aabb)
{
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index 3ef215079e236f369b65522293f76fbc1eaf4c91..a9179268a779fa239db1431e8b09ac4fcabd8315 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -35,10 +35,10 @@
/** \author Jia Pan */
-#include <hpp/fcl/BVH/BV_fitter.h>
+#include "BV_fitter.h"
#include <hpp/fcl/BVH/BVH_utility.h>
#include <limits>
-#include <hpp/fcl/math/tools.h>
+#include "../math/tools.h"
namespace hpp
{
@@ -150,9 +150,9 @@ void fit1(Vec3f* ps, RSS& bv)
{
bv.Tr.noalias() = ps[0];
bv.axes.setIdentity();
- bv.l[0] = 0;
- bv.l[1] = 0;
- bv.r = 0;
+ bv.length[0] = 0;
+ bv.length[1] = 0;
+ bv.radius = 0;
}
void fit2(Vec3f* ps, RSS& bv)
@@ -164,11 +164,11 @@ void fit2(Vec3f* ps, RSS& bv)
bv.axes.col(0) /= len_p1p2;
generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2));
- bv.l[0] = len_p1p2;
- bv.l[1] = 0;
+ bv.length[0] = len_p1p2;
+ bv.length[1] = 0;
bv.Tr = p2;
- bv.r = 0;
+ bv.radius = 0;
}
void fit3(Vec3f* ps, RSS& bv)
@@ -193,7 +193,7 @@ void fit3(Vec3f* ps, RSS& bv)
bv.axes.col(0).noalias() = e[imax].normalized();
bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0));
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.length, bv.radius);
}
void fit6(Vec3f* ps, RSS& bv)
@@ -215,7 +215,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
axisFromEigen(E, s, bv.axes);
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.length, bv.radius);
}
}
@@ -542,9 +542,9 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r);
bv.rss.Tr = origin;
- bv.rss.l[0] = l[0];
- bv.rss.l[1] = l[1];
- bv.rss.r = r;
+ bv.rss.length[0] = l[0];
+ bv.rss.length[1] = l[1];
+ bv.rss.radius = r;
return bv;
}
@@ -568,9 +568,9 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r);
bv.Tr = origin;
- bv.l[0] = l[0];
- bv.l[1] = l[1];
- bv.r = r;
+ bv.length[0] = l[0];
+ bv.length[1] = l[1];
+ bv.radius = r;
return bv;
diff --git a/include/hpp/fcl/BVH/BV_fitter.h b/src/BVH/BV_fitter.h
similarity index 92%
rename from include/hpp/fcl/BVH/BV_fitter.h
rename to src/BVH/BV_fitter.h
index 97a27f9da198a6d88890e6278224c31a32e6b693..d8070d695129b820c00a979007416b3e8cf5e70e 100644
--- a/include/hpp/fcl/BVH/BV_fitter.h
+++ b/src/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/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index 1674cef4a88b52ba85bb5c64f166efa20063f112..c709ed63862df7d09469cf063258de1f2a798669 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/BVH/BV_splitter.h>
+#include "BV_splitter.h"
namespace hpp
{
diff --git a/include/hpp/fcl/BVH/BV_splitter.h b/src/BVH/BV_splitter.h
similarity index 92%
rename from include/hpp/fcl/BVH/BV_splitter.h
rename to src/BVH/BV_splitter.h
index 7ad7bd996b0d6fef5b96318256579f03f0f6cd1b..7be8ca7490f7ae6fe3e268e4ac3a3c6b8d3e0e52 100644
--- a/include/hpp/fcl/BVH/BV_splitter.h
+++ b/src/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/collision.cpp b/src/collision.cpp
index c5405037d4e9cd41d46acf2596467c5d48efcd01..b07f2b5d69292b1f64488533b23a1ef8aa8de857 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -47,23 +47,12 @@ namespace hpp
namespace fcl
{
-template<typename GJKSolver>
-CollisionFunctionMatrix<GJKSolver>& getCollisionFunctionLookTable()
+CollisionFunctionMatrix& getCollisionFunctionLookTable()
{
- static CollisionFunctionMatrix<GJKSolver> table;
+ static CollisionFunctionMatrix table;
return table;
}
-template<typename NarrowPhaseSolver>
-std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
- const NarrowPhaseSolver* nsolver,
- const CollisionRequest& request,
- CollisionResult& result)
-{
- return collide(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(),
- nsolver, request, result);
-}
-
// reorder collision results in the order the call has been made.
void invertResults(CollisionResult& result)
{
@@ -81,18 +70,17 @@ void invertResults(CollisionResult& result)
}
}
-template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver_,
+ const 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& looktable = getCollisionFunctionLookTable();
result.distance_lower_bound = -1;
std::size_t res;
if(request.num_max_contacts == 0)
@@ -138,6 +126,14 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
return res;
}
+std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
+ const GJKSolver* nsolver,
+ const CollisionRequest& request,
+ CollisionResult& result)
+{
+ return collide(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver, request, result);
+}
+
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
const CollisionRequest& request, CollisionResult& result)
{
@@ -146,7 +142,7 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
case GST_INDEP:
{
GJKSolver solver;
- return collide<GJKSolver>(o1, o2, &solver, request, result);
+ return collide(o1, o2, &solver, request, result);
}
default:
return -1; // error
@@ -162,7 +158,7 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
case GST_INDEP:
{
GJKSolver solver;
- return collide<GJKSolver>(o1, tf1, o2, tf2, &solver, request, result);
+ return collide(o1, tf1, o2, tf2, &solver, request, result);
}
default:
std::cerr << "Warning! Invalid GJK solver" << std::endl;
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 675abce8f833f9166615a11bf0aaa44f5102c3ae..78e9719e6dd53686d43389fa31654e37c9b34085 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>
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> 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 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>
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> 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 otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
@@ -85,17 +85,16 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& t
return result.numContacts();
}
-template<typename NarrowPhaseSolver>
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 node (request);
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
@@ -103,34 +102,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>
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> 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 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>
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> 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 otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
@@ -139,16 +138,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>
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>
(o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
if (distance <= 0) {
@@ -179,16 +178,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>
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> 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 +204,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>
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 +223,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>
+struct BVHShapeCollider<OBB, T_SH>
{
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>, OBB, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
+template<typename T_SH>
+struct BVHShapeCollider<RSS, T_SH>
{
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>, RSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
+template<typename T_SH>
+struct BVHShapeCollider<kIOS, T_SH>
{
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>, kIOS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
+template<typename T_SH>
+struct BVHShapeCollider<OBBRSS, T_SH>
{
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>, OBBRSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -333,17 +332,16 @@ std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1
}
-template<typename T_BVH, typename NarrowPhaseSolver>
+template<typename T_BVH>
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()
+CollisionFunctionMatrix::CollisionFunctionMatrix()
{
for(int i = 0; i < NODE_COUNT; ++i)
{
@@ -351,200 +349,200 @@ 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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<ConvexBase, Box, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<ConvexBase, Sphere, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<ConvexBase, Capsule, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<ConvexBase, Cone, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<ConvexBase, Cylinder, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<ConvexBase, ConvexBase, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<ConvexBase, Plane, NarrowPhaseSolver>;
- collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<ConvexBase, 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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>, ConvexBase, 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>, ConvexBase, 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>, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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>;
+ collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere>;
+ collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule>;
+ collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone>;
+ collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder>;
+ collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, ConvexBase>;
+ collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane>;
+ collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide<Box, Halfspace>;
+
+ collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box>;
+ collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere>;
+ collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone>;
+ collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, ConvexBase>;
+ collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane>;
+ collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere, Halfspace>;
+
+ collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box>;
+ collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, ConvexBase>;
+ collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane>;
+ collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide<Capsule, Halfspace>;
+
+ collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box>;
+ collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere>;
+ collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule>;
+ collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone>;
+ collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder>;
+ collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, ConvexBase>;
+ collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane>;
+ collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide<Cone, Halfspace>;
+
+ collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box>;
+ collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, ConvexBase>;
+ collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane>;
+ collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide<Cylinder, Halfspace>;
+
+ collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<ConvexBase, Box>;
+ collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<ConvexBase, Sphere>;
+ collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<ConvexBase, Capsule>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<ConvexBase, Cone>;
+ collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<ConvexBase, Cylinder>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<ConvexBase, ConvexBase>;
+ collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<ConvexBase, Plane>;
+ collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<ConvexBase, Halfspace>;
+
+ collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box>;
+ collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere>;
+ collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule>;
+ collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone>;
+ collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder>;
+ collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, ConvexBase>;
+ collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane>;
+ collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide<Plane, Halfspace>;
+
+ collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace, Box>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace, Sphere>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace, Capsule>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace, Cone>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace, Cylinder>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide<Halfspace, ConvexBase>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide<Halfspace, Plane>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide<Halfspace, Halfspace>;
+
+ collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box>::collide;
+ collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere>::collide;
+ collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule>::collide;
+ collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone>::collide;
+ collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder>::collide;
+ collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, ConvexBase>::collide;
+ collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane>::collide;
+ collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider<AABB, Halfspace>::collide;
+
+ collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box>::collide;
+ collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere>::collide;
+ collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule>::collide;
+ collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone>::collide;
+ collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder>::collide;
+ collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, ConvexBase>::collide;
+ collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane>::collide;
+ collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider<OBB, Halfspace>::collide;
+
+ collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box>::collide;
+ collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere>::collide;
+ collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule>::collide;
+ collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone>::collide;
+ collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder>::collide;
+ collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, ConvexBase>::collide;
+ collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane>::collide;
+ collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider<RSS, Halfspace>::collide;
+
+ collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box>::collide;
+ collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, ConvexBase>::collide;
+ collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane>::collide;
+ collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<16>, Halfspace>::collide;
+
+ collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box>::collide;
+ collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, ConvexBase>::collide;
+ collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane>::collide;
+ collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<18>, Halfspace>::collide;
+
+ collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box>::collide;
+ collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, ConvexBase>::collide;
+ collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane>::collide;
+ collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<24>, Halfspace>::collide;
+
+ collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box>::collide;
+ collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere>::collide;
+ collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone>::collide;
+ collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, ConvexBase>::collide;
+ collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane>::collide;
+ collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider<kIOS, Halfspace>::collide;
+
+ collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS, Capsule>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS, Cone>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, ConvexBase>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider<OBBRSS, Halfspace>::collide;
+
+ collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB>;
+ collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB>;
+ collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS>;
+ collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16> >;
+ collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18> >;
+ collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24> >;
+ collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS>;
+ collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS>;
#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<ConvexBase, 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<ConvexBase, 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>;
+ collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere>;
+ collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone>;
+ collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<ConvexBase>;
+ collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane>;
+ collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace>;
+
+ collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box>;
+ collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere>;
+ collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule>;
+ collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone>;
+ collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder>;
+ collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<ConvexBase>;
+ collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace>;
+
+ collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide;
+
+ collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB>;
+ collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB>;
+ collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS>;
+ collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS>;
+ collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<16> >;
+ collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<18> >;
+ collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<24> >;
+
+ collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB>;
+ collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB>;
+ collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS>;
+ collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS>;
+ collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS>;
+ collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16> >;
+ collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18> >;
+ collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24> >;
#endif
}
-template struct CollisionFunctionMatrix<GJKSolver>;
+//template struct CollisionFunctionMatrix;
}
} // namespace hpp
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..95c4a8b2ee9cd2ae4d1ef757939dbe83b5d19322 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -46,32 +46,22 @@ namespace hpp
namespace fcl
{
-template<typename GJKSolver>
-DistanceFunctionMatrix<GJKSolver>& getDistanceFunctionLookTable()
+DistanceFunctionMatrix& getDistanceFunctionLookTable()
{
- static DistanceFunctionMatrix<GJKSolver> table;
+ static DistanceFunctionMatrix table;
return table;
}
-template<typename NarrowPhaseSolver>
-FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
- const DistanceRequest& request, DistanceResult& result)
-{
- return distance<NarrowPhaseSolver>(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver,
- request, result);
-}
-
-template<typename NarrowPhaseSolver>
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& looktable = getDistanceFunctionLookTable();
OBJECT_TYPE object_type1 = o1->getObjectType();
NODE_TYPE node_type1 = o1->getNodeType();
@@ -118,6 +108,12 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
return res;
}
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver* nsolver,
+ const DistanceRequest& request, DistanceResult& result)
+{
+ return distance(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver,
+ request, result);
+}
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result)
{
@@ -126,7 +122,7 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Di
case GST_INDEP:
{
GJKSolver solver;
- return distance<GJKSolver>(o1, o2, &solver, request, result);
+ return distance(o1, o2, &solver, request, result);
}
default:
return -1; // error
@@ -142,7 +138,7 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
case GST_INDEP:
{
GJKSolver solver;
- return distance<GJKSolver>(o1, tf1, o2, tf2, &solver, request, result);
+ return distance(o1, tf1, o2, tf2, &solver, request, result);
}
default:
return -1;
diff --git a/src/distance_box_halfspace.cpp b/src/distance_box_halfspace.cpp
index 80fc3fe265b44f5ab7c5881a61a78fea54b19662..8ad3a97db8f4c76d078c02207a5b38b20c3c2f5b 100644
--- a/src/distance_box_halfspace.cpp
+++ b/src/distance_box_halfspace.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Box, Halfspace, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Box, Halfspace>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Halfspace, Box, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Halfspace, Box>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_box_plane.cpp b/src/distance_box_plane.cpp
index 0c438f324b45313beb37a50274691bc3ca86bde7..1619e4876a29625f313ca760f5e6e332e158d503 100644
--- a/src/distance_box_plane.cpp
+++ b/src/distance_box_plane.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Box, Plane, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Box, Plane>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Plane, Box, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Plane, Box>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_box_sphere.cpp b/src/distance_box_sphere.cpp
index 6ee8870f717afaf65774f53374632d15b103c4be..553cbeb621adc522f20819fe2a0564800a9a4cc4 100644
--- a/src/distance_box_sphere.cpp
+++ b/src/distance_box_sphere.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Box, Sphere, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Box, Sphere>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Sphere, Box, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Sphere, Box>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 5e6867c59a142f103380fce46a01ee5117597222..c73f2742d754a84e1e5e3ac2b3a4604c17178ecc 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -26,7 +26,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Capsule, Capsule, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Capsule, Capsule>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest& request,
@@ -321,7 +321,7 @@ namespace fcl {
}
template <>
- std::size_t ShapeShapeCollide <Capsule, Capsule, GJKSolver>
+ std::size_t ShapeShapeCollide <Capsule, Capsule>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const CollisionRequest& request,
@@ -331,7 +331,7 @@ namespace fcl {
DistanceResult distanceResult;
DistanceRequest distanceRequest (request.enable_contact);
- FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule, GJKSolver>
+ FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule>
(o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
if (distance <= 0) {
diff --git a/src/distance_capsule_halfspace.cpp b/src/distance_capsule_halfspace.cpp
index badb4eb85d2196762518541c1bf4ec747b18d2ab..49976683742bf5cb2311570b00c5db557cae7e9c 100644
--- a/src/distance_capsule_halfspace.cpp
+++ b/src/distance_capsule_halfspace.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Capsule, Halfspace, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Capsule, Halfspace>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Halfspace, Capsule, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Halfspace, Capsule>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_capsule_plane.cpp b/src/distance_capsule_plane.cpp
index b8d0a55aef768df778aa323b1d7ba943e83a7f0b..f719d38ba8ed69a89f8b65cf53bf2eebf3b09f9c 100644
--- a/src/distance_capsule_plane.cpp
+++ b/src/distance_capsule_plane.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Capsule, Plane, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Capsule, Plane>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Plane, Capsule, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Plane, Capsule>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_cone_halfspace.cpp b/src/distance_cone_halfspace.cpp
index 74b8dafeb115cda7ccd81da3d48ca4f5e9529a4e..d7bda73365fcb5ed43bafb7101a80dfc817e3aa1 100644
--- a/src/distance_cone_halfspace.cpp
+++ b/src/distance_cone_halfspace.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Cone, Halfspace, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Cone, Halfspace>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Halfspace, Cone, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Halfspace, Cone>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_cone_plane.cpp b/src/distance_cone_plane.cpp
index 9e0161fb5d0ecd1242d4543ce9e1915e406c042d..893192a111ba7511c7926f1fb37c0dd3734819a1 100644
--- a/src/distance_cone_plane.cpp
+++ b/src/distance_cone_plane.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Cone, Plane, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Cone, Plane>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Plane, Cone, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Plane, Cone>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_cylinder_halfspace.cpp b/src/distance_cylinder_halfspace.cpp
index 975c5926573b539fcd76a00ca7cab80c8b21c61a..086a6b3c1638867e05f0b9ad7d05bc2ed2980a7d 100644
--- a/src/distance_cylinder_halfspace.cpp
+++ b/src/distance_cylinder_halfspace.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Cylinder, Halfspace, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Cylinder, Halfspace>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Halfspace, Cylinder, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Halfspace, Cylinder>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_cylinder_plane.cpp b/src/distance_cylinder_plane.cpp
index 4ca23531236dd6cb634758e77452fd5d67b75f6a..88473ecf8d86c0389dda09d75b33b07aac2f2c06 100644
--- a/src/distance_cylinder_plane.cpp
+++ b/src/distance_cylinder_plane.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Cylinder, Plane, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Cylinder, Plane>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Plane, Cylinder, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Plane, Cylinder>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index e16c079370a15c6bedb5dbb42fea4688a2bb2259..c4b4e33c67dad5a0e66322ca369365f09d44d26b 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -38,8 +38,7 @@
#include <hpp/fcl/distance_func_matrix.h>
#include <../src/collision_node.h>
-#include <hpp/fcl/traversal/traversal_node_setup.h>
-#include <hpp/fcl/narrowphase/narrowphase.h>
+#include "traversal/traversal_node_setup.h"
namespace hpp
{
@@ -47,15 +46,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>
+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> node;
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -63,15 +62,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>
+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> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -79,15 +78,14 @@ 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,
+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 node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
+ OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -95,15 +93,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>
+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> 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 otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -111,15 +109,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>
+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> 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 otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
distance(&node);
@@ -129,12 +127,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>
+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> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
@@ -144,14 +142,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>
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> 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 +166,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>
+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 +183,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>
+struct BVHShapeDistancer<RSS, T_SH>
{
- 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>, RSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
+template<typename T_SH>
+struct BVHShapeDistancer<kIOS, T_SH>
{
- 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>, kIOS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
-template<typename T_SH, typename NarrowPhaseSolver>
-struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
+template<typename T_SH>
+struct BVHShapeDistancer<OBBRSS, T_SH>
{
- 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>, OBBRSS, T_SH>(o1, tf1, o2, tf2, nsolver, request, result);
}
};
@@ -280,16 +278,15 @@ FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1
}
-template<typename T_BVH, typename NarrowPhaseSolver>
+template<typename T_BVH>
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()
+DistanceFunctionMatrix::DistanceFunctionMatrix()
{
for(int i = 0; i < NODE_COUNT; ++i)
{
@@ -297,205 +294,205 @@ 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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, ConvexBase, 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<ConvexBase, Box, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<ConvexBase, Sphere, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<ConvexBase, Capsule, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<ConvexBase, Cone, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<ConvexBase, Cylinder, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<ConvexBase, ConvexBase, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<ConvexBase, Plane, NarrowPhaseSolver>;
- distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<ConvexBase, 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, ConvexBase, 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, ConvexBase, 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>;
+ distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere>;
+ distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule>;
+ distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone>;
+ distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder>;
+ distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, ConvexBase>;
+ distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane>;
+ distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance<Box, Halfspace>;
+
+ distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box>;
+ distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere>;
+ distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere, Capsule>;
+ distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone>;
+ distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder>;
+ distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere, ConvexBase>;
+ distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane>;
+ distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere, Halfspace>;
+
+ distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box>;
+ distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule, Capsule>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule, ConvexBase>;
+ distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule, Plane>;
+ distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance<Capsule, Halfspace>;
+
+ distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box>;
+ distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere>;
+ distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule>;
+ distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone>;
+ distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder>;
+ distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone, ConvexBase>;
+ distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane>;
+ distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance<Cone, Halfspace>;
+
+ distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box>;
+ distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder, Capsule>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder, Cone>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder, ConvexBase>;
+ distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder, Plane>;
+ distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance<Cylinder, Halfspace>;
+
+ distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<ConvexBase, Box>;
+ distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<ConvexBase, Sphere>;
+ distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<ConvexBase, Capsule>;
+ distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<ConvexBase, Cone>;
+ distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<ConvexBase, Cylinder>;
+ distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<ConvexBase, ConvexBase>;
+ distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<ConvexBase, Plane>;
+ distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<ConvexBase, Halfspace>;
+
+ distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box>;
+ distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere>;
+ distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane, Capsule>;
+ distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone>;
+ distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder>;
+ distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, ConvexBase>;
+ distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane>;
+ distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance<Plane, Halfspace>;
+
+ distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance<Halfspace, Box>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance<Halfspace, Sphere>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance<Halfspace, Capsule>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance<Halfspace, Cone>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance<Halfspace, Cylinder>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance<Halfspace, ConvexBase>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance<Halfspace, Plane>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance<Halfspace, Halfspace>;
/* 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, ConvexBase, 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>::distance;
+ distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB, Sphere>::distance;
+ distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB, Capsule>::distance;
+ distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB, Cone>::distance;
+ distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder>::distance;
+ distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, ConvexBase>::distance;
+ distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane>::distance;
+ distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace>::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, ConvexBase, 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, ConvexBase, 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>::distance;
+ distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere>::distance;
+ distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB, Capsule>::distance;
+ distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone>::distance;
+ distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder>::distance;
+ distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, ConvexBase>::distance;
+ distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane>::distance;
+ distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace>::distance;
+
+ distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box>::distance;
+ distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere>::distance;
+ distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS, Capsule>::distance;
+ distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone>::distance;
+ distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder>::distance;
+ distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, ConvexBase>::distance;
+ distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane>::distance;
+ distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer<RSS, Halfspace>::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>, ConvexBase, 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>, ConvexBase, 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>, ConvexBase, 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>::distance;
+ distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<16>, Sphere>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>, Capsule>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<16>, Cone>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<16>, ConvexBase>::distance;
+ distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>, Plane>::distance;
+ distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<16>, Halfspace>::distance;
+
+ distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box>::distance;
+ distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>, Capsule>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<18>, Cone>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<18>, ConvexBase>::distance;
+ distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>, Plane>::distance;
+ distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<18>, Halfspace>::distance;
+
+ distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box>::distance;
+ distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>, Capsule>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<24>, Cone>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<24>, ConvexBase>::distance;
+ distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>, Plane>::distance;
+ distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<24>, Halfspace>::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, ConvexBase, 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, ConvexBase, 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>::distance;
+ distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS, Sphere>::distance;
+ distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS, Capsule>::distance;
+ distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS, Cone>::distance;
+ distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder>::distance;
+ distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS, ConvexBase>::distance;
+ distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS, Plane>::distance;
+ distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer<kIOS, Halfspace>::distance;
+
+ distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS, Capsule>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS, Cone>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS, ConvexBase>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS, Plane>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace>::distance;
+
+ distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB>;
+ distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB>;
+ distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS>;
+ distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS>;
+ distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS>;
#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<ConvexBase, 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<ConvexBase, 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>;
+ distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere>;
+ distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone>;
+ distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder>;
+ distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<ConvexBase>;
+ distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane>;
+ distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace>;
+
+ distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box>;
+ distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere>;
+ distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule>;
+ distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone>;
+ distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder>;
+ distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<ConvexBase>;
+ distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane>;
+ distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace>;
+
+ distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance;
+
+ distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB>;
+ distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB>;
+ distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS>;
+ distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS>;
+ distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS>;
+ distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<16> >;
+ distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<18> >;
+ distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<24> >;
+
+ distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB>;
+ distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB>;
+ distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS>;
+ distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS>;
+ distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS>;
+ distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<16> >;
+ distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<18> >;
+ distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<24> >;
#endif
}
-template struct DistanceFunctionMatrix<GJKSolver>;
+//template struct DistanceFunctionMatrix;
}
} // namespace hpp
diff --git a/src/distance_func_matrix.h b/src/distance_func_matrix.h
index 5cb194384daf982f891d858de1297038277673ba..f9a291ee897c58d895b960b2948999d12c416d6f 100644
--- a/src/distance_func_matrix.h
+++ b/src/distance_func_matrix.h
@@ -35,22 +35,23 @@
/** \author Florent Lamiraux */
#include <hpp/fcl/collision_data.h>
+#include <hpp/fcl/narrowphase/narrowphase.h>
namespace hpp
{
namespace fcl {
- template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+ template<typename T_SH1, typename T_SH2>
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>
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/distance_sphere_cylinder.cpp b/src/distance_sphere_cylinder.cpp
index bb3686182e68779e27d9099d32a4082ca337b2ba..ef031a97cdf914d807ca006add367d091c19f485 100644
--- a/src/distance_sphere_cylinder.cpp
+++ b/src/distance_sphere_cylinder.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Sphere, Cylinder, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Sphere, Cylinder>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Cylinder, Sphere, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Cylinder, Sphere>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_sphere_halfspace.cpp b/src/distance_sphere_halfspace.cpp
index 90bc6b86348050b0f0e67018c94214ac28c85d72..9060e173c3710c34f701d76befcbea288dace809 100644
--- a/src/distance_sphere_halfspace.cpp
+++ b/src/distance_sphere_halfspace.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Sphere, Halfspace, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Sphere, Halfspace>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Halfspace, Sphere, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Halfspace, Sphere>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_sphere_plane.cpp b/src/distance_sphere_plane.cpp
index 5e8ae30d61c6fcc9ef70a522ed1d386268dccb8f..89ebb150271700891580937bcefac4c6695ff6a1 100644
--- a/src/distance_sphere_plane.cpp
+++ b/src/distance_sphere_plane.cpp
@@ -49,7 +49,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Sphere, Plane, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Sphere, Plane>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -65,7 +65,7 @@ namespace fcl {
}
template <>
- FCL_REAL ShapeShapeDistance <Plane, Sphere, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Plane, Sphere>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
diff --git a/src/distance_sphere_sphere.cpp b/src/distance_sphere_sphere.cpp
index de0203096db54bbe0007e49832605c10285e9438..aeca9663ad61fb62a0b659afca5bf5fd547cc9da 100644
--- a/src/distance_sphere_sphere.cpp
+++ b/src/distance_sphere_sphere.cpp
@@ -53,7 +53,7 @@ namespace fcl {
class GJKSolver;
template <>
- FCL_REAL ShapeShapeDistance <Sphere, Sphere, GJKSolver>
+ FCL_REAL ShapeShapeDistance <Sphere, Sphere>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest&,
@@ -97,7 +97,7 @@ namespace fcl {
}
template <>
- std::size_t ShapeShapeCollide <Sphere, Sphere, GJKSolver>
+ std::size_t ShapeShapeCollide <Sphere, Sphere>
(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const CollisionRequest& request,
diff --git a/src/intersect.cpp b/src/intersect.cpp
index a69343de864f491a98cb51a43639f7af61e1ac0a..21c8c4b6851b4e0ecfc78ff945f0286014e040fd 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -35,145 +35,17 @@
/** \author Jia Pan */
-#include <hpp/fcl/intersect.h>
+#include "intersect.h"
#include <iostream>
#include <limits>
#include <vector>
#include <boost/math/special_functions/fpclassify.hpp>
-#include <hpp/fcl/math/tools.h>
+#include "math/tools.h"
namespace hpp
{
namespace fcl
{
-const FCL_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9;
-
-
-bool PolySolver::isZero(FCL_REAL v)
-{
- return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
-}
-
-bool PolySolver::cbrt(FCL_REAL v)
-{
- return powf((float) v, (float) (1.0 / 3.0));
-}
-
-int PolySolver::solveLinear(FCL_REAL c[2], FCL_REAL s[1])
-{
- if(isZero(c[1]))
- return 0;
- s[0] = - c[0] / c[1];
- return 1;
-}
-
-int PolySolver::solveQuadric(FCL_REAL c[3], FCL_REAL s[2])
-{
- FCL_REAL p, q, D;
-
- // make sure we have a d2 equation
-
- if(isZero(c[2]))
- return solveLinear(c, s);
-
- // normal for: x^2 + px + q
- p = c[1] / (2.0 * c[2]);
- q = c[0] / c[2];
- D = p * p - q;
-
- if(isZero(D))
- {
- // one FCL_REAL root
- s[0] = s[1] = -p;
- return 1;
- }
-
- if(D < 0.0)
- // no real root
- return 0;
- else
- {
- // two real roots
- FCL_REAL sqrt_D = sqrt(D);
- s[0] = sqrt_D - p;
- s[1] = -sqrt_D - p;
- return 2;
- }
-}
-
-int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3])
-{
- int i, num;
- FCL_REAL sub, A, B, C, sq_A, p, q, cb_p, D;
- const FCL_REAL ONE_OVER_THREE = 1 / 3.0;
- const FCL_REAL PI = 3.14159265358979323846;
-
- // make sure we have a d2 equation
- if(isZero(c[3]))
- return solveQuadric(c, s);
-
- // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0
- A = c[2] / c[3];
- B = c[1] / c[3];
- C = c[0] / c[3];
-
- // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0
- sq_A = A * A;
- p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE;
- q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C);
-
- // use Cardano's formula
- cb_p = p * p * p;
- D = q * q + cb_p;
-
- if(isZero(D))
- {
- if(isZero(q))
- {
- // one triple solution
- s[0] = 0.0;
- num = 1;
- }
- else
- {
- // one single and one FCL_REAL solution
- FCL_REAL u = cbrt(-q);
- s[0] = 2.0 * u;
- s[1] = -u;
- num = 2;
- }
- }
- else
- {
- if(D < 0.0)
- {
- // three real solutions
- FCL_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p));
- FCL_REAL t = 2.0 * sqrt(-p);
- s[0] = t * cos(phi);
- s[1] = -t * cos(phi + PI / 3.0);
- s[2] = -t * cos(phi - PI / 3.0);
- num = 3;
- }
- else
- {
- // one real solution
- FCL_REAL sqrt_D = sqrt(D);
- FCL_REAL u = cbrt(sqrt_D + fabs(q));
- if(q > 0.0)
- s[0] = - u + p / u ;
- else
- s[0] = u - p / u;
- num = 1;
- }
- }
-
- // re-substitute
- sub = ONE_OVER_THREE * A;
- for(i = 0; i < num; i++)
- s[i] -= sub;
- return num;
-}
bool Intersect::buildTrianglePlane
(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t)
diff --git a/include/hpp/fcl/intersect.h b/src/intersect.h
similarity index 89%
rename from include/hpp/fcl/intersect.h
rename to src/intersect.h
index b56a6c626389fd6628ea5ef31b79a9f9a9fff38b..fd8dfd559040d88bde32a6871dc1a385554b8a99 100644
--- a/include/hpp/fcl/intersect.h
+++ b/src/intersect.h
@@ -41,42 +41,19 @@
#include <hpp/fcl/math/transform.h>
#include <boost/math/special_functions/erf.hpp>
-namespace hpp
-{
+namespace hpp
+{
namespace fcl
{
-/// @brief A class solves polynomial degree (1,2,3) equations
-class PolySolver
+/// @brief CCD intersect kernel among primitives
+class Intersect
{
public:
- /// @brief Solve a linear equation with coefficients c, return roots s and number of roots
- static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
-
- /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots
- static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
-
- /// @brief Solve a cubic function with coefficients c, return roots s and number of roots
- static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
-
-private:
- /// @brief Check whether v is zero
- static inline bool isZero(FCL_REAL v);
+ static bool buildTrianglePlane
+ (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
+}; // class Intersect
- /// @brief Compute v^{1/3}
- static inline bool cbrt(FCL_REAL v);
-
- static const FCL_REAL NEAR_ZERO_THRESHOLD;
-};
-
-/// @brief CCD intersect kernel among primitives
-class Intersect
-{
-public:
- static bool buildTrianglePlane
- (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
-}; // class Intersect
-
/// @brief Project functions
class Project
{
@@ -213,6 +190,6 @@ public:
}
-} // namespace hpp
-
+} // namespace hpp
+
#endif
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 4126cce0b4f59d4a4ed6bfb16c017fbb9a0b6411..a395abd75035d0b82a00b7b8f2a7d06fad146f0f 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 7f249a4811d2fc1e7208fb6d30d6afb19de10665..230884e9831a93f141a7b99f574b970d6e3f7a46 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -36,8 +36,8 @@
/** \author Jia Pan */
#include <hpp/fcl/narrowphase/gjk.h>
-#include <hpp/fcl/intersect.h>
-#include <hpp/fcl/math/tools.h>
+#include "../intersect.h"
+#include "../math/tools.h"
namespace hpp
{
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 619c77c95f2c458e191e5b9c00ed9770815a0726..f0b29a7270df353702081fcd2ae819fb332fcc34 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -37,7 +37,7 @@
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/intersect.h>
+#include "../intersect.h"
#include <boost/math/constants/constants.hpp>
#include <vector>
#include "../src/narrowphase/details.h"
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 866711258c113d1891d4b41c1ddab797726d18b2..b02f952317b493b36697e336f9a79f09a6421fe2 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -37,8 +37,8 @@
#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/BVH/BV_fitter.h>
-#include <hpp/fcl/math/tools.h>
+#include "../BVH/BV_fitter.h"
+#include "../math/tools.h"
namespace hpp
{
@@ -482,7 +482,7 @@ void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv)
/// Half space can only have very rough RSS
bv.axes.setIdentity();
bv.Tr.setZero();
- bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<FCL_REAL>::max();
+ bv.length[0] = bv.length[1] = bv.radius = std::numeric_limits<FCL_REAL>::max();
}
template<>
@@ -716,10 +716,10 @@ void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
bv.axes.col(0).noalias() = n;
- bv.l[0] = std::numeric_limits<FCL_REAL>::max();
- bv.l[1] = std::numeric_limits<FCL_REAL>::max();
+ bv.length[0] = std::numeric_limits<FCL_REAL>::max();
+ bv.length[1] = std::numeric_limits<FCL_REAL>::max();
- bv.r = 0;
+ bv.radius = 0;
Vec3f p = s.n * s.d;
bv.Tr = tf.transform(p);
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 86%
rename from include/hpp/fcl/traversal/traversal_node_bvh_shape.h
rename to src/traversal/traversal_node_bvh_shape.h
index 0ae2c53c5f3a307c5f51e58cf268c01eee16557d..e931a392748039f7ef84fdd0f56724df8af1f91d 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>
@@ -146,7 +146,7 @@ public:
/// @brief Traversal node for collision between mesh and shape
-template<typename BV, typename S, typename GJKSolver,
+template<typename BV, typename S,
int _Options = RelativeTransformationIsIdentity>
class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
{
@@ -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);
@@ -260,46 +260,46 @@ public:
};
/// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
-template<typename S, typename GJKSolver>
-class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, GJKSolver, 0>
+template<typename S>
+class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, 0>
{
public:
MeshShapeCollisionTraversalNodeOBB(const CollisionRequest& request) :
- MeshShapeCollisionTraversalNode<OBB, S, GJKSolver, 0>
+ MeshShapeCollisionTraversalNode<OBB, S, 0>
(request)
{
}
};
-template<typename S, typename GJKSolver>
-class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, GJKSolver, 0>
+template<typename S>
+class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, 0>
{
public:
MeshShapeCollisionTraversalNodeRSS (const CollisionRequest& request):
- MeshShapeCollisionTraversalNode<RSS, S, GJKSolver, 0>
+ MeshShapeCollisionTraversalNode<RSS, S, 0>
(request)
{
}
};
-template<typename S, typename GJKSolver>
-class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, GJKSolver, 0>
+template<typename S>
+class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, 0>
{
public:
MeshShapeCollisionTraversalNodekIOS(const CollisionRequest& request):
- MeshShapeCollisionTraversalNode<kIOS, S, GJKSolver, 0>
+ MeshShapeCollisionTraversalNode<kIOS, S, 0>
(request)
{
}
};
-template<typename S, typename GJKSolver>
-class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, GJKSolver, 0>
+template<typename S>
+class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, 0>
{
public:
MeshShapeCollisionTraversalNodeOBBRSS (const CollisionRequest& request) :
- MeshShapeCollisionTraversalNode <OBBRSS, S, GJKSolver, 0>
+ MeshShapeCollisionTraversalNode <OBBRSS, S, 0>
(request)
{
}
@@ -307,7 +307,7 @@ public:
/// @brief Traversal node for collision between shape and mesh
-template<typename S, typename BV, typename GJKSolver,
+template<typename S, typename BV,
int _Options = RelativeTransformationIsIdentity>
class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
{
@@ -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);
@@ -420,41 +420,41 @@ public:
};
/// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
-template<typename S, typename GJKSolver>
-class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, GJKSolver, 0>
+template<typename S>
+class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, 0>
{
public:
- ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB, GJKSolver>()
+ ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB>()
{
}
};
-template<typename S, typename GJKSolver>
-class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, GJKSolver, 0>
+template<typename S>
+class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, 0>
{
public:
- ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS, GJKSolver>()
+ ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS>()
{
}
};
-template<typename S, typename GJKSolver>
-class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, GJKSolver, 0>
+template<typename S>
+class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, 0>
{
public:
- ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS, GJKSolver>()
+ ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS>()
{
}
};
-template<typename S, typename GJKSolver>
-class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, GJKSolver, 0>
+template<typename S>
+class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, 0>
{
public:
- ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS, GJKSolver>()
+ ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS>()
{
}
};
@@ -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);
}
@@ -557,7 +557,7 @@ public:
/// @brief Traversal node for distance between mesh and shape
-template<typename BV, typename S, typename GJKSolver>
+template<typename BV, typename S>
class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode<BV, S>
{
public:
@@ -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++;
@@ -619,8 +619,8 @@ public:
namespace details
{
-template<typename BV, typename S, typename GJKSolver>
-void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */,
+template<typename BV, typename S>
+void meshShapeDistanceOrientedNodeleafComputeDistance(int b1, int /* b2 */,
const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices,
const Transform3f& tf1,
@@ -651,7 +651,7 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */,
}
-template<typename BV, typename S, typename GJKSolver>
+template<typename BV, typename S>
static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
const S& model2, const Transform3f& tf1, const Transform3f& tf2,
@@ -682,11 +682,11 @@ static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
/// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS)
-template<typename S, typename GJKSolver>
-class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S, GJKSolver>
+template<typename S>
+class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S>
{
public:
- MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode<RSS, S, GJKSolver>()
+ MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode<RSS, S>()
{
}
@@ -700,25 +700,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 GJKSolver>
-class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode<kIOS, S, GJKSolver>
+template<typename S>
+class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode<kIOS, S>
{
public:
- MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode<kIOS, S, GJKSolver>()
+ MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode<kIOS, S>()
{
}
@@ -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>
+class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S>
{
public:
- MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
+ MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S>()
{
}
@@ -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>
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>
+class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS>
{
public:
- ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>()
+ ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS>()
{
}
@@ -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>
+class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS>
{
public:
- ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>()
+ ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS>()
{
}
@@ -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>
+class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS>
{
public:
- ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
+ ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS>()
{
}
@@ -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..b35e9783846d6618b792e8f6a3dd6828adc1d3e6 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 "../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..c8f0f52220c39f9babaf1082b429ebcfcd0ebacf 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,10 @@ namespace fcl
{
/// @brief Algorithms for collision related with octree
-template<typename NarrowPhaseSolver>
class OcTreeSolver
{
private:
- const NarrowPhaseSolver* solver;
+ const GJKSolver* solver;
mutable const CollisionRequest* crequest;
mutable const DistanceRequest* drequest;
@@ -64,7 +63,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 +889,6 @@ private:
/// @brief Traversal node for octree collision
-template<typename NarrowPhaseSolver>
class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -903,17 +901,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 +921,10 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for octree distance
-template<typename NarrowPhaseSolver>
class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -940,17 +937,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 +955,11 @@ public:
const OcTree* model1;
const OcTree* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for shape-octree collision
-template<typename S, typename NarrowPhaseSolver>
+template<typename S>
class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -975,17 +972,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 +992,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for octree-shape collision
-template<typename S, typename NarrowPhaseSolver>
+template<typename S>
class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -1012,17 +1009,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 +1029,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for shape-octree distance
-template<typename S, typename NarrowPhaseSolver>
+template<typename S>
class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1048,12 +1045,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 +1058,11 @@ public:
const S* model1;
const OcTree* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for octree-shape distance
-template<typename S, typename NarrowPhaseSolver>
+template<typename S>
class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1077,12 +1074,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 +1087,11 @@ public:
const OcTree* model1;
const S* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for mesh-octree collision
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV>
class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -1107,17 +1104,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 +1124,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for octree-mesh collision
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV>
class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -1144,17 +1141,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 +1161,11 @@ public:
Transform3f tf1, tf2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for mesh-octree distance
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV>
class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1180,12 +1177,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 +1190,12 @@ public:
const BVHModel<BV>* model1;
const OcTree* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* otsolver;
};
/// @brief Traversal node for octree-mesh distance
-template<typename BV, typename NarrowPhaseSolver>
+template<typename BV>
class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
@@ -1210,12 +1207,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 +1220,7 @@ public:
const OcTree* model1;
const BVHModel<BV>* model2;
- const OcTreeSolver<NarrowPhaseSolver>* otsolver;
+ const OcTreeSolver* 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 71%
rename from include/hpp/fcl/traversal/traversal_node_setup.h
rename to src/traversal/traversal_node_setup.h
index 0912c231d15a567b242911d9e8ee60780ff5fc87..85cdfbc57c8d0868b54b13694af11625b38b1319 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,12 +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,
- const OcTree& model1, const Transform3f& tf1,
- const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- CollisionResult& result)
+inline bool initialize(OcTreeCollisionTraversalNode& node,
+ const OcTree& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
+ const OcTreeSolver* otsolver,
+ CollisionResult& result)
{
node.result = &result;
@@ -77,13 +76,12 @@ 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,
- const OcTree& model1, const Transform3f& tf1,
- const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
- const DistanceRequest& request,
- DistanceResult& result)
+inline bool initialize(OcTreeDistanceTraversalNode& node,
+ const OcTree& model1, const Transform3f& tf1,
+ const OcTree& model2, const Transform3f& tf2,
+ const OcTreeSolver* otsolver,
+ const DistanceRequest& request,
+ DistanceResult& result)
{
node.request = request;
node.result = &result;
@@ -100,11 +98,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>
+bool initialize(ShapeOcTreeCollisionTraversalNode<S>& node,
const S& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -121,11 +119,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>
+bool initialize(OcTreeShapeCollisionTraversalNode<S>& node,
const OcTree& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -142,11 +140,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>
+bool initialize(ShapeOcTreeDistanceTraversalNode<S>& node,
const S& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -165,11 +163,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>
+bool initialize(OcTreeShapeDistanceTraversalNode<S>& node,
const OcTree& model1, const Transform3f& tf1,
const S& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -188,11 +186,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>
+bool initialize(MeshOcTreeCollisionTraversalNode<BV>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -209,11 +207,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>
+bool initialize(OcTreeMeshCollisionTraversalNode<BV>& node,
const OcTree& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
CollisionResult& result)
{
node.result = &result;
@@ -230,11 +228,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>
+bool initialize(MeshOcTreeDistanceTraversalNode<BV>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
const OcTree& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -253,11 +251,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>
+bool initialize(OcTreeMeshDistanceTraversalNode<BV>& node,
const OcTree& model1, const Transform3f& tf1,
const BVHModel<BV>& model2, const Transform3f& tf2,
- const OcTreeSolver<NarrowPhaseSolver>* otsolver,
+ const OcTreeSolver* otsolver,
const DistanceRequest& request,
DistanceResult& result)
{
@@ -279,11 +277,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>
+bool initialize(ShapeCollisionTraversalNode<S1, S2>& 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 +296,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>
+bool initialize(MeshShapeCollisionTraversalNode<BV, S>& 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 +340,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, template<typename> class OrientedNode>
+static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S>& 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 +376,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>
+bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& 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>
+bool initialize(MeshShapeCollisionTraversalNodeRSS<S>& 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>
+bool initialize(MeshShapeCollisionTraversalNodekIOS<S>& 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>
+bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S>& 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 +423,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, template<typename> class OrientedNode>
+static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S>& 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 +451,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 +542,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>
+bool initialize(ShapeDistanceTraversalNode<S1, S2>& 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 +648,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>
+bool initialize(MeshShapeDistanceTraversalNode<BV, S>& 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 +695,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>
+bool initialize(ShapeMeshDistanceTraversalNode<S, BV>& 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 +745,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, template<typename> class OrientedNode>
+static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S>& 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 +776,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>
+bool initialize(MeshShapeDistanceTraversalNodeRSS<S>& 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 +788,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>
+bool initialize(MeshShapeDistanceTraversalNodekIOS<S>& 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 +800,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>
+bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S>& 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 +814,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, template<typename> class OrientedNode>
+static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S>& 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 +847,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>
+bool initialize(ShapeMeshDistanceTraversalNodeRSS<S>& 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 +859,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>
+bool initialize(ShapeMeshDistanceTraversalNodekIOS<S>& 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 +871,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>
+bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S>& 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..902527343c5e3bba0a56e595c8558330dc68892c 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>
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>
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..5aa55c448893b536e1efc23988af5fda1597ed03 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 afc040a13dbab940f701829043a0c1e32ecf4482..156e8afe104dfe7e5480f0ffc386bce3111447e1 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..87062cb9e864194d55c639086e2db6305809a9de 100644
--- a/test/math.cpp
+++ b/test/math.cpp
@@ -39,11 +39,11 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/intersect.h>
-#include <hpp/fcl/math/tools.h>
+#include "../src/intersect.h"
+#include "../src/math/tools.h"
using namespace hpp::fcl;
diff --git a/test/profiling.cpp b/test/profiling.cpp
index 6bfcfab8de5d28d4386f1d2147b9a6b63cd8647b..2e96e4c82abe0ece23c4d3c4398b91c708441db2 100644
--- a/test/profiling.cpp
+++ b/test/profiling.cpp
@@ -31,7 +31,7 @@
using namespace hpp::fcl;
-CollisionFunctionMatrix<GJKSolver> lookupTable;
+CollisionFunctionMatrix lookupTable;
bool supportedPair(const CollisionGeometry* o1, const CollisionGeometry* o2)
{
OBJECT_TYPE object_type1 = o1->getObjectType();
diff --git a/test/simple.cpp b/test/simple.cpp
index 725f6ac657d43743e8a668afdc36f9428d962979..67ccd8699ec559fabc498f58ff6b3da59368c207 100644
--- a/test/simple.cpp
+++ b/test/simple.cpp
@@ -3,7 +3,7 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include <hpp/fcl/intersect.h>
+#include "../src/intersect.h"
#include <hpp/fcl/collision.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include "fcl_resources/config.h"
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
{