From 868f9ee8508db6b04562391b6f78b7905cf17abb Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 9 Jul 2012 08:37:35 +0000
Subject: [PATCH] a faster (only a bit) implementation of dynamicAABBTree.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@122 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/BV/AABB.h | 24 +-
trunk/fcl/include/fcl/BV/OBB.h | 12 +-
trunk/fcl/include/fcl/BV/OBBRSS.h | 14 +-
trunk/fcl/include/fcl/BV/RSS.h | 26 +-
trunk/fcl/include/fcl/BV/kDOP.h | 36 +-
trunk/fcl/include/fcl/BV/kIOS.h | 20 +-
trunk/fcl/include/fcl/BVH_internal.h | 4 +-
trunk/fcl/include/fcl/BVH_utility.h | 12 +-
trunk/fcl/include/fcl/BV_node.h | 6 +-
trunk/fcl/include/fcl/BV_splitter.h | 14 +-
trunk/fcl/include/fcl/broad_phase_collision.h | 184 +++-
trunk/fcl/include/fcl/collision_data.h | 8 +-
trunk/fcl/include/fcl/collision_object.h | 2 +-
.../include/fcl/conservative_advancement.h | 2 +-
.../deprecated/geometric_shapes_intersect.cpp | 4 +-
.../deprecated/geometric_shapes_intersect.h | 12 +-
trunk/fcl/include/fcl/deprecated/gjk.h | 30 +-
trunk/fcl/include/fcl/deprecated/vec_3f.h | 62 +-
trunk/fcl/include/fcl/distance.h | 8 +-
trunk/fcl/include/fcl/distance_func_matrix.h | 2 +-
trunk/fcl/include/fcl/geometric_shapes.h | 36 +-
trunk/fcl/include/fcl/hierarchy_tree.h | 864 ++++++++++++++++--
trunk/fcl/include/fcl/intersect.h | 88 +-
trunk/fcl/include/fcl/interval.h | 34 +-
trunk/fcl/include/fcl/interval_matrix.h | 6 +-
trunk/fcl/include/fcl/interval_vector.h | 10 +-
trunk/fcl/include/fcl/math_details.h | 16 +
trunk/fcl/include/fcl/math_simd_details.h | 32 +-
trunk/fcl/include/fcl/matrix_3f.h | 48 +-
trunk/fcl/include/fcl/motion.h | 118 +--
trunk/fcl/include/fcl/motion_base.h | 4 +-
trunk/fcl/include/fcl/narrowphase/gjk.h | 26 +-
.../fcl/include/fcl/narrowphase/gjk_libccd.h | 8 +-
.../fcl/include/fcl/narrowphase/narrowphase.h | 68 +-
trunk/fcl/include/fcl/primitive.h | 2 +-
trunk/fcl/include/fcl/simd_intersect.h | 72 +-
trunk/fcl/include/fcl/simple_setup.h | 28 +-
trunk/fcl/include/fcl/taylor_matrix.h | 2 +-
trunk/fcl/include/fcl/taylor_model.h | 26 +-
trunk/fcl/include/fcl/taylor_vector.h | 6 +-
trunk/fcl/include/fcl/transform.h | 30 +-
trunk/fcl/include/fcl/traversal_node_base.h | 4 +-
.../include/fcl/traversal_node_bvh_shape.h | 66 +-
trunk/fcl/include/fcl/traversal_node_bvhs.h | 124 +--
trunk/fcl/include/fcl/traversal_node_shapes.h | 6 +-
trunk/fcl/include/fcl/vec_3f.h | 14 +-
trunk/fcl/src/BV/AABB.cpp | 38 +-
trunk/fcl/src/BV/OBB.cpp | 22 +-
trunk/fcl/src/BV/OBBRSS.cpp | 2 +-
trunk/fcl/src/BV/RSS.cpp | 116 +--
trunk/fcl/src/BV/kIOS.cpp | 30 +-
trunk/fcl/src/BVH_model.cpp | 8 +-
trunk/fcl/src/BVH_utility.cpp | 76 +-
trunk/fcl/src/BV_fitter.cpp | 68 +-
trunk/fcl/src/BV_splitter.cpp | 12 +-
trunk/fcl/src/broad_phase_collision.cpp | 433 +++++++--
trunk/fcl/src/conservative_advancement.cpp | 6 +-
trunk/fcl/src/distance.cpp | 18 +-
trunk/fcl/src/distance_func_matrix.cpp | 20 +-
trunk/fcl/src/geometric_shapes.cpp | 4 +-
trunk/fcl/src/geometric_shapes_utility.cpp | 86 +-
trunk/fcl/src/gjk.cpp | 102 +--
trunk/fcl/src/gjk_libccd.cpp | 8 +-
trunk/fcl/src/hierarchy_tree.cpp | 42 +-
trunk/fcl/src/intersect.cpp | 330 +++----
trunk/fcl/src/interval.cpp | 12 +-
trunk/fcl/src/interval_matrix.cpp | 22 +-
trunk/fcl/src/interval_vector.cpp | 10 +-
trunk/fcl/src/matrix_3f.cpp | 20 +-
trunk/fcl/src/motion.cpp | 44 +-
trunk/fcl/src/narrowphase.cpp | 186 ++--
trunk/fcl/src/simple_setup.cpp | 24 +-
trunk/fcl/src/taylor_matrix.cpp | 10 +-
trunk/fcl/src/taylor_model.cpp | 126 +--
trunk/fcl/src/taylor_vector.cpp | 6 +-
trunk/fcl/src/transform.cpp | 82 +-
trunk/fcl/src/traversal_node_base.cpp | 6 +-
trunk/fcl/src/traversal_node_bvhs.cpp | 84 +-
trunk/fcl/src/traversal_recurse.cpp | 6 +-
79 files changed, 2769 insertions(+), 1510 deletions(-)
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index cdd6a303..248d2c13 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -130,16 +130,16 @@ public:
/** \brief Merge the AABB and a point */
inline AABB& operator += (const Vec3f& p)
{
- min_ = min(min_, p);
- max_ = max(max_, p);
+ min_.ubound(p);
+ max_.lbound(p);
return *this;
}
/** \brief Merge the AABB and another AABB */
inline AABB& operator += (const AABB& other)
{
- min_ = min(min_, other.min_);
- max_ = max(max_, other.max_);
+ min_.ubound(other.min_);
+ max_.lbound(other.max_);
return *this;
}
@@ -151,31 +151,31 @@ public:
}
/** \brief Width of the AABB */
- inline BVH_REAL width() const
+ inline FCL_REAL width() const
{
return max_[0] - min_[0];
}
/** \brief Height of the AABB */
- inline BVH_REAL height() const
+ inline FCL_REAL height() const
{
return max_[1] - min_[1];
}
/** \brief Depth of the AABB */
- inline BVH_REAL depth() const
+ inline FCL_REAL depth() const
{
return max_[2] - min_[2];
}
/** \brief Volume of the AABB */
- inline BVH_REAL volume() const
+ inline FCL_REAL volume() const
{
return width() * height() * depth();
}
/** \brief Size of the AABB, for split order */
- inline BVH_REAL size() const
+ inline FCL_REAL size() const
{
return (max_ - min_).sqrLength();
}
@@ -186,9 +186,9 @@ public:
return (min_ + max_) * 0.5;
}
- BVH_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
+ FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
- BVH_REAL distance(const AABB& other) const;
+ FCL_REAL distance(const AABB& other) const;
inline bool equal(const AABB& other) const
{
@@ -203,7 +203,7 @@ public:
}
/**\brief expand the aabb by increase the 'thickness of the plate by a ratio */
- inline AABB& expand(const AABB& core, BVH_REAL ratio)
+ inline AABB& expand(const AABB& core, FCL_REAL ratio)
{
min_ = min_ * ratio - core.min_;
max_ = max_ * ratio - core.max_;
diff --git a/trunk/fcl/include/fcl/BV/OBB.h b/trunk/fcl/include/fcl/BV/OBB.h
index 619902bb..a3978113 100644
--- a/trunk/fcl/include/fcl/BV/OBB.h
+++ b/trunk/fcl/include/fcl/BV/OBB.h
@@ -88,31 +88,31 @@ public:
OBB operator + (const OBB& other) const;
/** \brief Width of the OBB */
- inline BVH_REAL width() const
+ inline FCL_REAL width() const
{
return 2 * extent[0];
}
/** \brief Height of the OBB */
- inline BVH_REAL height() const
+ inline FCL_REAL height() const
{
return 2 * extent[1];
}
/** \brief Depth of the OBB */
- inline BVH_REAL depth() const
+ inline FCL_REAL depth() const
{
return 2 * extent[2];
}
/** \brief Volume of the OBB */
- inline BVH_REAL volume() const
+ inline FCL_REAL volume() const
{
return width() * height() * depth();
}
/** \brief Size of the OBB, for split order */
- inline BVH_REAL size() const
+ inline FCL_REAL size() const
{
return extent.sqrLength();
}
@@ -126,7 +126,7 @@ public:
/** \brief The distance between two OBB
* Not implemented
*/
- BVH_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+ FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
diff --git a/trunk/fcl/include/fcl/BV/OBBRSS.h b/trunk/fcl/include/fcl/BV/OBBRSS.h
index d8d9050b..7d9b42db 100644
--- a/trunk/fcl/include/fcl/BV/OBBRSS.h
+++ b/trunk/fcl/include/fcl/BV/OBBRSS.h
@@ -91,27 +91,27 @@ public:
return result;
}
- inline BVH_REAL width() const
+ inline FCL_REAL width() const
{
return obb.width();
}
- inline BVH_REAL height() const
+ inline FCL_REAL height() const
{
return obb.height();
}
- inline BVH_REAL depth() const
+ inline FCL_REAL depth() const
{
return obb.depth();
}
- inline BVH_REAL volume() const
+ inline FCL_REAL volume() const
{
return obb.volume();
}
- inline BVH_REAL size() const
+ inline FCL_REAL size() const
{
return obb.size();
}
@@ -121,7 +121,7 @@ public:
return obb.center();
}
- BVH_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
+ FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
{
return rss.distance(other.rss, P, Q);
}
@@ -130,7 +130,7 @@ public:
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
-BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
}
diff --git a/trunk/fcl/include/fcl/BV/RSS.h b/trunk/fcl/include/fcl/BV/RSS.h
index 83cf4281..130fcf04 100644
--- a/trunk/fcl/include/fcl/BV/RSS.h
+++ b/trunk/fcl/include/fcl/BV/RSS.h
@@ -54,10 +54,10 @@ public:
Vec3f Tr;
/** \brief side lengths of rectangle */
- BVH_REAL l[2];
+ FCL_REAL l[2];
/** \brief radius of sphere summed with rectangle to form RSS */
- BVH_REAL r;
+ FCL_REAL r;
RSS() {}
@@ -91,31 +91,31 @@ public:
RSS operator + (const RSS& other) const;
/** \brief Width of the RSS */
- inline BVH_REAL width() const
+ inline FCL_REAL width() const
{
return l[0] + 2 * r;
}
/** \brief Height of the RSS */
- inline BVH_REAL height() const
+ inline FCL_REAL height() const
{
return l[1] + 2 * r;
}
/** \brief Depth of the RSS */
- inline BVH_REAL depth() const
+ inline FCL_REAL depth() const
{
return 2 * r;
}
/** \brief Volume of the RSS */
- inline BVH_REAL volume() const
+ inline FCL_REAL volume() const
{
return (l[0] * l[1] * 2 * r + 4 * 3.1415926 * r * r * r);
}
/** \brief Size of the RSS, for split order */
- inline BVH_REAL size() const
+ inline FCL_REAL size() const
{
return (sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
}
@@ -128,12 +128,12 @@ public:
/** \brief the distance between two RSS */
- BVH_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+ FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
protected:
/** \brief Clip val between a and b */
- static void clipToRange(BVH_REAL& val, BVH_REAL a, BVH_REAL b);
+ static void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b);
/** \brief Finds the parameters t & u corresponding to the two closest points on a pair of line segments.
* The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector
@@ -144,7 +144,7 @@ protected:
* instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb.
* Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
*/
- static void segCoords(BVH_REAL& t, BVH_REAL& u, BVH_REAL a, BVH_REAL b, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T);
+ static void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T);
/** \brief Returns whether the nearest point on rectangle edge
* Pb + B*u, 0 <= u <= b, to the rectangle edge,
@@ -154,14 +154,14 @@ protected:
* A,B, and Anorm are unit vectors.
* T is the vector between Pa and Pb.
*/
- static bool inVoronoi(BVH_REAL a, BVH_REAL b, BVH_REAL Anorm_dot_B, BVH_REAL Anorm_dot_T, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T);
+ static bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T);
public:
/** \brief distance between two oriented rectangles
* P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle
*/
- static BVH_REAL rectDistance(const Matrix3f& Rab, const Vec3f& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
+ static FCL_REAL rectDistance(const Matrix3f& Rab, const Vec3f& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
};
@@ -169,7 +169,7 @@ public:
* P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
* Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
*/
-BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
diff --git a/trunk/fcl/include/fcl/BV/kDOP.h b/trunk/fcl/include/fcl/BV/kDOP.h
index 6c59b40f..474730dc 100644
--- a/trunk/fcl/include/fcl/BV/kDOP.h
+++ b/trunk/fcl/include/fcl/BV/kDOP.h
@@ -49,7 +49,7 @@ namespace fcl
{
/** \brief Find the smaller and larger one of two values */
-inline void minmax(BVH_REAL a, BVH_REAL b, BVH_REAL& minv, BVH_REAL& maxv)
+inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv)
{
if(a > b)
{
@@ -63,7 +63,7 @@ inline void minmax(BVH_REAL a, BVH_REAL b, BVH_REAL& minv, BVH_REAL& maxv)
}
}
/** \brief Merge the interval [minv, maxv] and value p */
-inline void minmax(BVH_REAL p, BVH_REAL& minv, BVH_REAL& maxv)
+inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv)
{
if(p > maxv) maxv = p;
if(p < minv) minv = p;
@@ -72,11 +72,11 @@ inline void minmax(BVH_REAL p, BVH_REAL& minv, BVH_REAL& maxv)
/** \brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes */
template<size_t N>
-void getDistances(const Vec3f& p, BVH_REAL d[]) {}
+void getDistances(const Vec3f& p, FCL_REAL d[]) {}
/** \brief Specification of getDistances */
template<>
-inline void getDistances<5>(const Vec3f& p, BVH_REAL d[])
+inline void getDistances<5>(const Vec3f& p, FCL_REAL d[])
{
d[0] = p[0] + p[1];
d[1] = p[0] + p[2];
@@ -86,7 +86,7 @@ inline void getDistances<5>(const Vec3f& p, BVH_REAL d[])
}
template<>
-inline void getDistances<6>(const Vec3f& p, BVH_REAL d[])
+inline void getDistances<6>(const Vec3f& p, FCL_REAL d[])
{
d[0] = p[0] + p[1];
d[1] = p[0] + p[2];
@@ -97,7 +97,7 @@ inline void getDistances<6>(const Vec3f& p, BVH_REAL d[])
}
template<>
-inline void getDistances<9>(const Vec3f& p, BVH_REAL d[])
+inline void getDistances<9>(const Vec3f& p, FCL_REAL d[])
{
d[0] = p[0] + p[1];
d[1] = p[0] + p[2];
@@ -130,7 +130,7 @@ class KDOP
public:
KDOP()
{
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
for(size_t i = 0; i < N / 2; ++i)
{
dist_[i] = real_max;
@@ -146,7 +146,7 @@ public:
dist_[i] = dist_[N / 2 + i] = v[i];
}
- BVH_REAL d[(N - 6) / 2];
+ FCL_REAL d[(N - 6) / 2];
getDistances<(N - 6) / 2>(v, d);
for(size_t i = 0; i < (N - 6) / 2; ++i)
{
@@ -161,7 +161,7 @@ public:
minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
}
- BVH_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
+ FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
getDistances<(N - 6) / 2>(a, ad);
getDistances<(N - 6) / 2>(b, bd);
for(size_t i = 0; i < (N - 6) / 2; ++i)
@@ -191,7 +191,7 @@ public:
return false;
}
- BVH_REAL d[(N - 6) / 2];
+ FCL_REAL d[(N - 6) / 2];
getDistances(p, d);
for(size_t i = 0; i < (N - 6) / 2; ++i)
{
@@ -210,7 +210,7 @@ public:
minmax(p[i], dist_[i], dist_[N / 2 + i]);
}
- BVH_REAL pd[(N - 6) / 2];
+ FCL_REAL pd[(N - 6) / 2];
getDistances<(N - 6) / 2>(p, pd);
for(size_t i = 0; i < (N - 6) / 2; ++i)
{
@@ -239,30 +239,30 @@ public:
}
/** \brief The (AABB) width */
- inline BVH_REAL width() const
+ inline FCL_REAL width() const
{
return dist_[N / 2] - dist_[0];
}
/** \brief The (AABB) height */
- inline BVH_REAL height() const
+ inline FCL_REAL height() const
{
return dist_[N / 2 + 1] - dist_[1];
}
/** \brief The (AABB) depth */
- inline BVH_REAL depth() const
+ inline FCL_REAL depth() const
{
return dist_[N / 2 + 2] - dist_[2];
}
/** \brief The (AABB) volume */
- inline BVH_REAL volume() const
+ inline FCL_REAL volume() const
{
return width() * height() * depth();
}
- inline BVH_REAL size() const
+ inline FCL_REAL size() const
{
return width() * width() + height() * height() + depth() * depth();
}
@@ -276,7 +276,7 @@ public:
/** \brief The distance between two KDOP<N>
* Not implemented.
*/
- BVH_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
+ FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
{
std::cerr << "KDOP distance not implemented!" << std::endl;
return 0.0;
@@ -284,7 +284,7 @@ public:
private:
/** \brief distances to N KDOP planes */
- BVH_REAL dist_[N];
+ FCL_REAL dist_[N];
};
diff --git a/trunk/fcl/include/fcl/BV/kIOS.h b/trunk/fcl/include/fcl/BV/kIOS.h
index 8532702e..99044ce0 100644
--- a/trunk/fcl/include/fcl/BV/kIOS.h
+++ b/trunk/fcl/include/fcl/BV/kIOS.h
@@ -52,14 +52,14 @@ class kIOS
struct kIOS_Sphere
{
Vec3f o;
- BVH_REAL r;
+ FCL_REAL r;
};
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
{
Vec3f d = s1.o - s0.o;
- BVH_REAL dist2 = d.sqrLength();
- BVH_REAL diff_r = s1.r - s0.r;
+ FCL_REAL dist2 = d.sqrLength();
+ FCL_REAL diff_r = s1.r - s0.r;
/** The sphere with the larger radius encloses the other */
if(diff_r * diff_r >= dist2)
@@ -124,22 +124,22 @@ public:
}
/** \brief width of the kIOS */
- BVH_REAL width() const;
+ FCL_REAL width() const;
/** \brief height of the kIOS */
- BVH_REAL height() const;
+ FCL_REAL height() const;
/** \brief depth of the kIOS */
- BVH_REAL depth() const;
+ FCL_REAL depth() const;
/** \brief volume of the kIOS */
- BVH_REAL volume() const;
+ FCL_REAL volume() const;
/** \brief size of the kIOS, for split order */
- BVH_REAL size() const;
+ FCL_REAL size() const;
/** \brief The distance between two kIOS */
- BVH_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+ FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
@@ -147,7 +147,7 @@ private:
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
-BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
}
diff --git a/trunk/fcl/include/fcl/BVH_internal.h b/trunk/fcl/include/fcl/BVH_internal.h
index 3aa68194..95897374 100644
--- a/trunk/fcl/include/fcl/BVH_internal.h
+++ b/trunk/fcl/include/fcl/BVH_internal.h
@@ -37,11 +37,11 @@
#ifndef FCL_BVH_INTERNAL_H
#define FCL_BVH_INTERNAL_H
+#include "fcl/data_types.h"
+
namespace fcl
{
-typedef double BVH_REAL;
-
/** \brief States for BVH construction
* empty->begun->processed ->replace_begun->processed -> ......
* |
diff --git a/trunk/fcl/include/fcl/BVH_utility.h b/trunk/fcl/include/fcl/BVH_utility.h
index 5dc2cdcf..059d94e5 100644
--- a/trunk/fcl/include/fcl/BVH_utility.h
+++ b/trunk/fcl/include/fcl/BVH_utility.h
@@ -47,7 +47,7 @@ namespace fcl
{
/** \brief Expand the BVH bounding boxes according to uncertainty */
template<typename BV>
-void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, BVH_REAL r)
+void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, FCL_REAL r)
{
for(int i = 0; i < model.num_bvs; ++i)
{
@@ -73,10 +73,10 @@ void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, BVH_REAL r)
}
/** \brief Expand the BVH bounding boxes according to uncertainty, for OBB */
-void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, BVH_REAL r);
+void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, FCL_REAL r);
/** \brief Expand the BVH bounding boxes according to uncertainty, for RSS */
-void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r);
+void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, FCL_REAL r);
/** \brief Estimate the uncertainty of point clouds due to sampling procedure */
void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs);
@@ -88,7 +88,7 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, i
/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
* The bounding volume axes are known.
*/
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r);
+void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
/** \brief Compute the bounding volume extent and center for a set or subset of points.
* The bounding volume axes are known.
@@ -96,10 +96,10 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
/** \brief Compute the center and radius for a triangle's circumcircle */
-void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius);
+void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius);
/** \brief Compute the maximum distance from a given center point to a point cloud */
-BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
+FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
}
diff --git a/trunk/fcl/include/fcl/BV_node.h b/trunk/fcl/include/fcl/BV_node.h
index bfafb835..1341a623 100644
--- a/trunk/fcl/include/fcl/BV_node.h
+++ b/trunk/fcl/include/fcl/BV_node.h
@@ -91,7 +91,7 @@ struct BVNode : public BVNodeBase
return bv.overlap(other.bv);
}
- BVH_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
+ FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
{
return bv.distance(other.bv, P1, P2);
}
@@ -114,7 +114,7 @@ struct BVNode<OBB> : public BVNodeBase
return bv.overlap(other.bv);
}
- BVH_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
+ FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
{
return bv.distance(other.bv, P1, P2);
}
@@ -148,7 +148,7 @@ struct BVNode<RSS> : public BVNodeBase
return bv.overlap(other.bv);
}
- BVH_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
+ FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
{
return bv.distance(other.bv, P1, P2);
}
diff --git a/trunk/fcl/include/fcl/BV_splitter.h b/trunk/fcl/include/fcl/BV_splitter.h
index 192dfeb6..d3e1025a 100644
--- a/trunk/fcl/include/fcl/BV_splitter.h
+++ b/trunk/fcl/include/fcl/BV_splitter.h
@@ -135,7 +135,7 @@ private:
int split_axis;
/** \brief The split threshold */
- BVH_REAL split_value;
+ FCL_REAL split_value;
Vec3f* vertices;
Triangle* tri_indices;
@@ -168,7 +168,7 @@ private:
axis = 1;
split_axis = axis;
- BVH_REAL sum = 0;
+ FCL_REAL sum = 0;
if(type == BVH_MODEL_TRIANGLES)
{
@@ -202,7 +202,7 @@ private:
axis = 1;
split_axis = axis;
- std::vector<BVH_REAL> proj(num_primitives);
+ std::vector<FCL_REAL> proj(num_primitives);
if(type == BVH_MODEL_TRIANGLES)
{
@@ -296,7 +296,7 @@ private:
/** \brief Split the node according to the median of the data contained */
void computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives);
- BVH_REAL split_value;
+ FCL_REAL split_value;
Vec3f split_vector;
Vec3f* vertices;
@@ -369,7 +369,7 @@ private:
/** \brief Split the node according to the median of the data contained */
void computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
- BVH_REAL split_value;
+ FCL_REAL split_value;
Vec3f split_vector;
Vec3f* vertices;
@@ -442,7 +442,7 @@ private:
/** \brief Split the node according to the median of the data contained */
void computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
- BVH_REAL split_value;
+ FCL_REAL split_value;
Vec3f split_vector;
Vec3f* vertices;
@@ -513,7 +513,7 @@ private:
/** \brief Split the node according to the median of the data contained */
void computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives);
- BVH_REAL split_value;
+ FCL_REAL split_value;
Vec3f split_vector;
Vec3f* vertices;
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index c1a2db2c..b75bfd46 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -60,12 +60,12 @@ namespace fcl
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
/** \brief distance function for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now */
-bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, BVH_REAL& dist);
+bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
/** \brief return value is whether can stop now */
typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
-typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, BVH_REAL& dist);
+typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
/** \brief Base class for broad phase collision */
class BroadPhaseCollisionManager
@@ -218,7 +218,7 @@ protected:
/** \brief Spatial hash function: hash an AABB to a set of integer values */
struct SpatialHash
{
- SpatialHash(const AABB& scene_limit_, BVH_REAL cell_size_)
+ SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_)
{
cell_size = cell_size_;
scene_limit = scene_limit_;
@@ -253,7 +253,7 @@ struct SpatialHash
private:
- BVH_REAL cell_size;
+ FCL_REAL cell_size;
AABB scene_limit;
unsigned int width[3];
};
@@ -263,7 +263,7 @@ template<typename HashTable = SimpleHashTable<AABB, CollisionObject*, SpatialHas
class SpatialHashingCollisionManager : public BroadPhaseCollisionManager
{
public:
- SpatialHashingCollisionManager(BVH_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000)
+ SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000)
{
scene_limit = AABB(scene_min, scene_max);
SpatialHash hasher(scene_limit, cell_size);
@@ -341,7 +341,7 @@ protected:
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
/** \brief perform distance computation between one object and all the objects belonging ot the manager */
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
// all objects in the scene
@@ -496,7 +496,7 @@ void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, vo
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
@@ -541,13 +541,13 @@ bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, v
}
template<typename HashTable>
-bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(size() == 0) return false;
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
AABB aabb = obj->getAABB();
- if(min_dist < std::numeric_limits<BVH_REAL>::max())
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
@@ -556,7 +556,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
AABB overlap_aabb;
int status = 1;
- BVH_REAL old_min_distance;
+ FCL_REAL old_min_distance;
while(1)
{
@@ -632,7 +632,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
if(status == 1)
{
- if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
break;
else
{
@@ -703,7 +703,7 @@ void SpatialHashingCollisionManager<HashTable>::distance(void* cdata, DistanceCa
enable_tested_set_ = true;
tested_set.clear();
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
for(std::list<CollisionObject*>::const_iterator it = objs.begin(); it != objs.end(); ++it)
if(distance_(*it, cdata, callback, min_dist)) break;
@@ -744,7 +744,7 @@ void SpatialHashingCollisionManager<HashTable>::distance(BroadPhaseCollisionMana
return;
}
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
if(this->size() < other_manager->size())
{
@@ -889,13 +889,13 @@ protected:
else return aabb->cached.min_;
}
- inline BVH_REAL getVal(size_t i) const
+ inline FCL_REAL getVal(size_t i) const
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
}
- inline BVH_REAL& getVal(size_t i)
+ inline FCL_REAL& getVal(size_t i)
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
@@ -999,7 +999,7 @@ protected:
std::map<CollisionObject*, SaPAABB*> obj_aabb_map;
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
@@ -1099,12 +1099,12 @@ protected:
/** \brief check distance between one object and a list of objects, return value is whether stop is possible */
bool checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
static inline size_t selectOptimalAxis(const std::vector<CollisionObject*>& objs_x, const std::vector<CollisionObject*>& objs_y, const std::vector<CollisionObject*>& objs_z, std::vector<CollisionObject*>::const_iterator& it_beg, std::vector<CollisionObject*>::const_iterator& it_end)
{
@@ -1226,7 +1226,7 @@ protected:
CollisionObject* obj;
/** \brief end point value */
- BVH_REAL value;
+ FCL_REAL value;
/** \brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi */
char minmax;
@@ -1247,11 +1247,11 @@ protected:
bool checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
- bool checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
- bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
/** \brief vector stores all the end points */
std::vector<EndPoint> endpoints[3];
@@ -1328,7 +1328,7 @@ public:
/** \brief perform distance computation between one object and all the objects belonging to the manager */
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
}
@@ -1341,7 +1341,7 @@ public:
/** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
void distance(void* cdata, DistanceCallBack callback) const
{
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
}
@@ -1356,7 +1356,7 @@ public:
void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
}
@@ -1385,16 +1385,146 @@ private:
bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const;
- bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
- bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
- bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+ bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
void update_(CollisionObject* updated_obj);
};
+
+
+class DynamicAABBTreeCollisionManager2 : public BroadPhaseCollisionManager
+{
+public:
+ typedef alternative::NodeBase<AABB> DynamicAABBNode;
+ typedef boost::unordered_map<CollisionObject*, size_t> DynamicAABBTable;
+
+ int max_tree_nonbalanced_level;
+ int tree_incremental_balance_pass;
+ int tree_topdown_balance_threshold;
+
+ DynamicAABBTreeCollisionManager2()
+ {
+ max_tree_nonbalanced_level = 10;
+ tree_incremental_balance_pass = 10;
+ tree_topdown_balance_threshold = 2;
+ setup_ = false;
+ }
+
+ /** \brief add objects to the manager */
+ void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
+ /** \brief add one object to the manager */
+ void registerObject(CollisionObject* obj);
+
+ /** \brief remove one object from the manager */
+ void unregisterObject(CollisionObject* obj);
+
+ /** \brief initialize the manager, related with the specific type of manager */
+ void setup();
+
+ /** \brief update the condition of manager */
+ void update();
+
+ /** \brief update the manager by explicitly given the object updated */
+ void update(CollisionObject* updated_obj);
+
+ /** \brief update the manager by explicitly given the set of objects update */
+ void update(const std::vector<CollisionObject*>& updated_objs);
+
+ /** \brief clear the manager */
+ void clear()
+ {
+ dtree.clear();
+ table.clear();
+ }
+
+ /** \brief return the objects managed by the manager */
+ void getObjects(std::vector<CollisionObject*>& objs) const
+ {
+ objs.resize(this->size());
+ std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1));
+ }
+
+ /** \brief perform collision test between one object and all the objects belonging to the manager */
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+ {
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
+ }
+
+ /** \brief perform distance computation between one object and all the objects belonging to the manager */
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+ {
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
+ }
+
+ /** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
+ void collide(void* cdata, CollisionCallBack callback) const
+ {
+ selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
+ }
+
+ /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
+ void distance(void* cdata, DistanceCallBack callback) const
+ {
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
+ }
+
+ /** \brief perform collision test with objects belonging to another manager */
+ void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+ {
+ DynamicAABBTreeCollisionManager2* other_manager = static_cast<DynamicAABBTreeCollisionManager2*>(other_manager_);
+ collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
+ }
+
+ /** \brief perform distance test with objects belonging to another manager */
+ void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+ {
+ DynamicAABBTreeCollisionManager2* other_manager = static_cast<DynamicAABBTreeCollisionManager2*>(other_manager_);
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
+ distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
+ }
+
+ /** \brief whether the manager is empty */
+ bool empty() const
+ {
+ return dtree.empty();
+ }
+
+ /** \brief the number of objects managed by the manager */
+ size_t size() const
+ {
+ return dtree.size();
+ }
+
+
+private:
+ alternative::HierarchyTree<AABB> dtree;
+ boost::unordered_map<CollisionObject*, size_t> table;
+
+ bool setup_;
+
+ bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, CollisionCallBack callback) const;
+
+ bool collisionRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, CollisionCallBack callback) const;
+
+ bool selfCollisionRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, CollisionCallBack callback) const;
+
+ bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1, DynamicAABBNode* nodes2, size_t root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ bool distanceRecurse(DynamicAABBNode* nodes, size_t root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ bool selfDistanceRecurse(DynamicAABBNode* nodes, size_t root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
+
+ void update_(CollisionObject* updated_obj);
+};
+
}
#endif
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index f543be0a..c22eec13 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -12,7 +12,7 @@ namespace fcl
struct Contact
{
- BVH_REAL penetration_depth;
+ FCL_REAL penetration_depth;
Vec3f normal;
Vec3f pos;
const CollisionGeometry* o1;
@@ -37,7 +37,7 @@ struct Contact
}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
- const Vec3f& pos_, const Vec3f& normal_, BVH_REAL depth_)
+ const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_)
{
o1 = o1_;
o2 = o2_;
@@ -73,11 +73,11 @@ struct DistanceData
{
DistanceData()
{
- min_distance = std::numeric_limits<BVH_REAL>::max();
+ min_distance = std::numeric_limits<FCL_REAL>::max();
done = false;
}
- BVH_REAL min_distance;
+ FCL_REAL min_distance;
bool done;
};
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 9a274085..5b522945 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -75,7 +75,7 @@ public:
Vec3f aabb_center;
/** AABB radius */
- BVH_REAL aabb_radius;
+ FCL_REAL aabb_radius;
/** pointer to user defined data specific to this object */
void *user_data;
diff --git a/trunk/fcl/include/fcl/conservative_advancement.h b/trunk/fcl/include/fcl/conservative_advancement.h
index 9ef9f322..df2fc0fd 100644
--- a/trunk/fcl/include/fcl/conservative_advancement.h
+++ b/trunk/fcl/include/fcl/conservative_advancement.h
@@ -54,7 +54,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MotionBase<BV>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
- BVH_REAL& toc);
+ FCL_REAL& toc);
}
diff --git a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
index 46dfe608..6158e1fc 100644
--- a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
+++ b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
@@ -686,7 +686,7 @@ static void centerTriangle(const void* obj, ccd_vec3_t* c)
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal)
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
ccd_t ccd;
int res;
@@ -723,7 +723,7 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
- BVH_REAL* res)
+ FCL_REAL* res)
{
ccd_t ccd;
ccd_real_t dist;
diff --git a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
index a6ed5b38..78e5a513 100644
--- a/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
+++ b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
@@ -155,11 +155,11 @@ void triDeleteGJKObject(void* o);
/** \brief GJK collision algorithm */
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal);
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
- BVH_REAL* dist);
+ FCL_REAL* dist);
} // details
@@ -169,7 +169,7 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1,
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+ Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
@@ -187,7 +187,7 @@ bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
/** \brief intersection checking between one shape and a triangle */
template<typename S>
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3);
@@ -206,7 +206,7 @@ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
template<typename S>
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+ Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
@@ -226,7 +226,7 @@ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- BVH_REAL* dist)
+ FCL_REAL* dist)
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
diff --git a/trunk/fcl/include/fcl/deprecated/gjk.h b/trunk/fcl/include/fcl/deprecated/gjk.h
index 595f298d..b5946cb4 100644
--- a/trunk/fcl/include/fcl/deprecated/gjk.h
+++ b/trunk/fcl/include/fcl/deprecated/gjk.h
@@ -83,14 +83,14 @@ struct MinkowskiDiff
};
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m);
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m);
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL* w, size_t& m);
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m);
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, BVH_REAL* w, size_t& m);
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m);
-static const BVH_REAL GJK_EPS = 0.000001;
+static const FCL_REAL GJK_EPS = 0.000001;
static const size_t GJK_MAX_ITERATIONS = 128;
struct GJK
@@ -104,7 +104,7 @@ struct GJK
struct Simplex
{
SimplexV* c[4]; // simplex vertex
- BVH_REAL p[4]; // weight
+ FCL_REAL p[4]; // weight
size_t rank; // size of simplex (number of vertices)
};
@@ -112,7 +112,7 @@ struct GJK
MinkowskiDiff shape;
Vec3f ray;
- BVH_REAL distance;
+ FCL_REAL distance;
Simplex simplices[2];
@@ -147,7 +147,7 @@ private:
static const size_t EPA_MAX_FACES = 128;
static const size_t EPA_MAX_VERTICES = 64;
-static const BVH_REAL EPA_EPS = 0.000001;
+static const FCL_REAL EPA_EPS = 0.000001;
static const size_t EPA_MAX_ITERATIONS = 255;
struct EPA
@@ -157,7 +157,7 @@ private:
struct SimplexF
{
Vec3f n;
- BVH_REAL d;
+ FCL_REAL d;
SimplexV* c[3]; // a face has three vertices
SimplexF* f[3]; // a face has three adjacent faces
SimplexF* l[2]; // the pre and post faces in the list
@@ -209,7 +209,7 @@ public:
Status status;
GJK::Simplex result;
Vec3f normal;
- BVH_REAL depth;
+ FCL_REAL depth;
SimplexV sv_store[EPA_MAX_VERTICES];
SimplexF fc_store[EPA_MAX_FACES];
size_t nextsv;
@@ -222,7 +222,7 @@ public:
void initialize();
- bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist);
+ bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
@@ -245,7 +245,7 @@ public:
template<typename S1, typename S2>
bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- BVH_REAL* distance)
+ FCL_REAL* distance)
{
Vec3f guess(1, 0, 0);
details::MinkowskiDiff shape;
@@ -261,7 +261,7 @@ bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
Vec3f w0, w1;
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- BVH_REAL p = gjk.getSimplex()->p[i];
+ FCL_REAL p = gjk.getSimplex()->p[i];
w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
@@ -279,7 +279,7 @@ bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
template<typename S1, typename S2>
bool shapeIntersect2(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+ Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
Vec3f guess(1, 0, 0);
details::MinkowskiDiff shape;
@@ -322,7 +322,7 @@ bool shapeIntersect2(const S1& s1, const SimpleTransform& tf1,
template<typename S>
bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+ Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
Triangle2 tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
@@ -365,7 +365,7 @@ bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
template<typename S>
bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+ Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
Triangle2 tri(P1, P2, P3);
SimpleTransform tf2(R, T);
diff --git a/trunk/fcl/include/fcl/deprecated/vec_3f.h b/trunk/fcl/include/fcl/deprecated/vec_3f.h
index 52594d30..ead30c2d 100644
--- a/trunk/fcl/include/fcl/deprecated/vec_3f.h
+++ b/trunk/fcl/include/fcl/deprecated/vec_3f.h
@@ -50,21 +50,21 @@ class Vec3f
{
public:
/** \brief vector data */
- BVH_REAL v_[3];
+ FCL_REAL v_[3];
Vec3f() { v_[0] = 0; v_[1] = 0; v_[2] = 0; }
Vec3f(const Vec3f& other)
{
- memcpy(v_, other.v_, sizeof(BVH_REAL) * 3);
+ memcpy(v_, other.v_, sizeof(FCL_REAL) * 3);
}
- Vec3f(const BVH_REAL* v)
+ Vec3f(const FCL_REAL* v)
{
- memcpy(v_, v, sizeof(BVH_REAL) * 3);
+ memcpy(v_, v, sizeof(FCL_REAL) * 3);
}
- Vec3f(BVH_REAL x, BVH_REAL y, BVH_REAL z)
+ Vec3f(FCL_REAL x, FCL_REAL y, FCL_REAL z)
{
v_[0] = x;
v_[1] = y;
@@ -74,12 +74,12 @@ public:
~Vec3f() {}
/** \brief Get the ith element */
- inline BVH_REAL operator [] (size_t i) const
+ inline FCL_REAL operator [] (size_t i) const
{
return v_[i];
}
- inline BVH_REAL& operator[] (size_t i)
+ inline FCL_REAL& operator[] (size_t i)
{
return v_[i];
}
@@ -103,7 +103,7 @@ public:
return *this;
}
- inline Vec3f& operator *= (BVH_REAL t)
+ inline Vec3f& operator *= (FCL_REAL t)
{
v_[0] *= t;
v_[1] *= t;
@@ -139,7 +139,7 @@ public:
}
/** \brief Scale the vector by t */
- inline Vec3f operator * (BVH_REAL t) const
+ inline Vec3f operator * (FCL_REAL t) const
{
return Vec3f(v_[0] * t, v_[1] * t, v_[2] * t);
}
@@ -153,7 +153,7 @@ public:
}
/** \brief Return the dot product with another vector */
- inline BVH_REAL dot(const Vec3f& other) const
+ inline FCL_REAL dot(const Vec3f& other) const
{
return v_[0] * other.v_[0] + v_[1] * other.v_[1] + v_[2] * other.v_[2];
}
@@ -161,11 +161,11 @@ public:
/** \brief Normalization */
inline bool normalize()
{
- const BVH_REAL EPSILON = 1e-11;
- BVH_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
+ const FCL_REAL EPSILON = 1e-11;
+ FCL_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
if(sqr_length > EPSILON * EPSILON)
{
- BVH_REAL inv_length = (BVH_REAL)1.0 / (BVH_REAL)sqrt(sqr_length);
+ FCL_REAL inv_length = (FCL_REAL)1.0 / (FCL_REAL)sqrt(sqr_length);
v_[0] *= inv_length;
v_[1] *= inv_length;
v_[2] *= inv_length;
@@ -176,11 +176,11 @@ public:
inline Vec3f normalized() const
{
- const BVH_REAL EPSILON = 1e-11;
- BVH_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
+ const FCL_REAL EPSILON = 1e-11;
+ FCL_REAL sqr_length = v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
if(sqr_length > EPSILON * EPSILON)
{
- BVH_REAL inv_length = (BVH_REAL)1.0 / (BVH_REAL)sqrt(sqr_length);
+ FCL_REAL inv_length = (FCL_REAL)1.0 / (FCL_REAL)sqrt(sqr_length);
return *this * inv_length;
}
else
@@ -191,25 +191,25 @@ public:
/** \brief Return vector length */
- inline BVH_REAL length() const
+ inline FCL_REAL length() const
{
return sqrt(v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2]);
}
/** \brief Return vector square length */
- inline BVH_REAL sqrLength() const
+ inline FCL_REAL sqrLength() const
{
return v_[0] * v_[0] + v_[1] * v_[1] + v_[2] * v_[2];
}
/** \brief Set the vector using new values */
- inline void setValue(BVH_REAL x, BVH_REAL y, BVH_REAL z)
+ inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z)
{
v_[0] = x; v_[1] = y; v_[2] = z;
}
/** \brief Set the vector using new values */
- inline void setValue(BVH_REAL x)
+ inline void setValue(FCL_REAL x)
{
v_[0] = x; v_[1] = x; v_[2] = x;
}
@@ -217,7 +217,7 @@ public:
/** \brief Check whether two vectors are the same in value */
inline bool equal(const Vec3f& other) const
{
- const BVH_REAL EPSILON = 1e-11;
+ const FCL_REAL EPSILON = 1e-11;
return ((v_[0] - other.v_[0] < EPSILON) &&
(v_[0] - other.v_[0] > -EPSILON) &&
(v_[1] - other.v_[1] < EPSILON) &&
@@ -226,7 +226,7 @@ public:
(v_[2] - other.v_[2] > -EPSILON));
}
- inline BVH_REAL triple(const Vec3f& v1, const Vec3f& v2) const
+ inline FCL_REAL triple(const Vec3f& v1, const Vec3f& v2) const
{
return v_[0] * (v1.v_[1] * v2.v_[2] - v1.v_[2] * v2.v_[1]) +
v_[1] * (v1.v_[2] * v2.v_[0] - v1.v_[0] * v2.v_[2]) +
@@ -253,15 +253,15 @@ inline Vec3f max(const Vec3f& a, const Vec3f& b)
inline Vec3f abs(const Vec3f& v)
{
- BVH_REAL x = v[0] < 0 ? -v[0] : v[0];
- BVH_REAL y = v[1] < 0 ? -v[1] : v[1];
- BVH_REAL z = v[2] < 0 ? -v[2] : v[2];
+ FCL_REAL x = v[0] < 0 ? -v[0] : v[0];
+ FCL_REAL y = v[1] < 0 ? -v[1] : v[1];
+ FCL_REAL z = v[2] < 0 ? -v[2] : v[2];
return Vec3f(x, y, z);
}
-inline BVH_REAL triple(const Vec3f& a, const Vec3f& b, const Vec3f& c)
+inline FCL_REAL triple(const Vec3f& a, const Vec3f& b, const Vec3f& c)
{
return a.triple(b, c);
}
@@ -271,12 +271,12 @@ inline BVH_REAL triple(const Vec3f& a, const Vec3f& b, const Vec3f& c)
*/
static void generateCoordinateSystem(const Vec3f& w, Vec3f& u, Vec3f& v)
{
- BVH_REAL inv_length;
+ FCL_REAL inv_length;
if(fabs(w[0]) >= fabs(w[1]))
{
- inv_length = (BVH_REAL)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+ inv_length = (FCL_REAL)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
u[0] = -w[2] * inv_length;
- u[1] = (BVH_REAL)0;
+ u[1] = (FCL_REAL)0;
u[2] = w[0] * inv_length;
v[0] = w[1] * u[2];
v[1] = w[2] * u[0] - w[0] * u[2];
@@ -284,8 +284,8 @@ static void generateCoordinateSystem(const Vec3f& w, Vec3f& u, Vec3f& v)
}
else
{
- inv_length = (BVH_REAL)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = (BVH_REAL)0;
+ inv_length = (FCL_REAL)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+ u[0] = (FCL_REAL)0;
u[1] = w[2] * inv_length;
u[2] = -w[1] * inv_length;
v[0] = w[1] * u[2] - w[2] * u[1];
diff --git a/trunk/fcl/include/fcl/distance.h b/trunk/fcl/include/fcl/distance.h
index 5e5285b7..3a05c814 100644
--- a/trunk/fcl/include/fcl/distance.h
+++ b/trunk/fcl/include/fcl/distance.h
@@ -44,17 +44,17 @@ namespace fcl
template<typename NarrowPhaseSolver>
-BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver);
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver);
template<typename NarrowPhaseSolver>
-BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
+FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver);
-BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2);
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2);
-BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
+FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2);
}
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
index 6f9e0503..f909505c 100644
--- a/trunk/fcl/include/fcl/distance_func_matrix.h
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -46,7 +46,7 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix
{
- typedef BVH_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver);
+ typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver);
DistanceFunc distance_matrix[16][16];
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index 0a317ab7..2757df85 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -73,7 +73,7 @@ public:
class Box : public ShapeBase
{
public:
- Box(BVH_REAL x, BVH_REAL y, BVH_REAL z) : ShapeBase(), side(x, y, z) {}
+ Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z) {}
/** box side length */
Vec3f side;
@@ -89,10 +89,10 @@ public:
class Sphere : public ShapeBase
{
public:
- Sphere(BVH_REAL radius_) : ShapeBase(), radius(radius_) {}
+ Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {}
/** \brief Radius of the sphere */
- BVH_REAL radius;
+ FCL_REAL radius;
/** \brief Compute AABB */
void computeLocalAABB();
@@ -105,13 +105,13 @@ public:
class Capsule : public ShapeBase
{
public:
- Capsule(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+ Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
/** \brief Radius of capsule */
- BVH_REAL radius;
+ FCL_REAL radius;
/** \brief Length along z axis */
- BVH_REAL lz;
+ FCL_REAL lz;
/** \brief Compute AABB */
void computeLocalAABB();
@@ -124,13 +124,13 @@ public:
class Cone : public ShapeBase
{
public:
- Cone(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+ Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
/** \brief Radius of the cone */
- BVH_REAL radius;
+ FCL_REAL radius;
/** \brief Length along z axis */
- BVH_REAL lz;
+ FCL_REAL lz;
/** \brief Compute AABB */
void computeLocalAABB();
@@ -143,13 +143,13 @@ public:
class Cylinder : public ShapeBase
{
public:
- Cylinder(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+ Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
/** \brief Radius of the cylinder */
- BVH_REAL radius;
+ FCL_REAL radius;
/** \brief Length along z axis */
- BVH_REAL lz;
+ FCL_REAL lz;
/** \brief Compute AABB */
void computeLocalAABB();
@@ -164,7 +164,7 @@ class Convex : public ShapeBase
public:
/** Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information */
Convex(Vec3f* plane_normals_,
- BVH_REAL* plane_dis_,
+ FCL_REAL* plane_dis_,
int num_planes_,
Vec3f* points_,
int num_points_,
@@ -183,7 +183,7 @@ public:
sum += points[i];
}
- center = sum * (BVH_REAL)(1.0 / num_points);
+ center = sum * (FCL_REAL)(1.0 / num_points);
fillEdges();
}
@@ -213,7 +213,7 @@ public:
Vec3f* plane_normals;
- BVH_REAL* plane_dis;
+ FCL_REAL* plane_dis;
/** An array of indices to the points of each polygon, it should be the number of vertices
* followed by that amount of indices to "points" in counter clockwise order
@@ -245,10 +245,10 @@ class Plane : public ShapeBase
{
public:
/** \brief Construct a plane with normal direction and offset */
- Plane(const Vec3f& n_, BVH_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); }
+ Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); }
/** \brief Construct a plane with normal direction and offset */
- Plane(BVH_REAL a, BVH_REAL b, BVH_REAL c, BVH_REAL d_) : n(a, b, c), d(d_)
+ Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : n(a, b, c), d(d_)
{
unitNormalTest();
}
@@ -263,7 +263,7 @@ public:
Vec3f n;
/** \brief Plane offset */
- BVH_REAL d;
+ FCL_REAL d;
protected:
diff --git a/trunk/fcl/include/fcl/hierarchy_tree.h b/trunk/fcl/include/fcl/hierarchy_tree.h
index cfe7b523..76099e40 100644
--- a/trunk/fcl/include/fcl/hierarchy_tree.h
+++ b/trunk/fcl/include/fcl/hierarchy_tree.h
@@ -40,6 +40,7 @@
#include <vector>
#include "fcl/BV/AABB.h"
#include "fcl/vec_3f.h"
+#include "fcl/morton.h"
#include <boost/bind.hpp>
namespace fcl
@@ -140,6 +141,7 @@ public:
{
if(root_node)
recurseDeleteNode(root_node);
+ n_leaves = 0;
delete free_node;
free_node = NULL;
max_lookahead_level = -1;
@@ -152,25 +154,6 @@ public:
return (NULL == root_node);
}
- /** \brief update one leaf node's bounding volume */
- void update_(NodeType* leaf, const BV& bv)
- {
- NodeType* root = removeLeaf(leaf);
- if(root)
- {
- if(max_lookahead_level >= 0)
- {
- for(int i = 0; (i < max_lookahead_level) && root->parent; ++i)
- root = root->parent;
- }
- else
- root = root_node;
- }
-
- leaf->bv = bv;
- insertLeaf(root, leaf);
- }
-
/** \brief update one leaf node */
void update(NodeType* leaf, int lookahead_level = -1)
{
@@ -196,7 +179,7 @@ public:
}
/** \brief update one leaf's bounding volume, with prediction */
- bool update(NodeType* leaf, const BV& bv, const Vec3f& vel, BVH_REAL margin)
+ bool update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
{
if(leaf->bv.contain(bv)) return false;
update_(leaf, bv);
@@ -211,51 +194,11 @@ public:
return true;
}
- /** \brief balance the tree from bottom */
- void balanceBottomup()
- {
- if(root_node)
- {
- std::vector<NodeType*> leaves;
- leaves.reserve(n_leaves);
- fetchLeaves(root_node, leaves);
- bottomup(leaves.begin(), leaves.end());
- root_node = leaves[0];
- }
- }
-
- /** \brief balance the tree from top */
- void balanceTopdown(int bu_threshold = 128)
- {
- if(root_node)
- {
- std::vector<NodeType*> leaves;
- leaves.reserve(n_leaves);
- fetchLeaves(root_node, leaves);
- root_node = topdown(leaves.begin(), leaves.end(), bu_threshold);
- }
- }
-
- /** \brief refit */
- void refit()
+ size_t getMaxHeight() const
{
- if(root_node)
- {
- recurseRefit(root_node);
- }
+ return getMaxHeight(root_node);
}
- void recurseRefit(NodeType* node)
- {
- if(!node->isLeaf())
- {
- recurseRefit(node->childs[0]);
- recurseRefit(node->childs[1]);
- node->bv = node->childs[0]->bv + node->childs[1]->bv;
- }
- else
- return;
- }
size_t getMaxHeight(NodeType* node) const
{
@@ -280,13 +223,39 @@ public:
max_depth = std::max(max_depth, depth);
}
+ /** \brief balance the tree from bottom */
+ void balanceBottomup()
+ {
+ if(root_node)
+ {
+ std::vector<NodeType*> leaves;
+ leaves.reserve(n_leaves);
+ fetchLeaves(root_node, leaves);
+ bottomup(leaves.begin(), leaves.end());
+ root_node = leaves[0];
+ }
+ }
+
+ /** \brief balance the tree from top */
+ void balanceTopdown(int bu_threshold = 128)
+ {
+ if(root_node)
+ {
+ std::vector<NodeType*> leaves;
+ leaves.reserve(n_leaves);
+ fetchLeaves(root_node, leaves);
+ root_node = topdown(leaves.begin(), leaves.end(), bu_threshold);
+ }
+ }
+
+
/** \brief balance the tree in an incremental way */
- void balanceIncremental(int passes)
+ void balanceIncremental(int iterations)
{
- if(passes < 0) passes = n_leaves;
- if(root_node && (passes > 0))
+ if(iterations < 0) iterations = n_leaves;
+ if(root_node && (iterations > 0))
{
- for(int i = 0; i < passes; ++i)
+ for(int i = 0; i < iterations; ++i)
{
NodeType* node = root_node;
unsigned int bit = 0;
@@ -301,6 +270,13 @@ public:
}
}
+ /** \brief refit */
+ void refit()
+ {
+ if(root_node)
+ recurseRefit(root_node);
+ }
+
/** \brief extract all the leaves of the tree */
void extractLeaves(const NodeType* root, std::vector<NodeType*>& leaves) const
{
@@ -350,12 +326,12 @@ public:
while(lbeg < lcur_end - 1)
{
NodeVecIterator min_it1, min_it2;
- BVH_REAL min_size = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1)
{
for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2)
{
- BVH_REAL cur_size = ((*it1)->bv + (*it2)->bv).size();
+ FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size();
if(cur_size < min_size)
{
min_size = cur_size;
@@ -392,7 +368,7 @@ public:
vol += (*it)->bv;
int best_axis = 0;
- BVH_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
+ FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
if(extent[1] > extent[0]) best_axis = 1;
if(extent[2] > extent[best_axis]) best_axis = 2;
@@ -432,7 +408,7 @@ public:
split_p += (*it)->bv.center();
vol += (*it)->bv;
}
- split_p /= (BVH_REAL)(num_leaves);
+ split_p /= (FCL_REAL)(num_leaves);
int best_axis = -1;
int bestmidp = num_leaves;
int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
@@ -458,7 +434,7 @@ public:
if(best_axis < 0) best_axis = 0;
- BVH_REAL split_value = split_p[best_axis];
+ FCL_REAL split_value = split_p[best_axis];
NodeVecIterator lcenter = lbeg;
for(it = lbeg; it < lend; ++it)
{
@@ -486,9 +462,33 @@ public:
}
return *lbeg;
}
+
+ NodeType* topdown_morton(const NodeVecIterator lbeg, const NodeVecIterator lend, int bu_threshold)
+ {
+ return NULL;
+ }
private:
+ /** \brief update one leaf node's bounding volume */
+ void update_(NodeType* leaf, const BV& bv)
+ {
+ NodeType* root = removeLeaf(leaf);
+ if(root)
+ {
+ if(max_lookahead_level >= 0)
+ {
+ for(int i = 0; (i < max_lookahead_level) && root->parent; ++i)
+ root = root->parent;
+ }
+ else
+ root = root_node;
+ }
+
+ leaf->bv = bv;
+ insertLeaf(root, leaf);
+ }
+
/** \brief sort node n and its parent according to their memory position */
NodeType* sort(NodeType* n, NodeType*& r)
{
@@ -676,10 +676,24 @@ private:
recurseDeleteNode(node->childs[0]);
recurseDeleteNode(node->childs[1]);
}
+
if(node == root_node) root_node = NULL;
deleteNode(node);
}
+ void recurseRefit(NodeType* node)
+ {
+ if(!node->isLeaf())
+ {
+ recurseRefit(node->childs[0]);
+ recurseRefit(node->childs[1]);
+ node->bv = node->childs[0]->bv + node->childs[1]->bv;
+ }
+ else
+ return;
+ }
+
+
static BV bounds(const std::vector<NodeType*>& leaves)
{
if(leaves.size() == 0) return BV();
@@ -719,12 +733,718 @@ protected:
template<>
-bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, BVH_REAL margin);
+bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin);
template<>
bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel);
+namespace alternative
+{
+
+template<typename BV>
+struct NodeBase
+{
+ BV bv;
+
+ union
+ {
+ size_t parent;
+ size_t next;
+ };
+
+ union
+ {
+ size_t childs[2];
+ void* data;
+ };
+
+ bool isLeaf() const { return (childs[1] == (size_t)(-1)); }
+ bool isInternal() const { return !isLeaf(); }
+};
+
+template<typename BV>
+struct nodeBaseLess
+{
+ nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
+ {
+ nodes = nodes_;
+ d = d_;
+ }
+
+ bool operator() (size_t i, size_t j) const
+ {
+ if(nodes[i].bv.center()[d] < nodes[j].bv.center()[d])
+ return true;
+ return false;
+ }
+
+ const NodeBase<BV>* nodes;
+ size_t d;
+};
+
+
+
+
+template<typename BV>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes)
+{
+ return 0;
+}
+
+template<>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
+
+template<typename BV>
+size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes)
+{
+ return 0;
+}
+
+template<>
+size_t select(const AABB& query, size_t node1, size_t node2, NodeBase<AABB>* nodes);
+
+
+template<typename BV>
+class HierarchyTree
+{
+ typedef NodeBase<BV> NodeType;
+public:
+ HierarchyTree()
+ {
+ root_node = NULL_NODE;
+ n_nodes = 0;
+ n_nodes_alloc = 16;
+ nodes = new NodeType[n_nodes_alloc];
+ for(size_t i = 0; i < n_nodes_alloc - 1; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+ n_leaves = 0;
+ freelist = 0;
+ opath = 0;
+ max_lookahead_level = -1;
+ }
+
+ ~HierarchyTree()
+ {
+ delete [] nodes;
+ }
+
+ void init(NodeType* leaves, int n_leaves_, int bu_threshold = 128)
+ {
+ n_leaves = n_leaves_;
+ root_node = NULL_NODE;
+ nodes = new NodeType[n_leaves * 2];
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ n_nodes_alloc = 2 * n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ root_node = topdown(ids, ids + n_leaves, bu_threshold);
+ delete [] ids;
+ }
+
+
+ size_t insert(const BV& bv, void* data)
+ {
+ size_t node = createNode(NULL_NODE, bv, data);
+ insertLeaf(root_node, node);
+ ++n_leaves;
+ return node;
+ }
+
+ void remove(size_t leaf)
+ {
+ removeLeaf(leaf);
+ deleteNode(leaf);
+ --n_leaves;
+ }
+
+ void clear()
+ {
+ delete [] nodes;
+ root_node = NULL_NODE;
+ n_nodes = 0;
+ n_nodes_alloc = 16;
+ nodes = new NodeType[n_nodes_alloc];
+ for(size_t i = 0; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+ n_leaves = 0;
+ freelist = 0;
+ opath = 0;
+ max_lookahead_level = -1;
+ }
+
+ bool empty() const
+ {
+ return (n_nodes == 0);
+ }
+
+
+ void update(size_t leaf, int lookahead_level = -1)
+ {
+ size_t root = removeLeaf(leaf);
+ if(root != NULL_NODE)
+ {
+ if(lookahead_level > 0)
+ {
+ for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
+ root = nodes[root].parent;
+ }
+ else
+ root = root_node;
+ }
+ insertLeaf(root, leaf);
+ }
+
+ bool update(size_t leaf, const BV& bv)
+ {
+ if(nodes[leaf].bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+ }
+
+ bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin)
+ {
+ if(nodes[leaf].bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+ }
+
+ bool update(size_t leaf, const BV& bv, const Vec3f& vel)
+ {
+ if(nodes[leaf].bv.contain(bv)) return false;
+ update_(leaf, bv);
+ return true;
+ }
+
+
+ size_t getMaxHeight() const
+ {
+ return getMaxHeight(root_node);
+ }
+
+ size_t getMaxHeight(size_t node) const
+ {
+ if(!nodes[node].isLeaf())
+ {
+ size_t height1 = getMaxHeight(nodes[node].childs[0]);
+ size_t height2 = getMaxHeight(nodes[node].childs[1]);
+ return std::max(height1, height2) + 1;
+ }
+ else
+ return 0;
+ }
+
+ void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const
+ {
+ if(!nodes[node].isLeaf())
+ {
+ getMaxDepth(nodes[node].childs[0], depth+1, max_depth);
+ getmaxDepth(nodes[node].childs[1], depth+1, max_depth);
+ }
+ else
+ max_depth = std::max(max_depth, depth);
+ }
+
+ void balanceBottomup()
+ {
+ if(root_node != NULL_NODE)
+ {
+ NodeType* leaves = new NodeType[n_leaves];
+ NodeType* leaves_ = leaves;
+ extractLeaves(root_node, leaves_);
+ root_node = NULL_NODE;
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ bottomup(ids, ids + n_leaves);
+ root_node = *ids;
+
+ delete [] ids;
+ }
+ }
+
+ void balanceTopdown(int bu_threshold = 128)
+ {
+ if(root_node != NULL_NODE)
+ {
+ NodeType* leaves = new NodeType[n_leaves];
+ NodeType* leaves_ = leaves;
+ extractLeaves(root_node, leaves_);
+ root_node = NULL_NODE;
+ memcpy(nodes, leaves, sizeof(NodeType) * n_leaves);
+ freelist = n_leaves;
+ n_nodes = n_leaves;
+ for(size_t i = n_leaves; i < n_nodes_alloc; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+
+ size_t* ids = new size_t[n_leaves];
+ for(size_t i = 0; i < n_leaves; ++i)
+ ids[i] = i;
+
+ root_node = topdown(ids, ids + n_leaves, bu_threshold);
+ delete [] ids;
+ }
+ }
+
+ void balanceIncremental(int iterations)
+ {
+ if(iterations < 0) iterations = n_leaves;
+ if((root_node != NULL_NODE) && (iterations > 0))
+ {
+ for(int i = 0; i < iterations; ++i)
+ {
+ size_t node = root_node;
+ unsigned int bit = 0;
+ while(!nodes[node].isLeaf())
+ {
+ node = nodes[node].childs[(opath>>bit)&1];
+ bit = (bit+1)&(sizeof(unsigned int) * 8-1);
+ }
+ update(node);
+ ++opath;
+ }
+ }
+ }
+
+ void refit()
+ {
+ if(root_node != NULL_NODE)
+ recurseRefit(root_node);
+ }
+
+ void extractLeaves(size_t root, NodeType*& leaves) const
+ {
+ if(!nodes[root].isLeaf())
+ {
+ extractLeaves(nodes[root].childs[0], leaves);
+ extractLeaves(nodes[root].childs[1], leaves);
+ }
+ else
+ {
+ *leaves = nodes[root];
+ leaves++;
+ }
+ }
+
+ size_t size() const
+ {
+ return n_leaves;
+ }
+
+ size_t getRoot() const
+ {
+ return root_node;
+ }
+
+ NodeType* getNodes() const
+ {
+ return nodes;
+ }
+
+ void print(size_t root, int depth)
+ {
+ for(int i = 0; i < depth; ++i)
+ std::cout << " ";
+ NodeType* n = nodes + root;
+ std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl;
+ if(n->isLeaf())
+ {
+ }
+ else
+ {
+ print(n->childs[0], depth+1);
+ print(n->childs[1], depth+1);
+ }
+ }
+
+ void bottomup(size_t* lbeg, size_t* lend)
+ {
+ size_t* lcur_end = lend;
+ while(lbeg < lcur_end - 1)
+ {
+ size_t* min_it1, *min_it2;
+ FCL_REAL min_size = std::numeric_limits<FCL_REAL>::max();
+ for(size_t* it1 = lbeg; it1 < lcur_end; ++it1)
+ {
+ for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2)
+ {
+ FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
+ if(cur_size < min_size)
+ {
+ min_size = cur_size;
+ min_it1 = it1;
+ min_it2 = it2;
+ }
+ }
+ }
+
+ size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, NULL);
+ nodes[p].childs[0] = *min_it1;
+ nodes[p].childs[1] = *min_it2;
+ nodes[*min_it1].parent = p;
+ nodes[*min_it2].parent = p;
+ *min_it1 = p;
+ size_t tmp = *min_it2;
+ lcur_end--;
+ *min_it2 = *lcur_end;
+ *lcur_end = tmp;
+ }
+ }
+
+ size_t topdown(size_t* lbeg, size_t* lend, int bu_threshold)
+ {
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(num_leaves > bu_threshold)
+ {
+ BV vol = nodes[*lbeg].bv;
+ for(size_t* i = lbeg + 1; i < lend; ++i)
+ vol += nodes[*i].bv;
+
+ int best_axis = 0;
+ FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
+ if(extent[1] > extent[0]) best_axis = 1;
+ if(extent[2] > extent[best_axis]) best_axis = 2;
+
+ nodeBaseLess<BV> less_functor(nodes, best_axis);
+ size_t* lcenter = lbeg + num_leaves / 2;
+ std::nth_element(lbeg, lcenter, lend, less_functor);
+
+ size_t node = createNode(NULL_NODE, vol, NULL);
+ nodes[node].childs[0] = topdown(lbeg, lcenter, bu_threshold);
+ nodes[node].childs[1] = topdown(lcenter, lend, bu_threshold);
+ nodes[nodes[node].childs[0]].parent = node;
+ nodes[nodes[node].childs[1]].parent = node;
+ return node;
+ }
+ else
+ {
+ bottomup(lbeg, lend);
+ return *lbeg;
+ }
+ }
+ return *lbeg;
+ }
+
+ size_t topdown2(size_t* lbeg, size_t* lend, int bu_threshold)
+ {
+ int num_leaves = lend - lbeg;
+ if(num_leaves > 1)
+ {
+ if(num_leaves > bu_threshold)
+ {
+ Vec3f split_p = nodes[*lbeg].bv.center();
+ BV vol = nodes[*lbeg].bv;
+ for(size_t* i = lbeg + 1; i < lend; ++i)
+ {
+ split_p += nodes[*i].bv.center();
+ vol += nodes[*i].bv;
+ }
+ split_p /= (FCL_REAL)(num_leaves);
+ int best_axis = -1;
+ int bestmidp = num_leaves;
+ int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
+ for(size_t* i = lbeg; i < lend; ++i)
+ {
+ Vec3f x = nodes[*i].bv.center() - split_p;
+ for(size_t j = 0; j < 3; ++j)
+ ++splitcount[j][x[j] > 0 ? 1 : 0];
+ }
+
+ for(size_t i = 0; i < 3; ++i)
+ {
+ if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
+ {
+ int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
+ if(midp < bestmidp)
+ {
+ best_axis = i;
+ bestmidp = midp;
+ }
+ }
+ }
+
+ if(best_axis < 0) best_axis = 0;
+
+ FCL_REAL split_value = split_p[best_axis];
+ size_t* lcenter = lbeg;
+ for(size_t* i = lbeg; i < lend; ++i)
+ {
+ if(nodes[*i].bv.center()[best_axis] < split_value)
+ {
+ size_t temp = *i;
+ *i = *lcenter;
+ *lcenter = temp;
+ ++lcenter;
+ }
+ }
+
+ size_t node = createNode(NULL_NODE, vol, NULL);
+ nodes[node].childs[0] = topdown2(lbeg, lcenter, bu_threshold);
+ nodes[node].childs[1] = topdown2(lcenter, lend, bu_threshold);
+ nodes[nodes[node].childs[0]].parent = node;
+ nodes[nodes[node].childs[1]].parent = node;
+ return node;
+ }
+ else
+ {
+ bottomup(lbeg, lend);
+ return *lbeg;
+ }
+ }
+ return *lbeg;
+ }
+
+
+private:
+
+ void insertLeaf(size_t root, size_t leaf)
+ {
+ if(root_node == NULL_NODE)
+ {
+ root_node = leaf;
+ nodes[leaf].parent = NULL_NODE;
+ }
+ else
+ {
+ if(!nodes[root].isLeaf())
+ {
+ do
+ {
+ root = nodes[root].childs[select(leaf, nodes[root].childs[0], nodes[root].childs[1], nodes)];
+ }
+ while(!nodes[root].isLeaf());
+ }
+
+ size_t prev = nodes[root].parent;
+ size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, NULL);
+ if(prev != NULL_NODE)
+ {
+ nodes[prev].childs[indexOf(root)] = node;
+ nodes[node].childs[0] = root; nodes[root].parent = node;
+ nodes[node].childs[1] = leaf; nodes[leaf].parent = node;
+ do
+ {
+ if(!nodes[prev].bv.contain(nodes[node].bv))
+ nodes[prev].bv = nodes[nodes[prev].childs[0]].bv + nodes[nodes[prev].childs[1]].bv;
+ else
+ break;
+ node = prev;
+ } while (NULL_NODE != (prev = nodes[node].parent));
+ }
+ else
+ {
+ nodes[node].childs[0] = root; nodes[root].parent = node;
+ nodes[node].childs[1] = leaf; nodes[leaf].parent = node;
+ root_node = node;
+ }
+ }
+ }
+
+ size_t removeLeaf(size_t leaf)
+ {
+ if(leaf == root_node)
+ {
+ root_node = NULL_NODE;
+ return NULL_NODE;
+ }
+ else
+ {
+ size_t parent = nodes[leaf].parent;
+ size_t prev = nodes[parent].parent;
+ size_t sibling = nodes[parent].childs[1-indexOf(leaf)];
+
+ if(prev != NULL_NODE)
+ {
+ nodes[prev].childs[indexOf(parent)] = sibling;
+ nodes[sibling].parent = prev;
+ deleteNode(parent);
+ while(prev != NULL_NODE)
+ {
+ BV new_bv = nodes[nodes[prev].childs[0]].bv + nodes[nodes[prev].childs[1]].bv;
+ if(!new_bv.equal(nodes[prev].bv))
+ {
+ nodes[prev].bv = new_bv;
+ prev = nodes[prev].parent;
+ }
+ else break;
+ }
+
+ return (prev != NULL_NODE) ? prev : root_node;
+ }
+ else
+ {
+ root_node = sibling;
+ nodes[sibling].parent = NULL_NODE;
+ deleteNode(parent);
+ return root_node;
+ }
+ }
+ }
+
+
+ void update_(size_t leaf, const BV& bv)
+ {
+ size_t root = removeLeaf(leaf);
+ if(root != NULL_NODE)
+ {
+ if(max_lookahead_level >= 0)
+ {
+ for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
+ root = nodes[root].parent;
+ }
+
+ nodes[leaf].bv = bv;
+ insertLeaf(root, leaf);
+ }
+ }
+
+ inline size_t indexOf(size_t node)
+ {
+ return (nodes[nodes[node].parent].childs[1] == node);
+ }
+
+
+ size_t allocateNode()
+ {
+ if(freelist == NULL_NODE)
+ {
+ NodeType* old_nodes = nodes;
+ n_nodes_alloc *= 2;
+ nodes = new NodeType[n_nodes_alloc];
+ memcpy(nodes, old_nodes, n_nodes * sizeof(NodeType));
+ delete [] old_nodes;
+
+ for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i)
+ nodes[i].next = i + 1;
+ nodes[n_nodes_alloc - 1].next = NULL_NODE;
+ freelist = n_nodes;
+ }
+
+ size_t node_id = freelist;
+ freelist = nodes[node_id].next;
+ nodes[node_id].parent = NULL_NODE;
+ nodes[node_id].childs[0] = NULL_NODE;
+ nodes[node_id].childs[1] = NULL_NODE;
+ ++n_nodes;
+ return node_id;
+ }
+
+ size_t createNode(size_t parent,
+ const BV& bv1,
+ const BV& bv2,
+ void* data)
+ {
+ size_t node = allocateNode();
+ nodes[node].parent = parent;
+ nodes[node].data = data;
+ nodes[node].bv = bv1 + bv2;
+ return node;
+ }
+
+ size_t createNode(size_t parent,
+ const BV& bv,
+ void* data)
+ {
+ size_t node = allocateNode();
+ nodes[node].parent = parent;
+ nodes[node].data = data;
+ nodes[node].bv = bv;
+ return node;
+ }
+
+ size_t createNode(size_t parent,
+ void* data)
+ {
+ size_t node = allocateNode();
+ nodes[node].parent = parent;
+ nodes[node].data = data;
+ return node;
+ }
+
+ void deleteNode(size_t node)
+ {
+ nodes[node].next = freelist;
+ freelist = node;
+ --n_nodes;
+ }
+
+ void recurseRefit(size_t node)
+ {
+ if(!nodes[node].isLeaf())
+ {
+ recurseRefit(nodes[node].childs[0]);
+ recurseRefit(nodes[node].childs[1]);
+ nodes[node].bv = nodes[nodes[node].childs[0]].bv + nodes[nodes[node].childs[1]].bv;
+ }
+ else
+ return;
+ }
+
+ void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1)
+ {
+ if((!nodes[root].isLeaf()) && depth)
+ {
+ fetchLeaves(nodes[root].childs[0], leaves, depth-1);
+ fetchLeaves(nodes[root].childs[1], leaves, depth-1);
+ deleteNode(root);
+ }
+ else
+ {
+ *leaves = nodes[root];
+ leaves++;
+ }
+ }
+
+
+
+protected:
+ size_t root_node;
+ NodeType* nodes;
+ size_t n_nodes;
+ size_t n_nodes_alloc;
+
+ size_t n_leaves;
+ size_t freelist;
+ unsigned int opath;
+
+ int max_lookahead_level;
+
+public:
+ static const size_t NULL_NODE = -1;
+};
+
+template<typename BV>
+const size_t HierarchyTree<BV>::NULL_NODE;
+
+
+
+
+
+}
+
+
}
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index 87f30442..b2ae42eb 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -58,22 +58,22 @@ class PolySolver
{
public:
/** \brief Solve a linear equation with coefficients c, return roots s and number of roots */
- static int solveLinear(BVH_REAL c[2], BVH_REAL s[1]);
+ 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(BVH_REAL c[3], BVH_REAL s[2]);
+ 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(BVH_REAL c[4], BVH_REAL s[3]);
+ static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
private:
/** \brief Check whether v is zero */
- static inline bool isZero(BVH_REAL v);
+ static inline bool isZero(FCL_REAL v);
/** \brief Compute v^{1/3} */
- static inline bool cbrt(BVH_REAL v);
+ static inline bool cbrt(FCL_REAL v);
- static const BVH_REAL NEAR_ZERO_THRESHOLD;
+ static const FCL_REAL NEAR_ZERO_THRESHOLD;
};
#if USE_SVMLIGHT
@@ -101,7 +101,7 @@ public:
*/
static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/** \brief CCD intersect between two edges
* [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
@@ -110,17 +110,17 @@ public:
*/
static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/** \brief CCD intersect between one vertex and one face, using additional filter */
static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/** \brief CCD intersect between two edges, using additional filter */
static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
/** \brief CCD intersect between one vertex and and one edge */
static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
@@ -132,7 +132,7 @@ public:
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
Vec3f* contact_points = NULL,
unsigned int* num_contact_points = NULL,
- BVH_REAL* penetration_depth = NULL,
+ FCL_REAL* penetration_depth = NULL,
Vec3f* normal = NULL);
static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
@@ -140,23 +140,23 @@ public:
const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points = NULL,
unsigned int* num_contact_points = NULL,
- BVH_REAL* penetration_depth = NULL,
+ FCL_REAL* penetration_depth = NULL,
Vec3f* normal = NULL);
#if USE_SVMLIGHT
- static BVH_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+ static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
const CloudClassifierParam& solver, bool scaling = true);
- static BVH_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+ static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
- static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+ static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3);
- static BVH_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+ static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
const Matrix3f& R, const Vec3f& T);
#endif
@@ -169,12 +169,12 @@ private:
const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
/** \brief Check whether one value is zero */
- static inline bool isZero(BVH_REAL v);
+ static inline bool isZero(FCL_REAL v);
/** \brief Solve the cubic function using Newton method, also satisfies the interval restriction */
static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL& l, BVH_REAL& r, bool bVF, BVH_REAL coeffs[], Vec3f* data = NULL);
+ FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
/** \brief Check whether one point p is within triangle [a, b, c] */
static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
@@ -189,32 +189,32 @@ private:
* return FALSE if no solution exists.
*/
static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, BVH_REAL* mua, BVH_REAL* mub);
+ Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
/** \brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t */
static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL t);
+ FCL_REAL t);
/** \brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time */
static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL t, Vec3f* q_i = NULL);
+ FCL_REAL t, Vec3f* q_i = NULL);
/** \brief Check whether a root for VE intersection is valid */
static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- BVH_REAL t);
+ FCL_REAL t);
/** \brief Solve a square function for EE intersection (with interval restriction) */
- static bool solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
+ static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
bool bVF,
- BVH_REAL* ret);
+ FCL_REAL* ret);
/** \brief Solve a square function for VE intersection (with interval restriction) */
- static bool solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
+ static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
@@ -223,52 +223,52 @@ private:
*/
static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d);
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
/** \brief Compute the cubic coefficients for EE intersection */
static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d);
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
/** \brief Compute the cubic coefficients for VE intersection */
static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
const Vec3f& L,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c);
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
/** \brief filter for intersection, works for both VF and EE */
static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
/** \brief distance of point v to a plane n * x - t = 0 */
- static BVH_REAL distanceToPlane(const Vec3f& n, BVH_REAL t, const Vec3f& v);
+ static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
/** \brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 */
- static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, BVH_REAL t);
+ static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
/** \brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to */
static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
- const Vec3f& tn, BVH_REAL to,
+ const Vec3f& tn, FCL_REAL to,
Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
/** \brief build a plane passed through triangle v1 v2 v3 */
- static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, BVH_REAL* t);
+ static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
/** \brief build a plane pass through edge v1 and v2, normal is tn */
- static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, BVH_REAL* t);
+ static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
/** \brief compute the points which has deepest penetration depth */
- static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, BVH_REAL t, BVH_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
+ static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
/** \brief clip polygon by plane */
- static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, BVH_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
+ static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
/** \brief clip a line segment by plane */
- static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, BVH_REAL t, Vec3f* clipped_point);
+ static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
/** \brief compute the cdf(x) */
- static BVH_REAL gaussianCDF(BVH_REAL x)
+ static FCL_REAL gaussianCDF(FCL_REAL x)
{
return 0.5 * erfc(-x / sqrt(2.0));
}
@@ -281,9 +281,9 @@ private:
static void singleKernelGradient(KERNEL_PARM *kernel_parm, SVECTOR *a, SVECTOR *b, Vec3f& g);
#endif
- static const BVH_REAL EPSILON;
- static const BVH_REAL NEAR_ZERO_THRESHOLD;
- static const BVH_REAL CCD_RESOLUTION;
+ static const FCL_REAL EPSILON;
+ static const FCL_REAL NEAR_ZERO_THRESHOLD;
+ static const FCL_REAL CCD_RESOLUTION;
static const unsigned int MAX_TRIANGLE_CLIPS = 8;
};
@@ -306,9 +306,9 @@ public:
* if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
* coincident points on the intersection of the triangles, as might be expected.
*/
- static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
+ static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
- static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+ static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
Vec3f& P, Vec3f& Q);
@@ -319,11 +319,11 @@ public:
* coincident points on the intersection of the triangles, as might be expected.
* The returned P and Q are both in the coordinate of the first triangle's coordinate
*/
- static BVH_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
+ static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
- static BVH_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+ static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
diff --git a/trunk/fcl/include/fcl/interval.h b/trunk/fcl/include/fcl/interval.h
index 06eeb0d1..fe6a3b02 100644
--- a/trunk/fcl/include/fcl/interval.h
+++ b/trunk/fcl/include/fcl/interval.h
@@ -46,35 +46,35 @@ namespace fcl
struct Interval
{
- BVH_REAL i_[2];
+ FCL_REAL i_[2];
Interval() {}
- Interval(BVH_REAL v)
+ Interval(FCL_REAL v)
{
i_[0] = i_[1] = v;
}
- Interval(BVH_REAL left, BVH_REAL right)
+ Interval(FCL_REAL left, FCL_REAL right)
{
i_[0] = left; i_[1] = right;
}
- inline void setValue(BVH_REAL a, BVH_REAL b)
+ inline void setValue(FCL_REAL a, FCL_REAL b)
{
i_[0] = a; i_[1] = b;
}
- inline void setValue(BVH_REAL x)
+ inline void setValue(FCL_REAL x)
{
i_[0] = i_[1] = x;
}
- inline BVH_REAL operator [] (size_t i) const
+ inline FCL_REAL operator [] (size_t i) const
{
return i_[i];
}
- inline BVH_REAL& operator [] (size_t i)
+ inline FCL_REAL& operator [] (size_t i)
{
return i_[i];
}
@@ -112,13 +112,13 @@ struct Interval
Interval operator * (const Interval& other) const;
- inline Interval operator * (BVH_REAL d) const
+ inline Interval operator * (FCL_REAL d) const
{
if(d >= 0) return Interval(i_[0] * d, i_[1] * d);
return Interval(i_[1] * d, i_[0] * d);
}
- inline Interval& operator *= (BVH_REAL d)
+ inline Interval& operator *= (FCL_REAL d)
{
if(d >= 0)
{
@@ -127,7 +127,7 @@ struct Interval
}
else
{
- BVH_REAL tmp = i_[0];
+ FCL_REAL tmp = i_[0];
i_[0] = i_[1] * d;
i_[1] = tmp * d;
}
@@ -161,7 +161,7 @@ struct Interval
}
/** \brief Return the nearest distance for points within the interval to zero */
- inline BVH_REAL getAbsLower() const
+ inline FCL_REAL getAbsLower() const
{
if(i_[0] >= 0) return i_[0];
if(i_[1] >= 0) return 0;
@@ -169,14 +169,14 @@ struct Interval
}
/** \brief Return the farthest distance for points within the interval to zero */
- inline BVH_REAL getAbsUpper() const
+ inline FCL_REAL getAbsUpper() const
{
if(i_[0] + i_[1] >= 0) return i_[1];
return i_[0];
}
- inline bool contains(BVH_REAL v) const
+ inline bool contains(FCL_REAL v) const
{
if(v < i_[0]) return false;
if(v > i_[1]) return false;
@@ -184,13 +184,13 @@ struct Interval
}
/** \brief Compute the minimum interval contains v and original interval */
- inline void bound(BVH_REAL v)
+ inline void bound(FCL_REAL v)
{
if(v < i_[0]) i_[0] = v;
if(v > i_[1]) i_[1] = v;
}
- inline Interval bounded(BVH_REAL v) const
+ inline Interval bounded(FCL_REAL v) const
{
Interval res = *this;
if(v < res.i_[0]) res.i_[0] = v;
@@ -214,8 +214,8 @@ struct Interval
}
void print() const;
- inline BVH_REAL center() const { return 0.5 * (i_[0] + i_[1]); }
- inline BVH_REAL diameter() const { return i_[1] -i_[0]; }
+ inline FCL_REAL center() const { return 0.5 * (i_[0] + i_[1]); }
+ inline FCL_REAL diameter() const { return i_[1] -i_[0]; }
};
}
diff --git a/trunk/fcl/include/fcl/interval_matrix.h b/trunk/fcl/include/fcl/interval_matrix.h
index c7c4981e..147ad613 100644
--- a/trunk/fcl/include/fcl/interval_matrix.h
+++ b/trunk/fcl/include/fcl/interval_matrix.h
@@ -50,10 +50,10 @@ struct IMatrix3
Interval i_[3][3];
IMatrix3();
- IMatrix3(BVH_REAL v);
+ IMatrix3(FCL_REAL v);
IMatrix3(const Matrix3f& m);
- IMatrix3(BVH_REAL m[3][3][2]);
- IMatrix3(BVH_REAL m[3][3]);
+ IMatrix3(FCL_REAL m[3][3][2]);
+ IMatrix3(FCL_REAL m[3][3]);
IMatrix3(Interval m[3][3]);
void setIdentity();
diff --git a/trunk/fcl/include/fcl/interval_vector.h b/trunk/fcl/include/fcl/interval_vector.h
index dba1f789..2e3d3c61 100644
--- a/trunk/fcl/include/fcl/interval_vector.h
+++ b/trunk/fcl/include/fcl/interval_vector.h
@@ -49,11 +49,11 @@ struct IVector3
Interval i_[3];
IVector3();
- IVector3(BVH_REAL v);
- IVector3(BVH_REAL x, BVH_REAL y, BVH_REAL z);
- IVector3(BVH_REAL xl, BVH_REAL xu, BVH_REAL yl, BVH_REAL yu, BVH_REAL zl, BVH_REAL zu);
+ IVector3(FCL_REAL v);
+ IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z);
+ IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu);
IVector3(Interval v[3]);
- IVector3(BVH_REAL v[3][2]);
+ IVector3(FCL_REAL v[3][2]);
IVector3(const Interval& v1, const Interval& v2, const Interval& v3);
IVector3(const Vec3f& v);
@@ -77,7 +77,7 @@ struct IVector3
void print() const;
Vec3f center() const;
- BVH_REAL volumn() const;
+ FCL_REAL volumn() const;
void setZero();
void bound(const Vec3f& v);
diff --git a/trunk/fcl/include/fcl/math_details.h b/trunk/fcl/include/fcl/math_details.h
index ae29c78c..5726d369 100644
--- a/trunk/fcl/include/fcl/math_details.h
+++ b/trunk/fcl/include/fcl/math_details.h
@@ -84,6 +84,22 @@ struct Vec3Data
vs[0] = -vs[0]; vs[1] = -vs[1]; vs[2] = -vs[2];
}
+ inline Vec3Data<T>& ubound(const Vec3Data<T>& u)
+ {
+ vs[0] = std::min(vs[0], u.vs[0]);
+ vs[1] = std::min(vs[1], u.vs[1]);
+ vs[2] = std::min(vs[2], u.vs[2]);
+ return *this;
+ }
+
+ inline Vec3Data<T>& lbound(const Vec3Data<T>& l)
+ {
+ vs[0] = std::max(vs[0], l.vs[0]);
+ vs[1] = std::max(vs[1], l.vs[1]);
+ vs[2] = std::max(vs[2], l.vs[2]);
+ return *this;
+ }
+
T operator [] (size_t i) const { return vs[i]; }
T& operator [] (size_t i) { return vs[i]; }
diff --git a/trunk/fcl/include/fcl/math_simd_details.h b/trunk/fcl/include/fcl/math_simd_details.h
index d71196d4..37ea501d 100644
--- a/trunk/fcl/include/fcl/math_simd_details.h
+++ b/trunk/fcl/include/fcl/math_simd_details.h
@@ -38,6 +38,8 @@
#ifndef FCL_MATH_SIMD_DETAILS_H
#define FCL_MATH_SIMD_DETAILS_H
+#include "fcl/data_types.h"
+
#include <xmmintrin.h>
#if defined (__SSE3__)
#include <pmmintrin.h>
@@ -105,6 +107,18 @@ struct sse_meta_f4
inline void setValue(float x) { v = _mm_set1_ps(x); }
inline void setValue(__m128 x) { v = x; }
inline void negate() { v = _mm_sub_ps(xmms_0, v); }
+
+ inline sse_meta_f4& ubound(const sse_meta_f4& u)
+ {
+ v = _mm_min_ps(v, u.v);
+ return *this;
+ }
+
+ inline sse_meta_f4& lbound(const sse_meta_f4& l)
+ {
+ v = _mm_max_ps(v, l.v);
+ return *this;
+ }
inline void* operator new [] (size_t n) { return _mm_malloc(n, 16); }
inline void operator delete [] (void* x) { if(x) _mm_free(x); }
@@ -190,6 +204,20 @@ struct sse_meta_d4
v[1] = _mm_sub_pd(xmmd_0, v[1]);
}
+ inline sse_meta_d4& ubound(const sse_meta_d4& u)
+ {
+ v[0] = _mm_min_pd(v[0], u.v[0]);
+ v[1] = _mm_min_pd(v[1], u.v[1]);
+ return *this;
+ }
+
+ inline sse_meta_d4& lbound(const sse_meta_d4& l)
+ {
+ v[0] = _mm_max_pd(v[0], l.v[0]);
+ v[1] = _mm_max_pd(v[1], l.v[1]);
+ return *this;
+ }
+
inline void* operator new [] (size_t n)
{
return _mm_malloc(n, 16);
@@ -221,7 +249,7 @@ struct sse_meta_d4
inline sse_meta_d4& operator /= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_div_pd(v[0], d); v[1] = _mm_div_pd(v[1], d); return *this; }
inline sse_meta_d4 operator - () const
{
- static const union { int64_t i[2]; __m128d m; } negativemask __attribute__ ((aligned(16))) = {{0x8000000000000000, 0x8000000000000000}};
+ static const union { FCL_INT64 i[2]; __m128d m; } negativemask __attribute__ ((aligned(16))) = {{0x8000000000000000, 0x8000000000000000}};
return sse_meta_d4(_mm_xor_pd(v[0], negativemask.m), _mm_xor_pd(v[1], negativemask.m));
}
} __attribute__ ((aligned (16)));
@@ -376,7 +404,7 @@ static inline sse_meta_f4 abs(const sse_meta_f4& x)
static inline sse_meta_d4 abs(const sse_meta_d4& x)
{
- static const union { int64_t i[2]; __m128d m; } abs2mask __attribute__ ((aligned (16))) = {{0x7fffffffffffffff, 0x7fffffffffffffff}};
+ static const union { FCL_INT64 i[2]; __m128d m; } abs2mask __attribute__ ((aligned (16))) = {{0x7fffffffffffffff, 0x7fffffffffffffff}};
return sse_meta_d4(_mm_and_pd(x.v[0], abs2mask.m), _mm_and_pd(x.v[1], abs2mask.m));
}
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
index 944e5aa4..4e6e3be2 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -49,9 +49,9 @@ namespace fcl
/** \brief All zero matrix */
Matrix3f() {}
- Matrix3f(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
- BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
- BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
+ Matrix3f(FCL_REAL xx, FCL_REAL xy, FCL_REAL xz,
+ FCL_REAL yx, FCL_REAL yy, FCL_REAL yz,
+ FCL_REAL zx, FCL_REAL zy, FCL_REAL zz)
{
setValue(xx, xy, xz,
yx, yy, yz,
@@ -100,41 +100,41 @@ namespace fcl
return v_[i];
}
- inline BVH_REAL operator () (size_t i, size_t j) const
+ inline FCL_REAL operator () (size_t i, size_t j) const
{
return v_[i][j];
}
- inline BVH_REAL& operator() (size_t i, size_t j)
+ inline FCL_REAL& operator() (size_t i, size_t j)
{
return v_[i][j];
}
Matrix3f& operator *= (const Matrix3f& other);
- Matrix3f& operator += (BVH_REAL c);
+ Matrix3f& operator += (FCL_REAL c);
void setIdentity()
{
- setValue((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
- (BVH_REAL)0.0, (BVH_REAL)1.0, (BVH_REAL)0.0,
- (BVH_REAL)0.0, (BVH_REAL)0.0, (BVH_REAL)1.0);
+ setValue((FCL_REAL)1.0, (FCL_REAL)0.0, (FCL_REAL)0.0,
+ (FCL_REAL)0.0, (FCL_REAL)1.0, (FCL_REAL)0.0,
+ (FCL_REAL)0.0, (FCL_REAL)0.0, (FCL_REAL)1.0);
}
void setZero()
{
- setValue((BVH_REAL)0.0);
+ setValue((FCL_REAL)0.0);
}
static const Matrix3f& getIdentity()
{
- static const Matrix3f I((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
- (BVH_REAL)0.0, (BVH_REAL)1.0, (BVH_REAL)0.0,
- (BVH_REAL)0.0, (BVH_REAL)0.0, (BVH_REAL)1.0);
+ static const Matrix3f I((FCL_REAL)1.0, (FCL_REAL)0.0, (FCL_REAL)0.0,
+ (FCL_REAL)0.0, (FCL_REAL)1.0, (FCL_REAL)0.0,
+ (FCL_REAL)0.0, (FCL_REAL)0.0, (FCL_REAL)1.0);
return I;
}
- BVH_REAL determinant() const;
+ FCL_REAL determinant() const;
Matrix3f transpose() const;
Matrix3f inverse() const;
@@ -145,7 +145,7 @@ namespace fcl
Vec3f operator * (const Vec3f& v) const;
Vec3f transposeTimes(const Vec3f& v) const;
- inline BVH_REAL quadraticForm(const Vec3f& v) const
+ inline FCL_REAL quadraticForm(const Vec3f& v) const
{
return v[0] * v[0] * v_[0][0] + v[0] * v[1] * v_[0][1] + v[0] * v[2] * v_[0][2] +
v[1] * v[0] * v_[1][0] + v[1] * v[1] * v_[1][1] + v[1] * v[2] * v_[1][2] +
@@ -155,36 +155,36 @@ namespace fcl
/** S * M * S' */
Matrix3f tensorTransform(const Matrix3f& m) const;
- inline BVH_REAL transposeDotX(const Vec3f& v) const
+ inline FCL_REAL transposeDotX(const Vec3f& v) const
{
return v_[0][0] * v[0] + v_[1][0] * v[1] + v_[2][0] * v[2];
}
- inline BVH_REAL transposeDotY(const Vec3f& v) const
+ inline FCL_REAL transposeDotY(const Vec3f& v) const
{
return v_[0][1] * v[0] + v_[1][1] * v[1] + v_[2][1] * v[2];
}
- inline BVH_REAL transposeDotZ(const Vec3f& v) const
+ inline FCL_REAL transposeDotZ(const Vec3f& v) const
{
return v_[0][2] * v[0] + v_[1][2] * v[1] + v_[2][2] * v[2];
}
- inline BVH_REAL transposeDot(size_t i, const Vec3f& v) const
+ inline FCL_REAL transposeDot(size_t i, const Vec3f& v) const
{
return v_[0][i] * v[0] + v_[1][i] * v[1] + v_[2][i] * v[2];
}
- inline void setValue(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
- BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
- BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
+ inline void setValue(FCL_REAL xx, FCL_REAL xy, FCL_REAL xz,
+ FCL_REAL yx, FCL_REAL yy, FCL_REAL yz,
+ FCL_REAL zx, FCL_REAL zy, FCL_REAL zz)
{
v_[0].setValue(xx, xy, xz);
v_[1].setValue(yx, yy, yz);
v_[2].setValue(zx, zy, zz);
}
- inline void setValue(BVH_REAL x)
+ inline void setValue(FCL_REAL x)
{
v_[0].setValue(x);
v_[1].setValue(x);
@@ -194,7 +194,7 @@ namespace fcl
void relativeTransform(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, Matrix3f& R, Vec3f& T);
- void matEigen(const Matrix3f& R, BVH_REAL dout[3], Vec3f vout[3]);
+ void matEigen(const Matrix3f& R, FCL_REAL dout[3], Vec3f vout[3]);
Matrix3f abs(const Matrix3f& R);
}
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index e8af515f..357e5294 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -100,7 +100,7 @@ public:
Vec3f cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
Vec3f cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
- BVH_REAL cur_angle = cur_w.length();
+ FCL_REAL cur_angle = cur_w.length();
cur_w.normalize();
SimpleQuaternion cur_q;
@@ -116,20 +116,20 @@ public:
/** \brief Compute the motion bound for a bounding volume along a given direction n
* For general BV, not implemented so return trivial 0
*/
- BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
+ FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
- BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
+ FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
{
- BVH_REAL T_bound = computeTBound(n);
+ FCL_REAL T_bound = computeTBound(n);
- BVH_REAL R_bound = fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
- BVH_REAL R_bound_tmp = fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
+ FCL_REAL R_bound = fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
+ FCL_REAL R_bound_tmp = fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
R_bound_tmp = fabs(c.dot(n)) + c.length() + (c.cross(n)).length();
if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
- BVH_REAL dWdW_max = computeDWMax();
- BVH_REAL ratio = std::min(1 - tf_t, dWdW_max);
+ FCL_REAL dWdW_max = computeDWMax();
+ FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
R_bound *= 2 * ratio;
@@ -166,33 +166,33 @@ protected:
}
- BVH_REAL getWeight0(BVH_REAL t) const
+ FCL_REAL getWeight0(FCL_REAL t) const
{
return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0;
}
- BVH_REAL getWeight1(BVH_REAL t) const
+ FCL_REAL getWeight1(FCL_REAL t) const
{
return (4 - 6 * t * t + 3 * t * t * t) / 6.0;
}
- BVH_REAL getWeight2(BVH_REAL t) const
+ FCL_REAL getWeight2(FCL_REAL t) const
{
return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0;
}
- BVH_REAL getWeight3(BVH_REAL t) const
+ FCL_REAL getWeight3(FCL_REAL t) const
{
return t * t * t / 6.0;
}
- BVH_REAL computeTBound(const Vec3f& n) const
+ FCL_REAL computeTBound(const Vec3f& n) const
{
- BVH_REAL Ta = TA.dot(n);
- BVH_REAL Tb = TB.dot(n);
- BVH_REAL Tc = TC.dot(n);
+ FCL_REAL Ta = TA.dot(n);
+ FCL_REAL Tb = TB.dot(n);
+ FCL_REAL Tc = TC.dot(n);
- std::vector<BVH_REAL> T_potential;
+ std::vector<FCL_REAL> T_potential;
T_potential.push_back(tf_t);
T_potential.push_back(1);
if(Tb * Tb - 3 * Ta * Tc >= 0)
@@ -201,16 +201,16 @@ protected:
{
if(Tb != 0)
{
- BVH_REAL tmp = -Tc / (2 * Tb);
+ FCL_REAL tmp = -Tc / (2 * Tb);
if(tmp < 1 && tmp > tf_t)
T_potential.push_back(tmp);
}
}
else
{
- BVH_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
- BVH_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta);
- BVH_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta);
+ FCL_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
+ FCL_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta);
+ FCL_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta);
if(tmp1 < 1 && tmp1 > tf_t)
T_potential.push_back(tmp1);
if(tmp2 < 1 && tmp2 > tf_t)
@@ -218,15 +218,15 @@ protected:
}
}
- BVH_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
+ FCL_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
for(unsigned int i = 1; i < T_potential.size(); ++i)
{
- BVH_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
+ FCL_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
if(T_bound_tmp > T_bound) T_bound = T_bound_tmp;
}
- BVH_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
+ FCL_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
T_bound -= cur_delta;
T_bound /= 6.0;
@@ -234,7 +234,7 @@ protected:
return T_bound;
}
- BVH_REAL computeDWMax() const
+ FCL_REAL computeDWMax() const
{
// first compute ||w'||
int a00[5] = {1,-4,6,-4,1};
@@ -248,7 +248,7 @@ protected:
int a23[5] = {-3,2,1,0,0};
int a33[5] = {1,0,0,0,0};
- BVH_REAL a[5];
+ FCL_REAL a[5];
for(int i = 0; i < 5; ++i)
{
@@ -271,7 +271,7 @@ protected:
int da23[4] = {-12,6,2,0};
int da33[4] = {4,0,0,0};
- BVH_REAL da[4];
+ FCL_REAL da[4];
for(int i = 0; i < 4; ++i)
{
da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i]
@@ -281,20 +281,20 @@ protected:
da[i] /= 4.0;
}
- BVH_REAL roots[3];
+ FCL_REAL roots[3];
int root_num = PolySolver::solveCubic(da, roots);
- BVH_REAL dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
- BVH_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
+ FCL_REAL dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
+ FCL_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
if(dWdW_max < dWdW_1) dWdW_max = dWdW_1;
for(int i = 0; i < root_num; ++i)
{
- BVH_REAL v = roots[i];
+ FCL_REAL v = roots[i];
if(v >= tf_t && v <= 1)
{
- BVH_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
+ FCL_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
if(value > dWdW_max) dWdW_max = value;
}
}
@@ -308,12 +308,12 @@ protected:
Vec3f TA, TB, TC;
Vec3f RA, RB, RC;
- BVH_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
+ FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
/** \brief The transformation at current time t */
SimpleTransform tf;
/** \brief The time related with tf */
- BVH_REAL tf_t;
+ FCL_REAL tf_t;
};
template<typename BV>
@@ -364,12 +364,12 @@ public:
/** \brief Compute the motion bound for a bounding volume along a given direction n
* For general BV, not implemented so return trivial 0
*/
- BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
+ FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
- BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
+ FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
{
- BVH_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
- BVH_REAL tmp;
+ FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
+ FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
if(tmp > proj_max) proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
@@ -377,9 +377,9 @@ public:
proj_max = sqrt(proj_max);
- BVH_REAL v_dot_n = axis.dot(n) * linear_vel;
- BVH_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
- BVH_REAL mu = v_dot_n + w_cross_n * proj_max;
+ FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
+ FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
+ FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
return mu;
}
@@ -432,14 +432,14 @@ protected:
}
}
- SimpleQuaternion deltaRotation(BVH_REAL dt) const
+ SimpleQuaternion deltaRotation(FCL_REAL dt) const
{
SimpleQuaternion res;
- res.fromAxisAngle(axis, (BVH_REAL)(dt * angular_vel));
+ res.fromAxisAngle(axis, (FCL_REAL)(dt * angular_vel));
return res;
}
- SimpleQuaternion absoluteRotation(BVH_REAL dt) const
+ SimpleQuaternion absoluteRotation(FCL_REAL dt) const
{
SimpleQuaternion delta_t = deltaRotation(dt);
return delta_t * tf1.getQuatRotation();
@@ -461,10 +461,10 @@ protected:
Vec3f p;
/** \brief linear velocity along the axis */
- BVH_REAL linear_vel;
+ FCL_REAL linear_vel;
/** \brief angular velocity */
- BVH_REAL angular_vel;
+ FCL_REAL angular_vel;
};
@@ -474,7 +474,7 @@ protected:
* Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
template<>
-BVH_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
+FCL_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
/** \brief Linear interpolation motion
@@ -549,17 +549,17 @@ public:
/** \brief Compute the motion bound for a bounding volume along a given direction n
* For general BV, not implemented so return trivial 0
*/
- BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
+ FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
/** \brief Compute the motion bound for a triangle along a given direction n
* according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity
* and ci are the triangle vertex coordinates.
* Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
- BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
+ FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
{
- BVH_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
- BVH_REAL tmp;
+ FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
+ FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > proj_max) proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
@@ -567,9 +567,9 @@ public:
proj_max = sqrt(proj_max);
- BVH_REAL v_dot_n = linear_vel.dot(n);
- BVH_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- BVH_REAL mu = v_dot_n + w_cross_n * proj_max;
+ FCL_REAL v_dot_n = linear_vel.dot(n);
+ FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
+ FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
return mu;
}
@@ -611,14 +611,14 @@ protected:
}
- SimpleQuaternion deltaRotation(BVH_REAL dt) const
+ SimpleQuaternion deltaRotation(FCL_REAL dt) const
{
SimpleQuaternion res;
- res.fromAxisAngle(angular_axis, (BVH_REAL)(dt * angular_vel));
+ res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel));
return res;
}
- SimpleQuaternion absoluteRotation(BVH_REAL dt) const
+ SimpleQuaternion absoluteRotation(FCL_REAL dt) const
{
SimpleQuaternion delta_t = deltaRotation(dt);
return delta_t * tf1.getQuatRotation();
@@ -637,7 +637,7 @@ protected:
Vec3f linear_vel;
/** \brief Angular speed */
- BVH_REAL angular_vel;
+ FCL_REAL angular_vel;
/** \brief Angular velocity axis */
Vec3f angular_axis;
@@ -653,7 +653,7 @@ protected:
* Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
template<>
-BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
+FCL_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
}
diff --git a/trunk/fcl/include/fcl/motion_base.h b/trunk/fcl/include/fcl/motion_base.h
index 1b4f9443..6a310d44 100644
--- a/trunk/fcl/include/fcl/motion_base.h
+++ b/trunk/fcl/include/fcl/motion_base.h
@@ -55,10 +55,10 @@ public:
virtual bool integrate(double dt) = 0;
/** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
- virtual BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const = 0;
+ virtual FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const = 0;
/** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */
- virtual BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
+ virtual FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
/** \brief Get the rotation and translation in current step */
virtual void getCurrentTransform(Matrix3f& R, Vec3f& T) const = 0;
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk.h b/trunk/fcl/include/fcl/narrowphase/gjk.h
index fc38a119..1cacbdf9 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk.h
@@ -83,11 +83,11 @@ struct MinkowskiDiff
};
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m);
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m);
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL* w, size_t& m);
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m);
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, BVH_REAL* w, size_t& m);
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m);
struct GJK
{
@@ -100,7 +100,7 @@ struct GJK
struct Simplex
{
SimplexV* c[4]; // simplex vertex
- BVH_REAL p[4]; // weight
+ FCL_REAL p[4]; // weight
size_t rank; // size of simplex (number of vertices)
};
@@ -108,11 +108,11 @@ struct GJK
MinkowskiDiff shape;
Vec3f ray;
- BVH_REAL distance;
+ FCL_REAL distance;
Simplex simplices[2];
- GJK(unsigned int max_iterations_, BVH_REAL tolerance_)
+ GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
{
max_iterations = max_iterations_;
tolerance = tolerance_;
@@ -145,14 +145,14 @@ private:
Simplex* simplex;
Status status;
- BVH_REAL tolerance;
+ FCL_REAL tolerance;
unsigned int max_iterations;
};
static const size_t EPA_MAX_FACES = 128;
static const size_t EPA_MAX_VERTICES = 64;
-static const BVH_REAL EPA_EPS = 0.000001;
+static const FCL_REAL EPA_EPS = 0.000001;
static const size_t EPA_MAX_ITERATIONS = 255;
struct EPA
@@ -162,7 +162,7 @@ private:
struct SimplexF
{
Vec3f n;
- BVH_REAL d;
+ FCL_REAL d;
SimplexV* c[3]; // a face has three vertices
SimplexF* f[3]; // a face has three adjacent faces
SimplexF* l[2]; // the pre and post faces in the list
@@ -211,7 +211,7 @@ private:
unsigned int max_face_num;
unsigned int max_vertex_num;
unsigned int max_iterations;
- BVH_REAL tolerance;
+ FCL_REAL tolerance;
public:
@@ -220,13 +220,13 @@ public:
Status status;
GJK::Simplex result;
Vec3f normal;
- BVH_REAL depth;
+ FCL_REAL depth;
SimplexV* sv_store;
SimplexF* fc_store;
size_t nextsv;
SimplexList hull, stock;
- EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, BVH_REAL tolerance_)
+ EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
{
max_face_num = max_face_num_;
max_vertex_num = max_vertex_num_;
@@ -244,7 +244,7 @@ public:
void initialize();
- bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist);
+ bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
index cc2658dc..dcd6dea6 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
@@ -155,13 +155,13 @@ void triDeleteGJKObject(void* o);
/** \brief GJK collision algorithm */
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
- unsigned int max_iterations, BVH_REAL tolerance,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
+ unsigned int max_iterations, FCL_REAL tolerance,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal);
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
- unsigned int max_iterations, BVH_REAL tolerance,
- BVH_REAL* dist);
+ unsigned int max_iterations, FCL_REAL tolerance,
+ FCL_REAL* dist);
} // details
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index 7f72db42..6091684a 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -51,7 +51,7 @@ struct GJKSolver_libccd
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
@@ -70,7 +70,7 @@ struct GJKSolver_libccd
/** \brief intersection checking between one shape and a triangle */
template<typename S>
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3);
@@ -90,7 +90,7 @@ struct GJKSolver_libccd
template<typename S>
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
@@ -111,7 +111,7 @@ struct GJKSolver_libccd
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
@@ -134,7 +134,7 @@ struct GJKSolver_libccd
template<typename S>
bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3);
@@ -156,7 +156,7 @@ struct GJKSolver_libccd
template<typename S>
bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
@@ -185,8 +185,8 @@ struct GJKSolver_libccd
unsigned int max_collision_iterations;
unsigned int max_distance_iterations;
- BVH_REAL collision_tolerance;
- BVH_REAL distance_tolerance;
+ FCL_REAL collision_tolerance;
+ FCL_REAL distance_tolerance;
};
@@ -196,41 +196,41 @@ struct GJKSolver_libccd
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-sphere distance */
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- BVH_REAL* dist) const;
+ FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- BVH_REAL* dist) const;
+ FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- BVH_REAL* dist) const;
+ FCL_REAL* dist) const;
/** \brief Fast implementation for box-box collision */
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
const Box& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
struct GJKSolver_indep
@@ -238,7 +238,7 @@ struct GJKSolver_indep
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Vec3f guess(1, 0, 0);
details::MinkowskiDiff shape;
@@ -281,7 +281,7 @@ struct GJKSolver_indep
template<typename S>
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Triangle2 tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
@@ -324,7 +324,7 @@ struct GJKSolver_indep
template<typename S>
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
Triangle2 tri(P1, P2, P3);
SimpleTransform tf2(R, T);
@@ -369,7 +369,7 @@ struct GJKSolver_indep
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
- BVH_REAL* distance) const
+ FCL_REAL* distance) const
{
Vec3f guess(1, 0, 0);
details::MinkowskiDiff shape;
@@ -385,7 +385,7 @@ struct GJKSolver_indep
Vec3f w0, w1;
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- BVH_REAL p = gjk.getSimplex()->p[i];
+ FCL_REAL p = gjk.getSimplex()->p[i];
w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
@@ -406,7 +406,7 @@ struct GJKSolver_indep
template<typename S>
bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- BVH_REAL* distance) const
+ FCL_REAL* distance) const
{
Triangle2 tri(P1, P2, P3);
Vec3f guess(1, 0, 0);
@@ -423,7 +423,7 @@ struct GJKSolver_indep
Vec3f w0, w1;
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- BVH_REAL p = gjk.getSimplex()->p[i];
+ FCL_REAL p = gjk.getSimplex()->p[i];
w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
@@ -441,7 +441,7 @@ struct GJKSolver_indep
template<typename S>
bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- BVH_REAL* distance) const
+ FCL_REAL* distance) const
{
Triangle2 tri(P1, P2, P3);
SimpleTransform tf2(R, T);
@@ -459,7 +459,7 @@ struct GJKSolver_indep
Vec3f w0, w1;
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- BVH_REAL p = gjk.getSimplex()->p[i];
+ FCL_REAL p = gjk.getSimplex()->p[i];
w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
@@ -488,50 +488,50 @@ struct GJKSolver_indep
unsigned int epa_max_face_num;
unsigned int epa_max_vertex_num;
unsigned int epa_max_iterations;
- BVH_REAL epa_tolerance;
- BVH_REAL gjk_tolerance;
- BVH_REAL gjk_max_iterations;
+ FCL_REAL epa_tolerance;
+ FCL_REAL gjk_tolerance;
+ FCL_REAL gjk_max_iterations;
};
/** \brief Fast implementation for sphere-sphere collision */
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-triangle collision */
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/** \brief Fast implementation for sphere-sphere distance */
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- BVH_REAL* dist) const;
+ FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- BVH_REAL* dist) const;
+ FCL_REAL* dist) const;
/** \brief Fast implementation for sphere-triangle distance */
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- BVH_REAL* dist) const;
+ FCL_REAL* dist) const;
/** \brief Fast implementation for box-box collision */
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
const Box& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
}
diff --git a/trunk/fcl/include/fcl/primitive.h b/trunk/fcl/include/fcl/primitive.h
index 0352506d..9b25878b 100644
--- a/trunk/fcl/include/fcl/primitive.h
+++ b/trunk/fcl/include/fcl/primitive.h
@@ -89,7 +89,7 @@ struct Uncertainty
Matrix3f Sigma;
/** \brief Variations along the eigen axes */
- BVH_REAL sigma[3];
+ FCL_REAL sigma[3];
/** \brief eigen axes of uncertainty matrix */
Vec3f axis[3];
diff --git a/trunk/fcl/include/fcl/simd_intersect.h b/trunk/fcl/include/fcl/simd_intersect.h
index 79c90c91..d35d87df 100644
--- a/trunk/fcl/include/fcl/simd_intersect.h
+++ b/trunk/fcl/include/fcl/simd_intersect.h
@@ -47,14 +47,14 @@ namespace fcl
{
-static inline __m128 sse_four_spheres_intersect(const Vec3f& o1, BVH_REAL r1,
- const Vec3f& o2, BVH_REAL r2,
- const Vec3f& o3, BVH_REAL r3,
- const Vec3f& o4, BVH_REAL r4,
- const Vec3f& o5, BVH_REAL r5,
- const Vec3f& o6, BVH_REAL r6,
- const Vec3f& o7, BVH_REAL r7,
- const Vec3f& o8, BVH_REAL r8)
+static inline __m128 sse_four_spheres_intersect(const Vec3f& o1, FCL_REAL r1,
+ const Vec3f& o2, FCL_REAL r2,
+ const Vec3f& o3, FCL_REAL r3,
+ const Vec3f& o4, FCL_REAL r4,
+ const Vec3f& o5, FCL_REAL r5,
+ const Vec3f& o6, FCL_REAL r6,
+ const Vec3f& o7, FCL_REAL r7,
+ const Vec3f& o8, FCL_REAL r8)
{
__m128 PX, PY, PZ, PR, QX, QY, QZ, QR;
PX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
@@ -80,10 +80,10 @@ static inline __m128 sse_four_spheres_intersect(const Vec3f& o1, BVH_REAL r1,
}
-static inline __m128 sse_four_spheres_four_AABBs_intersect(const Vec3f& o1, BVH_REAL r1,
- const Vec3f& o2, BVH_REAL r2,
- const Vec3f& o3, BVH_REAL r3,
- const Vec3f& o4, BVH_REAL r4,
+static inline __m128 sse_four_spheres_four_AABBs_intersect(const Vec3f& o1, FCL_REAL r1,
+ const Vec3f& o2, FCL_REAL r2,
+ const Vec3f& o3, FCL_REAL r3,
+ const Vec3f& o4, FCL_REAL r4,
const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
@@ -157,37 +157,37 @@ static inline __m128 sse_four_AABBs_intersect(const Vec3f& min1, const Vec3f& ma
return _mm_and_ps(T3, T4);
}
-static bool four_spheres_intersect_and(const Vec3f& o1, BVH_REAL r1,
- const Vec3f& o2, BVH_REAL r2,
- const Vec3f& o3, BVH_REAL r3,
- const Vec3f& o4, BVH_REAL r4,
- const Vec3f& o5, BVH_REAL r5,
- const Vec3f& o6, BVH_REAL r6,
- const Vec3f& o7, BVH_REAL r7,
- const Vec3f& o8, BVH_REAL r8)
+static bool four_spheres_intersect_and(const Vec3f& o1, FCL_REAL r1,
+ const Vec3f& o2, FCL_REAL r2,
+ const Vec3f& o3, FCL_REAL r3,
+ const Vec3f& o4, FCL_REAL r4,
+ const Vec3f& o5, FCL_REAL r5,
+ const Vec3f& o6, FCL_REAL r6,
+ const Vec3f& o7, FCL_REAL r7,
+ const Vec3f& o8, FCL_REAL r8)
{
__m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
return _mm_movemask_ps(CMP) == 15.f;
}
-static bool four_spheres_intersect_or(const Vec3f& o1, BVH_REAL r1,
- const Vec3f& o2, BVH_REAL r2,
- const Vec3f& o3, BVH_REAL r3,
- const Vec3f& o4, BVH_REAL r4,
- const Vec3f& o5, BVH_REAL r5,
- const Vec3f& o6, BVH_REAL r6,
- const Vec3f& o7, BVH_REAL r7,
- const Vec3f& o8, BVH_REAL r8)
+static bool four_spheres_intersect_or(const Vec3f& o1, FCL_REAL r1,
+ const Vec3f& o2, FCL_REAL r2,
+ const Vec3f& o3, FCL_REAL r3,
+ const Vec3f& o4, FCL_REAL r4,
+ const Vec3f& o5, FCL_REAL r5,
+ const Vec3f& o6, FCL_REAL r6,
+ const Vec3f& o7, FCL_REAL r7,
+ const Vec3f& o8, FCL_REAL r8)
{
__m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
return __mm_movemask_ps(CMP) > 0;
}
/** \brief four spheres versus four AABBs SIMD test */
-static bool four_spheres_four_AABBs_intersect_and(const Vec3f& o1, BVH_REAL r1,
- const Vec3f& o2, BVH_REAL r2,
- const Vec3f& o3, BVH_REAL r3,
- const Vec3f& o4, BVH_REAL r4,
+static bool four_spheres_four_AABBs_intersect_and(const Vec3f& o1, FCL_REAL r1,
+ const Vec3f& o2, FCL_REAL r2,
+ const Vec3f& o3, FCL_REAL r3,
+ const Vec3f& o4, FCL_REAL r4,
const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
@@ -197,10 +197,10 @@ static bool four_spheres_four_AABBs_intersect_and(const Vec3f& o1, BVH_REAL r1,
return _mm_movemask_ps(CMP) == 15.f;
}
-static bool four_spheres_four_AABBs_intersect_or(const Vec3f& o1, BVH_REAL r1,
- const Vec3f& o2, BVH_REAL r2,
- const Vec3f& o3, BVH_REAL r3,
- const Vec3f& o4, BVH_REAL r4,
+static bool four_spheres_four_AABBs_intersect_or(const Vec3f& o1, FCL_REAL r1,
+ const Vec3f& o2, FCL_REAL r2,
+ const Vec3f& o3, FCL_REAL r3,
+ const Vec3f& o4, FCL_REAL r4,
const Vec3f& min1, const Vec3f& max1,
const Vec3f& min2, const Vec3f& max2,
const Vec3f& min3, const Vec3f& max3,
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 59bc0d3e..1acb7e96 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -414,12 +414,12 @@ template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudCollisionTraversalNode<BV>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold = 0.5,
+ FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
int num_max_contacts = 1,
bool exhaustive = false,
bool enable_contact = false,
- BVH_REAL expand_r = 1)
+ FCL_REAL expand_r = 1)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
|| !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
@@ -489,35 +489,35 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node,
bool initialize(PointCloudCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
BVHModel<OBB>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold = 0.5,
+ FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
int num_max_contacts = 1,
bool exhaustive = false,
bool enable_contact = false,
- BVH_REAL expand_r = 1);
+ FCL_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
bool initialize(PointCloudCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
BVHModel<RSS>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold = 0.5,
+ FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
int num_max_contacts = 1,
bool exhaustive = false,
bool enable_contact = false,
- BVH_REAL expand_r = 1);
+ FCL_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold = 0.5,
+ FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
int num_max_contacts = 1,
bool exhaustive = false,
bool enable_contact = false,
- BVH_REAL expand_r = 1)
+ FCL_REAL expand_r = 1)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -585,23 +585,23 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold = 0.5,
+ FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
int num_max_contacts = 1,
bool exhaustive = false,
bool enable_contact = false,
- BVH_REAL expand_r = 1);
+ FCL_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold = 0.5,
+ FCL_REAL collision_prob_threshold = 0.5,
int leaf_size_threshold = 1,
int num_max_contacts = 1,
bool exhaustive = false,
bool enable_contact = false,
- BVH_REAL expand_r = 1);
+ FCL_REAL expand_r = 1);
#endif
@@ -995,7 +995,7 @@ template<typename BV>
bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
BVHModel<BV>& model1,
BVHModel<BV>& model2,
- const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, BVH_REAL w = 1,
+ const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -1043,7 +1043,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS */
inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, BVH_REAL w = 1)
+ const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
diff --git a/trunk/fcl/include/fcl/taylor_matrix.h b/trunk/fcl/include/fcl/taylor_matrix.h
index 81674d8c..ef545b2b 100644
--- a/trunk/fcl/include/fcl/taylor_matrix.h
+++ b/trunk/fcl/include/fcl/taylor_matrix.h
@@ -71,7 +71,7 @@ struct TMatrix3
void print() const;
void setIdentity();
void setZero();
- BVH_REAL diameter() const;
+ FCL_REAL diameter() const;
};
}
diff --git a/trunk/fcl/include/fcl/taylor_model.h b/trunk/fcl/include/fcl/taylor_model.h
index c7c41428..5d73c672 100644
--- a/trunk/fcl/include/fcl/taylor_model.h
+++ b/trunk/fcl/include/fcl/taylor_model.h
@@ -49,35 +49,35 @@ namespace fcl
struct TaylorModel
{
/** \brief Coefficients of the cubic polynomial approximation */
- BVH_REAL coeffs_[4];
+ FCL_REAL coeffs_[4];
/** \brief interval remainder */
Interval r_;
- void setTimeInterval(BVH_REAL l, BVH_REAL r);
+ void setTimeInterval(FCL_REAL l, FCL_REAL r);
TaylorModel();
- TaylorModel(BVH_REAL coeff);
- TaylorModel(BVH_REAL coeffs[3], const Interval& r);
- TaylorModel(BVH_REAL c0, BVH_REAL c1, BVH_REAL c2, BVH_REAL c3, const Interval& r);
+ TaylorModel(FCL_REAL coeff);
+ TaylorModel(FCL_REAL coeffs[3], const Interval& r);
+ TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r);
TaylorModel operator + (const TaylorModel& other) const;
TaylorModel operator - (const TaylorModel& other) const;
TaylorModel& operator += (const TaylorModel& other);
TaylorModel& operator -= (const TaylorModel& other);
TaylorModel operator * (const TaylorModel& other) const;
- TaylorModel operator * (BVH_REAL d) const;
+ TaylorModel operator * (FCL_REAL d) const;
TaylorModel operator - () const;
void print() const;
Interval getBound() const;
- Interval getBound(BVH_REAL l, BVH_REAL r) const;
+ Interval getBound(FCL_REAL l, FCL_REAL r) const;
Interval getTightBound() const;
- Interval getTightBound(BVH_REAL l, BVH_REAL r) const;
+ Interval getTightBound(FCL_REAL l, FCL_REAL r) const;
- Interval getBound(BVH_REAL t) const;
+ Interval getBound(FCL_REAL t) const;
void setZero();
@@ -89,12 +89,12 @@ struct TaylorModel
Interval t5_; // [t1, t2]^5
Interval t6_; // [t1, t2]^6
- static const BVH_REAL PI_;
+ static const FCL_REAL PI_;
};
-void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0);
-void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0);
-void generateTaylorModelForLinearFunc(TaylorModel& tm, BVH_REAL p, BVH_REAL v);
+void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
+void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
+void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v);
}
diff --git a/trunk/fcl/include/fcl/taylor_vector.h b/trunk/fcl/include/fcl/taylor_vector.h
index c43a6a1e..50a98e44 100644
--- a/trunk/fcl/include/fcl/taylor_vector.h
+++ b/trunk/fcl/include/fcl/taylor_vector.h
@@ -53,7 +53,7 @@ struct TVector3
TVector3(const Vec3f& v);
TVector3 operator + (const TVector3& other) const;
- TVector3 operator + (BVH_REAL d) const;
+ TVector3 operator + (FCL_REAL d) const;
TVector3& operator += (const TVector3& other);
TVector3 operator - (const TVector3& other) const;
TVector3& operator -= (const TVector3& other);
@@ -66,10 +66,10 @@ struct TVector3
TVector3 cross(const TVector3& other) const;
IVector3 getBound() const;
- IVector3 getBound(BVH_REAL t) const;
+ IVector3 getBound(FCL_REAL t) const;
void print() const;
- BVH_REAL volumn() const;
+ FCL_REAL volumn() const;
void setZero();
TaylorModel squareLength() const;
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index f72038ff..260af6db 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -57,7 +57,7 @@ public:
data[3] = 0;
}
- SimpleQuaternion(BVH_REAL a, BVH_REAL b, BVH_REAL c, BVH_REAL d)
+ SimpleQuaternion(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
{
data[0] = a; // w
data[1] = b; // x
@@ -78,13 +78,13 @@ public:
void toAxes(Vec3f axis[3]) const;
/** \brief Axis and angle to quaternion */
- void fromAxisAngle(const Vec3f& axis, BVH_REAL angle);
+ void fromAxisAngle(const Vec3f& axis, FCL_REAL angle);
/** \brief Quaternion to axis and angle */
- void toAxisAngle(Vec3f& axis, BVH_REAL& angle) const;
+ void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const;
/** \brief Dot product between quaternions */
- BVH_REAL dot(const SimpleQuaternion& other) const;
+ FCL_REAL dot(const SimpleQuaternion& other) const;
/** \brief addition */
SimpleQuaternion operator + (const SimpleQuaternion& other) const;
@@ -102,8 +102,8 @@ public:
SimpleQuaternion operator - () const;
/** \brief scalar multiplication */
- SimpleQuaternion operator * (BVH_REAL t) const;
- const SimpleQuaternion& operator *= (BVH_REAL t);
+ SimpleQuaternion operator * (FCL_REAL t) const;
+ const SimpleQuaternion& operator *= (FCL_REAL t);
/** \brief conjugate */
SimpleQuaternion conj() const;
@@ -114,19 +114,19 @@ public:
/** \brief rotate a vector */
Vec3f transform(const Vec3f& v) const;
- inline const BVH_REAL& getW() const { return data[0]; }
- inline const BVH_REAL& getX() const { return data[1]; }
- inline const BVH_REAL& getY() const { return data[2]; }
- inline const BVH_REAL& getZ() const { return data[3]; }
+ inline const FCL_REAL& getW() const { return data[0]; }
+ inline const FCL_REAL& getX() const { return data[1]; }
+ inline const FCL_REAL& getY() const { return data[2]; }
+ inline const FCL_REAL& getZ() const { return data[3]; }
- inline BVH_REAL& getW() { return data[0]; }
- inline BVH_REAL& getX() { return data[1]; }
- inline BVH_REAL& getY() { return data[2]; }
- inline BVH_REAL& getZ() { return data[3]; }
+ inline FCL_REAL& getW() { return data[0]; }
+ inline FCL_REAL& getX() { return data[1]; }
+ inline FCL_REAL& getY() { return data[2]; }
+ inline FCL_REAL& getZ() { return data[3]; }
private:
- BVH_REAL data[4];
+ FCL_REAL data[4];
};
/** \brief Simple transform class used locally by InterpMotion */
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index 18a7b8a0..6557ef16 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -109,11 +109,11 @@ public:
virtual ~DistanceTraversalNodeBase();
- virtual BVH_REAL BVTesting(int b1, int b2) const;
+ virtual FCL_REAL BVTesting(int b1, int b2) const;
virtual void leafTesting(int b1, int b2) const;
- virtual bool canStop(BVH_REAL c) const;
+ virtual bool canStop(FCL_REAL c) const;
};
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index 72782ba7..da72cc0d 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -87,7 +87,7 @@ public:
mutable int num_bv_tests;
mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
+ mutable FCL_REAL query_time_seconds;
};
@@ -137,7 +137,7 @@ public:
mutable int num_bv_tests;
mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
+ mutable FCL_REAL query_time_seconds;
};
@@ -149,7 +149,7 @@ struct BVHShapeCollisionPair
BVHShapeCollisionPair(int id_) : id(id_) {}
- BVHShapeCollisionPair(int id_, const Vec3f& n, const Vec3f& contactp, BVH_REAL depth) : id(id_),
+ BVHShapeCollisionPair(int id_, const Vec3f& n, const Vec3f& contactp, FCL_REAL depth) : id(id_),
normal(n), contact_point(contactp), penetration_depth(depth) {}
/** \brief The index of BVH's in-collision primitive */
@@ -162,7 +162,7 @@ struct BVHShapeCollisionPair
Vec3f contact_point;
/** \brief Penetration depth for two triangles */
- BVH_REAL penetration_depth;
+ FCL_REAL penetration_depth;
};
struct BVHShapeCollisionPairComp
@@ -203,7 +203,7 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- BVH_REAL penetration;
+ FCL_REAL penetration;
Vec3f normal;
Vec3f contactp;
@@ -269,7 +269,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- BVH_REAL penetration;
+ FCL_REAL penetration;
Vec3f normal;
Vec3f contactp;
@@ -428,7 +428,7 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- BVH_REAL penetration;
+ FCL_REAL penetration;
Vec3f normal;
Vec3f contactp;
@@ -614,7 +614,7 @@ public:
return model1->getBV(b).rightChild();
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
return model1->getBV(b1).bv.distance(model2_bv);
}
@@ -625,7 +625,7 @@ public:
mutable int num_bv_tests;
mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
+ mutable FCL_REAL query_time_seconds;
};
template<typename S, typename BV>
@@ -657,7 +657,7 @@ public:
return model2->getBV(b).rightChild();
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
return model1_bv.distance(model2->getBV(b2).bv);
}
@@ -668,7 +668,7 @@ public:
mutable int num_bv_tests;
mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
+ mutable FCL_REAL query_time_seconds;
};
@@ -687,7 +687,7 @@ public:
rel_err = 0;
abs_err = 0;
- min_distance = std::numeric_limits<BVH_REAL>::max();
+ min_distance = std::numeric_limits<FCL_REAL>::max();
nsolver = NULL;
}
@@ -706,7 +706,7 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- BVH_REAL d;
+ FCL_REAL d;
nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d);
if(d < min_distance)
@@ -717,7 +717,7 @@ public:
}
}
- bool canStop(BVH_REAL c) const
+ bool canStop(FCL_REAL c) const
{
if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
return true;
@@ -727,10 +727,10 @@ public:
Vec3f* vertices;
Triangle* tri_indices;
- BVH_REAL rel_err;
- BVH_REAL abs_err;
+ FCL_REAL rel_err;
+ FCL_REAL abs_err;
- mutable BVH_REAL min_distance;
+ mutable FCL_REAL min_distance;
mutable int last_tri_id;
const NarrowPhaseSolver* nsolver;
@@ -749,7 +749,7 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
bool enable_statistics,
int & num_leaf_tests,
int& last_tri_id,
- BVH_REAL& min_distance)
+ FCL_REAL& min_distance)
{
if(enable_statistics) num_leaf_tests++;
@@ -761,7 +761,7 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- BVH_REAL dist;
+ FCL_REAL dist;
nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, R, T, &dist);
@@ -781,7 +781,7 @@ static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, i
const Matrix3f& R, const Vec3f& T,
const S& s, const SimpleTransform& tf,
const NarrowPhaseSolver* nsolver,
- BVH_REAL& min_distance)
+ FCL_REAL& min_distance)
{
const Triangle& last_tri = tri_indices[last_tri_id];
@@ -791,7 +791,7 @@ static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, i
Vec3f last_tri_P, last_tri_Q;
- BVH_REAL dist;
+ FCL_REAL dist;
nsolver->shapeTriangleDistance(s, tf, p1, p2, p3, R, T, &dist);
min_distance = dist;
@@ -824,7 +824,7 @@ public:
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
@@ -860,7 +860,7 @@ public:
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
@@ -895,7 +895,7 @@ public:
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
@@ -926,7 +926,7 @@ public:
rel_err = 0;
abs_err = 0;
- min_distance = std::numeric_limits<BVH_REAL>::max();
+ min_distance = std::numeric_limits<FCL_REAL>::max();
nsolver = NULL;
}
@@ -945,7 +945,7 @@ public:
const Vec3f& p2 = vertices[tri_id[1]];
const Vec3f& p3 = vertices[tri_id[2]];
- BVH_REAL d;
+ FCL_REAL d;
nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d);
if(d < min_distance)
@@ -956,7 +956,7 @@ public:
}
}
- bool canStop(BVH_REAL c) const
+ bool canStop(FCL_REAL c) const
{
if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
return true;
@@ -966,10 +966,10 @@ public:
Vec3f* vertices;
Triangle* tri_indices;
- BVH_REAL rel_err;
- BVH_REAL abs_err;
+ FCL_REAL rel_err;
+ FCL_REAL abs_err;
- mutable BVH_REAL min_distance;
+ mutable FCL_REAL min_distance;
mutable int last_tri_id;
const NarrowPhaseSolver* nsolver;
@@ -994,7 +994,7 @@ public:
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
@@ -1029,7 +1029,7 @@ public:
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
@@ -1064,7 +1064,7 @@ public:
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index bda78846..f9df09ff 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -85,8 +85,8 @@ public:
/** \brief Determine the traversal order, is the first BVTT subtree better */
bool firstOverSecond(int b1, int b2) const
{
- BVH_REAL sz1 = model1->getBV(b1).bv.size();
- BVH_REAL sz2 = model2->getBV(b2).bv.size();
+ FCL_REAL sz1 = model1->getBV(b1).bv.size();
+ FCL_REAL sz2 = model2->getBV(b2).bv.size();
bool l1 = model1->getBV(b1).isLeaf();
bool l2 = model2->getBV(b2).isLeaf();
@@ -135,7 +135,7 @@ public:
/** \brief statistical information */
mutable int num_bv_tests;
mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
+ mutable FCL_REAL query_time_seconds;
};
@@ -147,7 +147,7 @@ struct BVHCollisionPair
BVHCollisionPair(int id1_, int id2_) : id1(id1_), id2(id2_) {}
- BVHCollisionPair(int id1_, int id2_, const Vec3f& contactp, const Vec3f& n, BVH_REAL depth) : id1(id1_),
+ BVHCollisionPair(int id1_, int id2_, const Vec3f& contactp, const Vec3f& n, FCL_REAL depth) : id1(id1_),
id2(id2_), contact_point(contactp), normal(n), penetration_depth(depth) {}
/** \brief The index of one in-collision primitive */
@@ -163,7 +163,7 @@ struct BVHCollisionPair
Vec3f normal;
/** \brief Penetration depth for two triangles */
- BVH_REAL penetration_depth;
+ FCL_REAL penetration_depth;
};
/** \brief Sorting rule between two BVHCollisionPair, for testing */
@@ -215,7 +215,7 @@ public:
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
- BVH_REAL penetration;
+ FCL_REAL penetration;
Vec3f normal;
int n_contacts;
Vec3f contacts[2];
@@ -333,7 +333,7 @@ struct BVHPointCollisionPair
{
BVHPointCollisionPair() {}
- BVHPointCollisionPair(int id1_start_, int id1_num_, int id2_start_, int id2_num_, BVH_REAL collision_prob_)
+ BVHPointCollisionPair(int id1_start_, int id1_num_, int id2_start_, int id2_num_, FCL_REAL collision_prob_)
: id1_start(id1_start_), id1_num(id1_num_), id2_start(id2_start_), id2_num(id2_num_), collision_prob(collision_prob_) {}
int id1_start;
@@ -342,7 +342,7 @@ struct BVHPointCollisionPair
int id2_start;
int id2_num;
- BVH_REAL collision_prob;
+ FCL_REAL collision_prob;
};
struct BVHPointCollisionPairComp
@@ -393,7 +393,7 @@ public:
const BVNode<BV>& node1 = this->model1->getBV(b1);
const BVNode<BV>& node2 = this->model2->getBV(b2);
- BVH_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
+ FCL_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
node1.num_primitives,
vertices2 + node2.first_primitive, uc2.get() + node2.first_primitive,
node2.num_primitives,
@@ -426,9 +426,9 @@ public:
int leaf_size_threshold;
- BVH_REAL collision_prob_threshold;
+ FCL_REAL collision_prob_threshold;
- mutable BVH_REAL max_collision_prob;
+ mutable FCL_REAL max_collision_prob;
CloudClassifierParam classifier_param;
};
@@ -502,7 +502,7 @@ public:
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
- BVH_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
+ FCL_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
node1.num_primitives,
q1, q2, q3);
@@ -532,9 +532,9 @@ public:
int leaf_size_threshold;
- BVH_REAL collision_prob_threshold;
+ FCL_REAL collision_prob_threshold;
- mutable BVH_REAL max_collision_prob;
+ mutable FCL_REAL max_collision_prob;
};
@@ -570,7 +570,7 @@ struct BVHContinuousCollisionPair
{
BVHContinuousCollisionPair() {}
- BVHContinuousCollisionPair(int id1_, int id2_, BVH_REAL time) : id1(id1_), id2(id2_), collision_time(time) {}
+ BVHContinuousCollisionPair(int id1_, int id2_, FCL_REAL time) : id1(id1_), id2(id2_), collision_time(time) {}
/** \brief The index of one in-collision primitive */
int id1;
@@ -579,7 +579,7 @@ struct BVHContinuousCollisionPair
int id2;
/** \brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free */
- BVH_REAL collision_time;
+ FCL_REAL collision_time;
};
@@ -611,7 +611,7 @@ public:
const BVNode<BV>& node1 = this->model1->getBV(b1);
const BVNode<BV>& node2 = this->model2->getBV(b2);
- BVH_REAL collision_time = 2;
+ FCL_REAL collision_time = 2;
Vec3f collision_pos;
int primitive_id1 = node1.primitiveId();
@@ -634,7 +634,7 @@ public:
T1[i] = vertices2 + tri_id2[i];
}
- BVH_REAL tmp;
+ FCL_REAL tmp;
Vec3f tmpv;
// 6 VF checks
@@ -737,7 +737,7 @@ public:
const BVNode<BV>& node1 = this->model1->getBV(b1);
const BVNode<BV>& node2 = this->model2->getBV(b2);
- BVH_REAL collision_time = 2;
+ FCL_REAL collision_time = 2;
Vec3f collision_pos;
int primitive_id1 = node1.primitiveId();
@@ -757,7 +757,7 @@ public:
Vec3f* T0 = prev_vertices2 + vertex_id2;
Vec3f* T1 = vertices2 + vertex_id2;
- BVH_REAL tmp;
+ FCL_REAL tmp;
Vec3f tmpv;
// 3 VF checks
@@ -828,7 +828,7 @@ public:
const BVNode<BV>& node1 = this->model1->getBV(b1);
const BVNode<BV>& node2 = this->model2->getBV(b2);
- BVH_REAL collision_time = 2;
+ FCL_REAL collision_time = 2;
Vec3f collision_pos;
int primitive_id1 = node1.primitiveId();
@@ -848,7 +848,7 @@ public:
T1[i] = vertices2 + tri_id2[i];
}
- BVH_REAL tmp;
+ FCL_REAL tmp;
Vec3f tmpv;
// 3 VF checks
@@ -921,8 +921,8 @@ public:
bool firstOverSecond(int b1, int b2) const
{
- BVH_REAL sz1 = model1->getBV(b1).bv.size();
- BVH_REAL sz2 = model2->getBV(b2).bv.size();
+ FCL_REAL sz1 = model1->getBV(b1).bv.size();
+ FCL_REAL sz2 = model2->getBV(b2).bv.size();
bool l1 = model1->getBV(b1).isLeaf();
bool l2 = model2->getBV(b2).isLeaf();
@@ -952,7 +952,7 @@ public:
return model2->getBV(b).rightChild();
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return model1->getBV(b1).distance(model2->getBV(b2));
@@ -963,7 +963,7 @@ public:
mutable int num_bv_tests;
mutable int num_leaf_tests;
- mutable BVH_REAL query_time_seconds;
+ mutable FCL_REAL query_time_seconds;
};
@@ -986,7 +986,7 @@ public:
rel_err = 0;
abs_err = 0;
- min_distance = std::numeric_limits<BVH_REAL>::max();
+ min_distance = std::numeric_limits<FCL_REAL>::max();
}
void leafTesting(int b1, int b2) const
@@ -1013,7 +1013,7 @@ public:
// nearest point pair
Vec3f P1, P2;
- BVH_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
+ FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
P1, P2);
if(d < min_distance)
@@ -1028,7 +1028,7 @@ public:
}
}
- bool canStop(BVH_REAL c) const
+ bool canStop(FCL_REAL c) const
{
if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
return true;
@@ -1042,13 +1042,13 @@ public:
Triangle* tri_indices2;
/** \brief relative and absolute error, default value is 0.01 for both terms */
- BVH_REAL rel_err;
- BVH_REAL abs_err;
+ FCL_REAL rel_err;
+ FCL_REAL abs_err;
/** \brief distance and points establishing the minimum distance for the models, within the relative and absolute error bounds specified.
* p1 is in model1's local coordinate system while p2 is in model2's local coordinate system
*/
- mutable BVH_REAL min_distance;
+ mutable FCL_REAL min_distance;
mutable Vec3f p1;
mutable Vec3f p2;
@@ -1067,7 +1067,7 @@ public:
void postprocess();
- BVH_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
@@ -1085,7 +1085,7 @@ public:
void postprocess();
- BVH_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
@@ -1102,7 +1102,7 @@ public:
void postprocess();
- BVH_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
@@ -1113,13 +1113,13 @@ public:
struct ConservativeAdvancementStackData
{
- ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, BVH_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
+ ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
Vec3f P1;
Vec3f P2;
int c1;
int c2;
- BVH_REAL d;
+ FCL_REAL d;
};
// when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
@@ -1127,11 +1127,11 @@ template<typename BV>
class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
{
public:
- MeshConservativeAdvancementTraversalNode(BVH_REAL w_ = 1) : MeshDistanceTraversalNode<BV>()
+ MeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshDistanceTraversalNode<BV>()
{
delta_t = 1;
toc = 0;
- t_err = (BVH_REAL)0.00001;
+ t_err = (FCL_REAL)0.00001;
w = w_;
@@ -1139,11 +1139,11 @@ public:
motion2 = NULL;
}
- BVH_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
Vec3f P1, P2;
- BVH_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
+ FCL_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
@@ -1174,7 +1174,7 @@ public:
// nearest point pair
Vec3f P1, P2;
- BVH_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3,
+ FCL_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3,
P1, P2);
if(d < this->min_distance)
@@ -1192,12 +1192,12 @@ public:
// n is the local frame of object 1
Vec3f n = P2 - P1;
// here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
- BVH_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
- BVH_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
+ FCL_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
+ FCL_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
- BVH_REAL bound = bound1 + bound2;
+ FCL_REAL bound = bound1 + bound2;
- BVH_REAL cur_delta_t;
+ FCL_REAL cur_delta_t;
if(bound <= d) cur_delta_t = 1;
else cur_delta_t = d / bound;
@@ -1206,12 +1206,12 @@ public:
}
- bool canStop(BVH_REAL c) const
+ bool canStop(FCL_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
Vec3f n;
int c1, c2;
@@ -1233,12 +1233,12 @@ public:
assert(c == d);
- BVH_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
- BVH_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
+ FCL_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
+ FCL_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
- BVH_REAL bound = bound1 + bound2;
+ FCL_REAL bound = bound1 + bound2;
- BVH_REAL cur_delta_t;
+ FCL_REAL cur_delta_t;
if(bound <= c) cur_delta_t = 1;
else cur_delta_t = c / bound;
@@ -1252,7 +1252,7 @@ public:
else
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
if(d > c)
stack[stack.size() - 2] = stack[stack.size() - 1];
@@ -1265,14 +1265,14 @@ public:
/** \brief CA controlling variable: early stop for the early iterations of CA */
- BVH_REAL w;
+ FCL_REAL w;
/** \brief The time from beginning point */
- BVH_REAL toc;
- BVH_REAL t_err;
+ FCL_REAL toc;
+ FCL_REAL t_err;
/** \brief The delta_t each step */
- mutable BVH_REAL delta_t;
+ mutable FCL_REAL delta_t;
/** \brief Motions for the two objects in query */
MotionBase<BV>* motion1;
@@ -1284,22 +1284,22 @@ public:
/** for OBB and RSS, there is local coordinate of BV, so normal need to be transformed */
template<>
-bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const;
+bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const;
template<>
-bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const;
+bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const;
class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode<RSS>
{
public:
- MeshConservativeAdvancementTraversalNodeRSS(BVH_REAL w_ = 1);
+ MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1);
- BVH_REAL BVTesting(int b1, int b2) const;
+ FCL_REAL BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
- bool canStop(BVH_REAL c) const;
+ bool canStop(FCL_REAL c) const;
Matrix3f R;
Vec3f T;
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 8063fea9..e413ecc9 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -80,7 +80,7 @@ public:
mutable Vec3f contact_point;
- mutable BVH_REAL penetration_depth;
+ mutable FCL_REAL penetration_depth;
bool enable_contact;
@@ -101,7 +101,7 @@ public:
nsolver = NULL;
}
- BVH_REAL BVTesting(int, int) const
+ FCL_REAL BVTesting(int, int) const
{
return -1; // should not be used
}
@@ -114,7 +114,7 @@ public:
const S1* model1;
const S2* model2;
- mutable BVH_REAL min_distance;
+ mutable FCL_REAL min_distance;
mutable Vec3f p1;
mutable Vec3f p2;
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 002f6104..d4d202f0 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -108,6 +108,18 @@ public:
inline void setValue(U x) { data.setValue(x); }
inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
inline void negate() { data.negate(); }
+
+ inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
+ {
+ data.ubound(u.data);
+ return *this;
+ }
+
+ inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
+ {
+ data.lbound(l.data);
+ return *this;
+ }
};
template <typename T>
@@ -169,7 +181,7 @@ void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
}
-typedef Vec3fX<details::Vec3Data<BVH_REAL> > Vec3f;
+typedef Vec3fX<details::Vec3Data<FCL_REAL> > Vec3f;
}
diff --git a/trunk/fcl/src/BV/AABB.cpp b/trunk/fcl/src/BV/AABB.cpp
index b645d52f..ed3e0e92 100644
--- a/trunk/fcl/src/BV/AABB.cpp
+++ b/trunk/fcl/src/BV/AABB.cpp
@@ -44,24 +44,24 @@ namespace fcl
AABB::AABB()
{
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
min_.setValue(real_max, real_max, real_max);
max_.setValue(-real_max, -real_max, -real_max);
}
-BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
+FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
{
- BVH_REAL result = 0;
+ FCL_REAL result = 0;
for(size_t i = 0; i < 3; ++i)
{
- const BVH_REAL& amin = min_[i];
- const BVH_REAL& amax = max_[i];
- const BVH_REAL& bmin = other.min_[i];
- const BVH_REAL& bmax = other.max_[i];
+ const FCL_REAL& amin = min_[i];
+ const FCL_REAL& amax = max_[i];
+ const FCL_REAL& bmin = other.min_[i];
+ const FCL_REAL& bmax = other.max_[i];
if(amin > bmax)
{
- BVH_REAL delta = bmax - amin;
+ FCL_REAL delta = bmax - amin;
result += delta * delta;
if(P && Q)
{
@@ -71,7 +71,7 @@ BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
}
else if(bmin > amax)
{
- BVH_REAL delta = amax - bmin;
+ FCL_REAL delta = amax - bmin;
result += delta * delta;
if(P && Q)
{
@@ -85,13 +85,13 @@ BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
{
if(bmin >= amin)
{
- BVH_REAL t = 0.5 * (amax + bmin);
+ FCL_REAL t = 0.5 * (amax + bmin);
(*P)[i] = t;
(*Q)[i] = t;
}
else
{
- BVH_REAL t = 0.5 * (amin + bmax);
+ FCL_REAL t = 0.5 * (amin + bmax);
(*P)[i] = t;
(*Q)[i] = t;
}
@@ -102,24 +102,24 @@ BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
return sqrt(result);
}
-BVH_REAL AABB::distance(const AABB& other) const
+FCL_REAL AABB::distance(const AABB& other) const
{
- BVH_REAL result = 0;
+ FCL_REAL result = 0;
for(size_t i = 0; i < 3; ++i)
{
- const BVH_REAL& amin = min_[i];
- const BVH_REAL& amax = max_[i];
- const BVH_REAL& bmin = other.min_[i];
- const BVH_REAL& bmax = other.max_[i];
+ const FCL_REAL& amin = min_[i];
+ const FCL_REAL& amax = max_[i];
+ const FCL_REAL& bmin = other.min_[i];
+ const FCL_REAL& bmax = other.max_[i];
if(amin > bmax)
{
- BVH_REAL delta = bmax - amin;
+ FCL_REAL delta = bmax - amin;
result += delta * delta;
}
else if(bmin > amax)
{
- BVH_REAL delta = amax - bmin;
+ FCL_REAL delta = amax - bmin;
result += delta * delta;
}
}
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index c2c619ba..ddd9a032 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -63,7 +63,7 @@ bool OBB::overlap(const OBB& other) const
bool OBB::contain(const Vec3f& p) const
{
Vec3f local_p = p - To;
- BVH_REAL proj = local_p.dot(axis[0]);
+ FCL_REAL proj = local_p.dot(axis[0]);
if((proj > extent[0]) || (proj < -extent[0]))
return false;
@@ -95,8 +95,8 @@ OBB& OBB::operator += (const Vec3f& p)
OBB OBB::operator + (const OBB& other) const
{
Vec3f center_diff = To - other.To;
- BVH_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
- BVH_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
+ FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
+ FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
if(center_diff.length() > 2 * (max_extent + max_extent2))
{
return merge_largedist(*this, other);
@@ -110,8 +110,8 @@ OBB OBB::operator + (const OBB& other) const
bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b)
{
- register BVH_REAL t, s;
- const BVH_REAL reps = 1e-6;
+ register FCL_REAL t, s;
+ const FCL_REAL reps = 1e-6;
Matrix3f Bf = abs(B);
Bf += reps;
@@ -259,7 +259,7 @@ OBB OBB::merge_largedist(const OBB& b1, const OBB& b2)
b2.computeVertices(vertex + 8);
Matrix3f M;
Vec3f E[3];
- BVH_REAL s[3] = {0, 0, 0};
+ FCL_REAL s[3] = {0, 0, 0};
// obb axes
Vec3f& R0 = b.axis[0];
@@ -308,13 +308,13 @@ OBB OBB::merge_smalldist(const OBB& b1, const OBB& b2)
q1 = -q1;
SimpleQuaternion q = q0 + q1;
- BVH_REAL inv_length = 1.0 / sqrt(q.dot(q));
+ FCL_REAL inv_length = 1.0 / sqrt(q.dot(q));
q = q * inv_length;
q.toAxes(b.axis);
Vec3f vertex[8], diff;
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
Vec3f pmin(real_max, real_max, real_max);
Vec3f pmax(-real_max, -real_max, -real_max);
@@ -324,7 +324,7 @@ OBB OBB::merge_smalldist(const OBB& b1, const OBB& b2)
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
- BVH_REAL dot = diff.dot(b.axis[j]);
+ FCL_REAL dot = diff.dot(b.axis[j]);
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
@@ -338,7 +338,7 @@ OBB OBB::merge_smalldist(const OBB& b1, const OBB& b2)
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
- BVH_REAL dot = diff.dot(b.axis[j]);
+ FCL_REAL dot = diff.dot(b.axis[j]);
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
@@ -356,7 +356,7 @@ OBB OBB::merge_smalldist(const OBB& b1, const OBB& b2)
}
-BVH_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
+FCL_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
{
std::cerr << "OBB distance not implemented!" << std::endl;
return 0.0;
diff --git a/trunk/fcl/src/BV/OBBRSS.cpp b/trunk/fcl/src/BV/OBBRSS.cpp
index c821df05..5df9821b 100644
--- a/trunk/fcl/src/BV/OBBRSS.cpp
+++ b/trunk/fcl/src/BV/OBBRSS.cpp
@@ -44,7 +44,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS
return overlap(R0, T0, b1.obb, b2.obb);
}
-BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P, Vec3f* Q)
+FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P, Vec3f* Q)
{
return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
diff --git a/trunk/fcl/src/BV/RSS.cpp b/trunk/fcl/src/BV/RSS.cpp
index 6c6253f6..31ff3cb0 100644
--- a/trunk/fcl/src/BV/RSS.cpp
+++ b/trunk/fcl/src/BV/RSS.cpp
@@ -51,7 +51,7 @@ bool RSS::overlap(const RSS& other) const
axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
- BVH_REAL dist = rectDistance(R, T, l, other.l);
+ FCL_REAL dist = rectDistance(R, T, l, other.l);
if(dist <= (r + other.r)) return true;
return false;
}
@@ -69,7 +69,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
- BVH_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l);
+ FCL_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l);
if(dist <= (b1.r + b2.r)) return true;
return false;
}
@@ -77,10 +77,10 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
bool RSS::contain(const Vec3f& p) const
{
Vec3f local_p = p - Tr;
- BVH_REAL proj0 = local_p.dot(axis[0]);
- BVH_REAL proj1 = local_p.dot(axis[1]);
- BVH_REAL proj2 = local_p.dot(axis[2]);
- BVH_REAL abs_proj2 = fabs(proj2);
+ FCL_REAL proj0 = local_p.dot(axis[0]);
+ FCL_REAL proj1 = local_p.dot(axis[1]);
+ FCL_REAL proj2 = local_p.dot(axis[2]);
+ FCL_REAL abs_proj2 = fabs(proj2);
Vec3f proj(proj0, proj1, proj2);
// projection is within the rectangle
@@ -93,7 +93,7 @@ bool RSS::contain(const Vec3f& p) const
}
else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
{
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(proj0, y, 0);
if((proj - v).sqrLength() < r * r)
return true;
@@ -102,7 +102,7 @@ bool RSS::contain(const Vec3f& p) const
}
else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
{
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
Vec3f v(x, proj1, 0);
if((proj - v).sqrLength() < r * r)
return true;
@@ -111,8 +111,8 @@ bool RSS::contain(const Vec3f& p) const
}
else
{
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(x, y, 0);
if((proj - v).sqrLength() < r * r)
return true;
@@ -124,10 +124,10 @@ bool RSS::contain(const Vec3f& p) const
RSS& RSS::operator += (const Vec3f& p)
{
Vec3f local_p = p - Tr;
- BVH_REAL proj0 = local_p.dot(axis[0]);
- BVH_REAL proj1 = local_p.dot(axis[1]);
- BVH_REAL proj2 = local_p.dot(axis[2]);
- BVH_REAL abs_proj2 = fabs(proj2);
+ FCL_REAL proj0 = local_p.dot(axis[0]);
+ FCL_REAL proj1 = local_p.dot(axis[1]);
+ FCL_REAL proj2 = local_p.dot(axis[2]);
+ FCL_REAL abs_proj2 = fabs(proj2);
Vec3f proj(proj0, proj1, proj2);
// projection is within the rectangle
@@ -147,23 +147,23 @@ RSS& RSS::operator += (const Vec3f& p)
}
else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
{
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(proj0, y, 0);
- BVH_REAL new_r_sqr = (proj - v).sqrLength();
+ FCL_REAL new_r_sqr = (proj - v).sqrLength();
if(new_r_sqr < r * r)
; // do nothing
else
{
if(abs_proj2 < r)
{
- BVH_REAL delta_y = - sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
+ FCL_REAL delta_y = - sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
l[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
}
else
{
- BVH_REAL delta_y = fabs(proj1 - y);
+ FCL_REAL delta_y = fabs(proj1 - y);
l[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
@@ -177,23 +177,23 @@ RSS& RSS::operator += (const Vec3f& p)
}
else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
{
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
Vec3f v(x, proj1, 0);
- BVH_REAL new_r_sqr = (proj - v).sqrLength();
+ FCL_REAL new_r_sqr = (proj - v).sqrLength();
if(new_r_sqr < r * r)
; // do nothing
else
{
if(abs_proj2 < r)
{
- BVH_REAL delta_x = - sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
+ FCL_REAL delta_x = - sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
l[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
}
else
{
- BVH_REAL delta_x = fabs(proj0 - x);
+ FCL_REAL delta_x = fabs(proj0 - x);
l[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
@@ -207,21 +207,21 @@ RSS& RSS::operator += (const Vec3f& p)
}
else
{
- BVH_REAL x = (proj0 > 0) ? l[0] : 0;
- BVH_REAL y = (proj1 > 0) ? l[1] : 0;
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
Vec3f v(x, y, 0);
- BVH_REAL new_r_sqr = (proj - v).sqrLength();
+ FCL_REAL new_r_sqr = (proj - v).sqrLength();
if(new_r_sqr < r * r)
; // do nothing
else
{
if(abs_proj2 < r)
{
- BVH_REAL diag = sqrt(new_r_sqr - proj2 * proj2);
- BVH_REAL delta_diag = - sqrt(r * r - proj2 * proj2) + diag;
+ FCL_REAL diag = sqrt(new_r_sqr - proj2 * proj2);
+ FCL_REAL delta_diag = - sqrt(r * r - proj2 * proj2) + diag;
- BVH_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
- BVH_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
+ 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;
@@ -233,8 +233,8 @@ RSS& RSS::operator += (const Vec3f& p)
}
else
{
- BVH_REAL delta_x = fabs(proj0 - x);
- BVH_REAL delta_y = fabs(proj1 - y);
+ FCL_REAL delta_x = fabs(proj0 - x);
+ FCL_REAL delta_y = fabs(proj1 - y);
l[0] += delta_x;
l[1] += delta_y;
@@ -296,7 +296,7 @@ RSS RSS::operator + (const RSS& other) const
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3] = {0, 0, 0};
+ FCL_REAL s[3] = {0, 0, 0};
getCovariance(v, NULL, NULL, NULL, 16, M);
matEigen(M, s, E);
@@ -321,7 +321,7 @@ RSS RSS::operator + (const RSS& other) const
return bv;
}
-BVH_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
+FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
{
// compute what transform [R,T] that takes us from cs1 to cs2.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
@@ -332,12 +332,12 @@ BVH_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
- BVH_REAL dist = rectDistance(R, T, l, other.l, P, Q);
+ FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
dist -= (r + other.r);
- return (dist < (BVH_REAL)0.0) ? (BVH_REAL)0.0 : dist;
+ return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
-BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
+FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
{
Matrix3f R0b2(R0[0].dot(b2.axis[0]), R0[0].dot(b2.axis[1]), R0[0].dot(b2.axis[2]),
R0[1].dot(b2.axis[0]), R0[1].dot(b2.axis[1]), R0[1].dot(b2.axis[2]),
@@ -351,23 +351,23 @@ BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
- BVH_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l, P, Q);
+ FCL_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l, P, Q);
dist -= (b1.r + b2.r);
- return (dist < (BVH_REAL)0.0) ? (BVH_REAL)0.0 : dist;
+ return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
-BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL a[2], const BVH_REAL b[2], Vec3f* P, Vec3f* Q)
+FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P, Vec3f* Q)
{
- BVH_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
+ FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
A0_dot_B0 = Rab[0][0];
A0_dot_B1 = Rab[0][1];
A1_dot_B0 = Rab[1][0];
A1_dot_B1 = Rab[1][1];
- BVH_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
- BVH_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
+ FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
+ FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
aA0_dot_B0 = a[0] * A0_dot_B0;
aA0_dot_B1 = a[0] * A0_dot_B1;
@@ -381,13 +381,13 @@ BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL
Vec3f Tba = Rab.transposeTimes(Tab);
Vec3f S;
- BVH_REAL t, u;
+ FCL_REAL t, u;
// determine if any edge pair contains the closest points
- BVH_REAL ALL_x, ALU_x, AUL_x, AUU_x;
- BVH_REAL BLL_x, BLU_x, BUL_x, BUU_x;
- BVH_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
+ FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x;
+ FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x;
+ FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
ALL_x = -Tba[0];
ALU_x = ALL_x + aA1_dot_B0;
@@ -544,14 +544,14 @@ BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL
}
}
- BVH_REAL ALL_y, ALU_y, AUL_y, AUU_y;
+ FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y;
ALL_y = -Tba[1];
ALU_y = ALL_y + aA1_dot_B1;
AUL_y = ALL_y + aA0_dot_B1;
AUU_y = ALU_y + aA0_dot_B1;
- BVH_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
+ FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
if(ALL_y < ALU_y)
{
@@ -698,14 +698,14 @@ BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL
}
}
- BVH_REAL BLL_y, BLU_y, BUL_y, BUU_y;
+ FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y;
BLL_y = Tab[1];
BLU_y = BLL_y + bA1_dot_B1;
BUL_y = BLL_y + bA1_dot_B0;
BUU_y = BLU_y + bA1_dot_B0;
- BVH_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
+ FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
if(ALL_x < AUL_x)
{
@@ -850,7 +850,7 @@ BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL
}
}
- BVH_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
+ FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
if(ALL_y < AUL_y)
{
@@ -998,7 +998,7 @@ BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL
// no edges passed, take max separation along face normals
- BVH_REAL sep1, sep2;
+ FCL_REAL sep1, sep2;
if(Tab[2] > 0.0)
{
@@ -1066,21 +1066,21 @@ BVH_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const BVH_REAL
}
}
- BVH_REAL sep = (sep1 > sep2 ? sep1 : sep2);
+ FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2);
return (sep > 0 ? sep : 0);
}
-void RSS::clipToRange(BVH_REAL& val, BVH_REAL a, BVH_REAL b)
+void RSS::clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b)
{
if(val < a) val = a;
else if(val > b) val = b;
}
-void RSS::segCoords(BVH_REAL& t, BVH_REAL& u, BVH_REAL a, BVH_REAL b, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T)
+void RSS::segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
{
- BVH_REAL denom = 1 - A_dot_B * A_dot_B;
+ FCL_REAL denom = 1 - A_dot_B * A_dot_B;
if(denom == 0) t = 0;
else
@@ -1104,11 +1104,11 @@ void RSS::segCoords(BVH_REAL& t, BVH_REAL& u, BVH_REAL a, BVH_REAL b, BVH_REAL A
}
}
-bool RSS::inVoronoi(BVH_REAL a, BVH_REAL b, BVH_REAL Anorm_dot_B, BVH_REAL Anorm_dot_T, BVH_REAL A_dot_B, BVH_REAL A_dot_T, BVH_REAL B_dot_T)
+bool RSS::inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
{
if(fabs(Anorm_dot_B) < 1e-7) return false;
- BVH_REAL t, u, v;
+ FCL_REAL t, u, v;
u = -Anorm_dot_T / Anorm_dot_B;
clipToRange(u, 0, b);
diff --git a/trunk/fcl/src/BV/kIOS.cpp b/trunk/fcl/src/BV/kIOS.cpp
index 29d3677b..6e808cf0 100644
--- a/trunk/fcl/src/BV/kIOS.cpp
+++ b/trunk/fcl/src/BV/kIOS.cpp
@@ -50,8 +50,8 @@ bool kIOS::overlap(const kIOS& other) const
{
for(unsigned int j = 0; j < other.num_spheres; ++j)
{
- BVH_REAL o_dist = (spheres[i].o - other.spheres[j].o).sqrLength();
- BVH_REAL sum_r = spheres[i].r + other.spheres[j].r;
+ FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).sqrLength();
+ FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
if(o_dist > sum_r * sum_r)
return false;
}
@@ -67,7 +67,7 @@ bool kIOS::contain(const Vec3f& p) const
{
for(unsigned int i = 0; i < num_spheres; ++i)
{
- BVH_REAL r = spheres[i].r;
+ FCL_REAL r = spheres[i].r;
if((spheres[i].o - p).sqrLength() > r * r)
return false;
}
@@ -79,8 +79,8 @@ kIOS& kIOS::operator += (const Vec3f& p)
{
for(unsigned int i = 0; i < num_spheres; ++i)
{
- BVH_REAL r = spheres[i].r;
- BVH_REAL new_r_sqr = (p - spheres[i].o).sqrLength();
+ FCL_REAL r = spheres[i].r;
+ FCL_REAL new_r_sqr = (p - spheres[i].o).sqrLength();
if(new_r_sqr > r * r)
{
spheres[i].r = sqrt(new_r_sqr);
@@ -107,40 +107,40 @@ kIOS kIOS::operator + (const kIOS& other) const
return result;
}
-BVH_REAL kIOS::width() const
+FCL_REAL kIOS::width() const
{
return obb_bv.width();
}
-BVH_REAL kIOS::height() const
+FCL_REAL kIOS::height() const
{
return obb_bv.height();
}
-BVH_REAL kIOS::depth() const
+FCL_REAL kIOS::depth() const
{
return obb_bv.depth();
}
-BVH_REAL kIOS::volume() const
+FCL_REAL kIOS::volume() const
{
return obb_bv.volume();
}
-BVH_REAL kIOS::size() const
+FCL_REAL kIOS::size() const
{
return volume();
}
-BVH_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const
+FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const
{
- BVH_REAL d_max = 0;
+ FCL_REAL d_max = 0;
unsigned int id_a = -1, id_b = -1;
for(unsigned int i = 0; i < num_spheres; ++i)
{
for(unsigned int j = 0; j < other.num_spheres; ++j)
{
- BVH_REAL d = (spheres[i].o - other.spheres[j].o).length() - (spheres[i].r + other.spheres[j].r);
+ FCL_REAL d = (spheres[i].o - other.spheres[j].o).length() - (spheres[i].r + other.spheres[j].r);
if(d_max < d)
{
d_max = d;
@@ -157,7 +157,7 @@ BVH_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const
if(id_a != -1 && id_b != -1)
{
Vec3f v = spheres[id_a].o - spheres[id_b].o;
- BVH_REAL len_v = v.length();
+ FCL_REAL len_v = v.length();
*P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
*Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
}
@@ -185,7 +185,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
return b1.overlap(b2_temp);
}
-BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
+FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
{
kIOS b2_temp = b2;
for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index a323783d..060995e6 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -707,9 +707,9 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_pri
const Vec3f& p1 = vertices[t[0]];
const Vec3f& p2 = vertices[t[1]];
const Vec3f& p3 = vertices[t[2]];
- BVH_REAL x = (p1[0] + p2[0] + p3[0]) / 3.0;
- BVH_REAL y = (p1[1] + p2[1] + p3[1]) / 3.0;
- BVH_REAL z = (p1[2] + p2[2] + p3[2]) / 3.0;
+ FCL_REAL x = (p1[0] + p2[0] + p3[0]) / 3.0;
+ FCL_REAL y = (p1[1] + p2[1] + p3[1]) / 3.0;
+ FCL_REAL z = (p1[2] + p2[2] + p3[2]) / 3.0;
p.setValue(x, y, z);
}
else
@@ -866,7 +866,7 @@ void BVHModel<BV>::computeLocalAABB()
aabb_radius = 0;
for(int i = 0; i < num_vertices; ++i)
{
- BVH_REAL r = (aabb_center - vertices[i]).sqrLength();
+ FCL_REAL r = (aabb_center - vertices[i]).sqrLength();
if(r > aabb_radius) aabb_radius = r;
}
diff --git a/trunk/fcl/src/BVH_utility.cpp b/trunk/fcl/src/BVH_utility.cpp
index acfd7c7f..d7b3cf19 100644
--- a/trunk/fcl/src/BVH_utility.cpp
+++ b/trunk/fcl/src/BVH_utility.cpp
@@ -43,7 +43,7 @@
namespace fcl
{
-void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, BVH_REAL r = 1.0)
+void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
{
for(int i = 0; i < model.getNumBVs(); ++i)
{
@@ -74,7 +74,7 @@ void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, BVH_REAL r = 1.0)
}
}
-void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, BVH_REAL r = 1.0)
+void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
{
for(int i = 0; i < model.getNumBVs(); ++i)
{
@@ -367,14 +367,14 @@ void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, i
/** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin.
* The bounding volume axes are known.
*/
-void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, BVH_REAL l[2], BVH_REAL& r)
+void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
- BVH_REAL (*P)[3] = new BVH_REAL[size_P][3];
+ FCL_REAL (*P)[3] = new FCL_REAL[size_P][3];
int P_id = 0;
@@ -435,22 +435,22 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
}
}
- BVH_REAL minx, maxx, miny, maxy, minz, maxz;
+ FCL_REAL minx, maxx, miny, maxy, minz, maxz;
- BVH_REAL cz, radsqr;
+ FCL_REAL cz, radsqr;
minz = maxz = P[0][2];
for(int i = 1; i < size_P; ++i)
{
- BVH_REAL z_value = P[i][2];
+ FCL_REAL z_value = P[i][2];
if(z_value < minz) minz = z_value;
else if(z_value > maxz) maxz = z_value;
}
- r = (BVH_REAL)0.5 * (maxz - minz);
+ r = (FCL_REAL)0.5 * (maxz - minz);
radsqr = r * r;
- cz = (BVH_REAL)0.5 * (maxz + minz);
+ cz = (FCL_REAL)0.5 * (maxz + minz);
// compute an initial length of rectangle along x direction
@@ -458,12 +458,12 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
int minindex, maxindex;
minindex = maxindex = 0;
- BVH_REAL mintmp, maxtmp;
+ FCL_REAL mintmp, maxtmp;
mintmp = maxtmp = P[0][0];
for(int i = 1; i < size_P; ++i)
{
- BVH_REAL x_value = P[i][0];
+ FCL_REAL x_value = P[i][0];
if(x_value < mintmp)
{
minindex = i;
@@ -476,7 +476,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
}
}
- BVH_REAL x, dz;
+ FCL_REAL x, dz;
dz = P[minindex][2] - cz;
minx = P[minindex][0] + sqrt(std::max(radsqr - dz * dz, 0.0));
dz = P[maxindex][2] - cz;
@@ -515,7 +515,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
mintmp = maxtmp = P[0][1];
for(int i = 1; i < size_P; ++i)
{
- BVH_REAL y_value = P[i][1];
+ FCL_REAL y_value = P[i][1];
if(y_value < mintmp)
{
minindex = i;
@@ -528,7 +528,7 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
}
}
- BVH_REAL y;
+ FCL_REAL y;
dz = P[minindex][2] - cz;
miny = P[minindex][1] + sqrt(std::max(radsqr - dz * dz, 0.0));
dz = P[maxindex][2] - cz;
@@ -560,8 +560,8 @@ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, uns
// corners may have some points which are not covered - grow lengths if necessary
// quite conservative (can be improved)
- BVH_REAL dx, dy, u, t;
- BVH_REAL a = sqrt((BVH_REAL)0.5);
+ FCL_REAL dx, dy, u, t;
+ FCL_REAL a = sqrt((FCL_REAL)0.5);
for(int i = 0; i < size_P; ++i)
{
if(P[i][0] > maxx)
@@ -652,10 +652,10 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
bool indirect_index = true;
if(!indices) indirect_index = false;
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
- BVH_REAL min_coord[3] = {real_max, real_max, real_max};
- BVH_REAL max_coord[3] = {-real_max, -real_max, -real_max};
+ FCL_REAL min_coord[3] = {real_max, real_max, real_max};
+ FCL_REAL max_coord[3] = {-real_max, -real_max, -real_max};
for(int i = 0; i < n; ++i)
{
@@ -663,7 +663,7 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
const Vec3f& p = ps[index];
Vec3f v(p[0], p[1], p[2]);
- BVH_REAL proj[3];
+ FCL_REAL proj[3];
proj[0] = axis[0].dot(v);
proj[1] = axis[1].dot(v);
proj[2] = axis[2].dot(v);
@@ -710,10 +710,10 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
bool indirect_index = true;
if(!indices) indirect_index = false;
- BVH_REAL real_max = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
- BVH_REAL min_coord[3] = {real_max, real_max, real_max};
- BVH_REAL max_coord[3] = {-real_max, -real_max, -real_max};
+ FCL_REAL min_coord[3] = {real_max, real_max, real_max};
+ FCL_REAL max_coord[3] = {-real_max, -real_max, -real_max};
for(int i = 0; i < n; ++i)
{
@@ -725,7 +725,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps[point_id];
Vec3f v(p[0], p[1], p[2]);
- BVH_REAL proj[3];
+ FCL_REAL proj[3];
proj[0] = axis[0].dot(v);
proj[1] = axis[1].dot(v);
proj[2] = axis[2].dot(v);
@@ -744,7 +744,7 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps2[point_id];
Vec3f v(p[0], p[1], p[2]);
- BVH_REAL proj[3];
+ FCL_REAL proj[3];
proj[0] = axis[0].dot(v);
proj[1] = axis[1].dot(v);
proj[2] = axis[2].dot(v);
@@ -778,14 +778,14 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indic
getExtentAndCenter_pointcloud(ps, ps2, indices, n, axis, center, extent);
}
-void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius)
+void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius)
{
Vec3f e1 = a - c;
Vec3f e2 = b - c;
- BVH_REAL e1_len2 = e1.sqrLength();
- BVH_REAL e2_len2 = e2.sqrLength();
+ FCL_REAL e1_len2 = e1.sqrLength();
+ FCL_REAL e2_len2 = e2.sqrLength();
Vec3f e3 = e1.cross(e2);
- BVH_REAL e3_len2 = e3.sqrLength();
+ FCL_REAL e3_len2 = e3.sqrLength();
radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2;
radius = sqrt(radius) * 0.5;
@@ -793,12 +793,12 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec
}
-static inline BVH_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
+static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
- BVH_REAL maxD = 0;
+ FCL_REAL maxD = 0;
for(int i = 0; i < n; ++i)
{
unsigned int index = indirect_index ? indices[i] : i;
@@ -809,7 +809,7 @@ static inline BVH_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps[point_id];
- BVH_REAL d = (p - query).sqrLength();
+ FCL_REAL d = (p - query).sqrLength();
if(d > maxD) maxD = d;
}
@@ -820,7 +820,7 @@ static inline BVH_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
int point_id = t[j];
const Vec3f& p = ps2[point_id];
- BVH_REAL d = (p - query).sqrLength();
+ FCL_REAL d = (p - query).sqrLength();
if(d > maxD) maxD = d;
}
}
@@ -829,24 +829,24 @@ static inline BVH_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
return sqrt(maxD);
}
-static inline BVH_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query)
+static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
- BVH_REAL maxD = 0;
+ FCL_REAL maxD = 0;
for(unsigned int i = 0; i < n; ++i)
{
int index = indirect_index ? indices[i] : i;
const Vec3f& p = ps[index];
- BVH_REAL d = (p - query).sqrLength();
+ FCL_REAL d = (p - query).sqrLength();
if(d > maxD) maxD = d;
if(ps2)
{
const Vec3f& v = ps2[index];
- BVH_REAL d = (v - query).sqrLength();
+ FCL_REAL d = (v - query).sqrLength();
if(d > maxD) maxD = d;
}
}
@@ -854,7 +854,7 @@ static inline BVH_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigne
return sqrt(maxD);
}
-BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
+FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
{
if(ts)
return maximumDistance_mesh(ps, ps2, ts, indices, n, query);
diff --git a/trunk/fcl/src/BV_fitter.cpp b/trunk/fcl/src/BV_fitter.cpp
index e491ec51..c3e57ee0 100644
--- a/trunk/fcl/src/BV_fitter.cpp
+++ b/trunk/fcl/src/BV_fitter.cpp
@@ -48,7 +48,7 @@ static const double invCosA = 2.0 / sqrt(3.0);
static const double sinA = 0.5;
static const double cosA = sqrt(3.0) / 2.0;
-static inline void axisFromEigen(Vec3f eigenV[3], BVH_REAL eigenS[3], Vec3f axis[3])
+static inline void axisFromEigen(Vec3f eigenV[3], FCL_REAL eigenS[3], Vec3f axis[3])
{
int min, mid, max;
if(eigenS[0] > eigenS[1]) { max = 0; min = 1; }
@@ -81,7 +81,7 @@ void fit2(Vec3f* ps, OBB& bv)
const Vec3f& p1 = ps[0];
const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
- BVH_REAL len_p1p2 = p1p2.length();
+ FCL_REAL len_p1p2 = p1p2.length();
p1p2.normalize();
bv.axis[0] = p1p2;
@@ -102,7 +102,7 @@ void fit3(Vec3f* ps, OBB& bv)
e[0] = p1 - p2;
e[1] = p2 - p3;
e[2] = p3 - p1;
- BVH_REAL len[3];
+ FCL_REAL len[3];
len[0] = e[0].sqrLength();
len[1] = e[1].sqrLength();
len[2] = e[2].sqrLength();
@@ -137,7 +137,7 @@ void fitn(Vec3f* ps, int n, OBB& bv)
{
Matrix3f M;
Vec3f E[3];
- BVH_REAL s[3] = {0, 0, 0}; // three eigen values
+ FCL_REAL s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, NULL, n, M);
matEigen(M, s, E);
@@ -168,7 +168,7 @@ void fit2(Vec3f* ps, RSS& bv)
const Vec3f& p1 = ps[0];
const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
- BVH_REAL len_p1p2 = p1p2.length();
+ FCL_REAL len_p1p2 = p1p2.length();
p1p2.normalize();
bv.axis[0] = p1p2;
@@ -189,7 +189,7 @@ void fit3(Vec3f* ps, RSS& bv)
e[0] = p1 - p2;
e[1] = p2 - p3;
e[2] = p3 - p1;
- BVH_REAL len[3];
+ FCL_REAL len[3];
len[0] = e[0].sqrLength();
len[1] = e[1].sqrLength();
len[2] = e[2].sqrLength();
@@ -223,7 +223,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
{
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3] = {0, 0, 0};
+ FCL_REAL s[3] = {0, 0, 0};
getCovariance(ps, NULL, NULL, NULL, n, M);
matEigen(M, s, E);
@@ -258,22 +258,22 @@ void fit2(Vec3f* ps, kIOS& bv)
const Vec3f& p1 = ps[0];
const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
- BVH_REAL len_p1p2 = p1p2.length();
+ FCL_REAL len_p1p2 = p1p2.length();
p1p2.normalize();
Vec3f* axis = bv.obb_bv.axis;
axis[0] = p1p2;
generateCoordinateSystem(axis[0], axis[1], axis[2]);
- BVH_REAL r0 = len_p1p2 * 0.5;
+ FCL_REAL r0 = len_p1p2 * 0.5;
bv.obb_bv.extent.setValue(r0, 0, 0);
bv.obb_bv.To = (p1 + p2) * 0.5;
bv.spheres[0].o = bv.obb_bv.To;
bv.spheres[0].r = r0;
- BVH_REAL r1 = r0 * invSinA;
- BVH_REAL r1cosA = r1 * cosA;
+ FCL_REAL r1 = r0 * invSinA;
+ FCL_REAL r1cosA = r1 * cosA;
bv.spheres[1].r = r1;
bv.spheres[2].r = r1;
Vec3f delta = axis[1] * r1cosA;
@@ -298,7 +298,7 @@ void fit3(Vec3f* ps, kIOS& bv)
e[0] = p1 - p2;
e[1] = p2 - p3;
e[2] = p3 - p1;
- BVH_REAL len[3];
+ FCL_REAL len[3];
len[0] = e[0].sqrLength();
len[1] = e[1].sqrLength();
len[2] = e[2].sqrLength();
@@ -320,14 +320,14 @@ void fit3(Vec3f* ps, kIOS& bv)
getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb_bv.axis, bv.obb_bv.To, bv.obb_bv.extent);
// compute radius and center
- BVH_REAL r0;
+ FCL_REAL r0;
Vec3f center;
circumCircleComputation(p1, p2, p3, center, r0);
bv.spheres[0].o = center;
bv.spheres[0].r = r0;
- BVH_REAL r1 = r0 * invSinA;
+ FCL_REAL r1 = r0 * invSinA;
Vec3f delta = bv.obb_bv.axis[2] * (r1 * cosA);
bv.spheres[1].r = r1;
@@ -340,7 +340,7 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
{
Matrix3f M;
Vec3f E[3];
- BVH_REAL s[3] = {0, 0, 0}; // three eigen values;
+ FCL_REAL s[3] = {0, 0, 0}; // three eigen values;
getCovariance(ps, NULL, NULL, NULL, n, M);
matEigen(M, s, E);
@@ -353,7 +353,7 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
// get center and extension
const Vec3f& center = bv.obb_bv.To;
const Vec3f& extent = bv.obb_bv.extent;
- BVH_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
+ FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
// decide the k in kIOS
if(extent[0] > kIOS_RATIO * extent[2])
@@ -369,12 +369,12 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
if(bv.num_spheres >= 3)
{
- BVH_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
+ FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
- BVH_REAL r11 = 0, r12 = 0;
+ FCL_REAL r11 = 0, r12 = 0;
r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o);
r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o);
bv.spheres[1].o += axis[2] * (-r10 + r11);
@@ -386,12 +386,12 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
if(bv.num_spheres >= 5)
{
- BVH_REAL r10 = bv.spheres[1].r;
+ FCL_REAL r10 = bv.spheres[1].r;
Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
bv.spheres[3].o = bv.spheres[0].o - delta;
bv.spheres[4].o = bv.spheres[0].o + delta;
- BVH_REAL r21 = 0, r22 = 0;
+ FCL_REAL r21 = 0, r22 = 0;
r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o);
r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o);
@@ -522,7 +522,7 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3]; // three eigen values
+ FCL_REAL s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
matEigen(M, s, E);
@@ -540,7 +540,7 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
OBBRSS bv;
Matrix3f M;
Vec3f E[3];
- BVH_REAL s[3];
+ FCL_REAL s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
matEigen(M, s, E);
@@ -553,8 +553,8 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent);
Vec3f origin;
- BVH_REAL l[2];
- BVH_REAL r;
+ FCL_REAL l[2];
+ FCL_REAL r;
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axis, origin, l, r);
bv.rss.Tr = origin;
@@ -571,7 +571,7 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3]; // three eigen values
+ FCL_REAL s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
matEigen(M, s, E);
axisFromEigen(E, s, bv.axis);
@@ -579,8 +579,8 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
// set rss origin, rectangle size and radius
Vec3f origin;
- BVH_REAL l[2];
- BVH_REAL r;
+ FCL_REAL l[2];
+ FCL_REAL r;
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, origin, l, r);
bv.Tr = origin;
@@ -599,7 +599,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- BVH_REAL s[3];
+ FCL_REAL s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
matEigen(M, s, E);
@@ -612,7 +612,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
const Vec3f& center = bv.obb_bv.To;
const Vec3f& extent = bv.obb_bv.extent;
- BVH_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center);
+ FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center);
// decide k in kIOS
if(extent[0] > kIOS_RATIO * extent[2])
@@ -627,13 +627,13 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
if(bv.num_spheres >= 3)
{
- BVH_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
+ FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
bv.spheres[1].o = center - delta;
bv.spheres[2].o = center + delta;
- BVH_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o);
- BVH_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o);
+ FCL_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o);
+ FCL_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o);
bv.spheres[1].o += axis[2] * (-r10 + r11);
bv.spheres[2].o += axis[2] * (r10 - r12);
@@ -644,12 +644,12 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
if(bv.num_spheres >= 5)
{
- BVH_REAL r10 = bv.spheres[1].r;
+ FCL_REAL r10 = bv.spheres[1].r;
Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
bv.spheres[3].o = bv.spheres[0].o - delta;
bv.spheres[4].o = bv.spheres[0].o + delta;
- BVH_REAL r21 = 0, r22 = 0;
+ FCL_REAL r21 = 0, r22 = 0;
r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o);
r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o);
diff --git a/trunk/fcl/src/BV_splitter.cpp b/trunk/fcl/src/BV_splitter.cpp
index 530bc146..f66a8bc9 100644
--- a/trunk/fcl/src/BV_splitter.cpp
+++ b/trunk/fcl/src/BV_splitter.cpp
@@ -87,19 +87,19 @@ void computeSplitVector<OBBRSS>(const OBBRSS& bv, Vec3f& split_vector)
}
template<typename BV>
-void computeSplitValue_bvcenter(const BV& bv, BVH_REAL& split_value)
+void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value)
{
Vec3f center = bv.center();
split_value = center[0];
}
template<typename BV>
-void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, BVH_REAL& split_value)
+void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value)
{
- BVH_REAL sum = 0.0;
+ FCL_REAL sum = 0.0;
if(type == BVH_MODEL_TRIANGLES)
{
- BVH_REAL c[3] = {0.0, 0.0, 0.0};
+ FCL_REAL c[3] = {0.0, 0.0, 0.0};
for(int i = 0; i < num_primitives; ++i)
{
@@ -128,9 +128,9 @@ void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles,
}
template<typename BV>
-void computeSplitValue_median(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, BVH_REAL& split_value)
+void computeSplitValue_median(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value)
{
- std::vector<BVH_REAL> proj(num_primitives);
+ std::vector<FCL_REAL> proj(num_primitives);
if(type == BVH_MODEL_TRIANGLES)
{
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index bc82b9b9..a853aad8 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -71,13 +71,13 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
return cdata->done;
}
-bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, BVH_REAL& dist)
+bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& dist)
{
DistanceData* cdata = static_cast<DistanceData*>(cdata_);
if(cdata->done) { dist = cdata->min_distance; return true; }
- BVH_REAL d = distance(o1, o2);
+ FCL_REAL d = distance(o1, o2);
if(cdata->min_distance > d) cdata->min_distance = d;
dist = cdata->min_distance;
@@ -137,7 +137,7 @@ void NaiveCollisionManager::distance(CollisionObject* obj, void* cdata, Distance
if(size() == 0) return;
std::list<CollisionObject*>::const_iterator it;
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
for(it = objs.begin(); it != objs.end(); ++it)
{
if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
@@ -170,7 +170,7 @@ void NaiveCollisionManager::distance(void* cdata, DistanceCallBack callback) con
{
if(size() == 0) return;
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
std::list<CollisionObject*>::const_iterator it1;
std::list<CollisionObject*>::const_iterator it2;
for(it1 = objs.begin(); it1 != objs.end(); ++it1)
@@ -220,7 +220,7 @@ void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
return;
}
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
std::list<CollisionObject*>::const_iterator it1;
std::list<CollisionObject*>::const_iterator it2;
for(it1 = objs.begin(); it1 != objs.end(); ++it1)
@@ -392,7 +392,7 @@ bool SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterat
return false;
}
-bool SSaPCollisionManager::checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool SSaPCollisionManager::checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
while(pos_start < pos_end)
{
@@ -469,18 +469,18 @@ bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, Collision
void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
-bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(size() == 0) return false;
static const unsigned int CUTOFF = 100;
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
Vec3f dummy_vector = obj->getAABB().max_;
- if(min_dist < std::numeric_limits<BVH_REAL>::max())
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
dummy_vector += Vec3f(min_dist, min_dist, min_dist);
std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
@@ -491,7 +491,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, Distance
std::vector<CollisionObject*>::const_iterator pos_end3 = objs_z.end();
int status = 1;
- BVH_REAL old_min_distance;
+ FCL_REAL old_min_distance;
while(1)
{
@@ -540,7 +540,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, Distance
if(status == 1)
{
- if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
break;
else
{
@@ -637,7 +637,7 @@ void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) cons
std::vector<CollisionObject*>::const_iterator it, it_end;
size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
for(; it != it_end; ++it)
{
if(distance_(*it, cdata, callback, min_dist))
@@ -677,7 +677,7 @@ void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
return;
}
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
std::vector<CollisionObject*>::const_iterator it;
if(this->size() < other_manager->size())
{
@@ -768,13 +768,13 @@ void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& o
}
- BVH_REAL scale[3];
+ FCL_REAL scale[3];
for(size_t coord = 0; coord < 3; ++coord)
{
std::sort(endpoints.begin(), endpoints.end(),
- boost::bind(std::less<BVH_REAL>(),
- boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
- boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
+ boost::bind(std::less<FCL_REAL>(),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
endpoints[0]->prev[coord] = NULL;
endpoints[0]->next[coord] = endpoints[1];
@@ -853,7 +853,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
else // otherwise, find the correct location in the list and insert
{
EndPoint* curr_lo = curr->lo;
- BVH_REAL curr_lo_val = curr_lo->getVal()[coord];
+ FCL_REAL curr_lo_val = curr_lo->getVal()[coord];
while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != NULL))
current = current->next[coord];
@@ -880,7 +880,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
current = curr->lo;
EndPoint* curr_hi = curr->hi;
- BVH_REAL curr_hi_val = curr_hi->getVal()[coord];
+ FCL_REAL curr_hi_val = curr_hi->getVal()[coord];
if(coord == 0)
{
@@ -927,7 +927,7 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
void SaPCollisionManager::setup()
{
- BVH_REAL scale[3];
+ FCL_REAL scale[3];
scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
@@ -1152,8 +1152,8 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionC
size_t axis = optimal_axis;
const AABB& obj_aabb = obj->getAABB();
- BVH_REAL min_val = obj_aabb.min_[axis];
- BVH_REAL max_val = obj_aabb.max_[axis];
+ FCL_REAL min_val = obj_aabb.min_[axis];
+ FCL_REAL max_val = obj_aabb.max_[axis];
EndPoint dummy;
SaPAABB dummy_aabb;
@@ -1163,9 +1163,9 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionC
// compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<BVH_REAL>(),
- boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+ boost::bind(std::less<FCL_REAL>(),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
EndPoint* end_pos = NULL;
if(res_it != velist[axis].end())
@@ -1197,12 +1197,12 @@ void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCa
collide_(obj, cdata, callback);
}
-bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
AABB aabb = obj->getAABB();
- if(min_dist < std::numeric_limits<BVH_REAL>::max())
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
@@ -1211,15 +1211,15 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
size_t axis = optimal_axis;
int status = 1;
- BVH_REAL old_min_distance;
+ FCL_REAL old_min_distance;
EndPoint* start_pos = elist[axis];
while(1)
{
old_min_distance = min_dist;
- BVH_REAL min_val = aabb.min_[axis];
- BVH_REAL max_val = aabb.max_[axis];
+ FCL_REAL min_val = aabb.min_[axis];
+ FCL_REAL max_val = aabb.max_[axis];
EndPoint dummy;
SaPAABB dummy_aabb;
@@ -1229,9 +1229,9 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<BVH_REAL>(),
- boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+ boost::bind(std::less<FCL_REAL>(),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
EndPoint* end_pos = NULL;
if(res_it != velist[axis].end())
@@ -1277,7 +1277,7 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
if(status == 1)
{
- if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
break;
else
{
@@ -1307,7 +1307,7 @@ void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCa
{
if(size() == 0) return;
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
@@ -1333,7 +1333,7 @@ void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
enable_tested_set_ = true;
tested_set.clear();
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
{
@@ -1381,7 +1381,7 @@ void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, v
return;
}
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
if(this->size() < other_manager->size())
{
@@ -1744,11 +1744,11 @@ bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, C
void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
-bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(size() == 0) return false;
@@ -1756,14 +1756,14 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata,
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
AABB aabb = obj->getAABB();
- if(min_dist < std::numeric_limits<BVH_REAL>::max())
+ if(min_dist < std::numeric_limits<FCL_REAL>::max())
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
}
int status = 1;
- BVH_REAL old_min_distance;
+ FCL_REAL old_min_distance;
while(1)
{
@@ -1810,7 +1810,7 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata,
if(status == 1)
{
- if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ if(old_min_distance < std::numeric_limits<FCL_REAL>::max())
break;
else
{
@@ -1897,7 +1897,7 @@ void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callba
{
enable_tested_set_ = true;
tested_set.clear();
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
for(size_t i = 0; i < endpoints[0].size(); ++i)
if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break;
@@ -1936,7 +1936,7 @@ void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_ma
return;
}
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
if(this->size() < other_manager->size())
{
@@ -1975,7 +1975,7 @@ bool IntervalTreeCollisionManager::checkColl(std::deque<SimpleInterval*>::const_
return false;
}
-bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
while(pos_start < pos_end)
{
@@ -2056,10 +2056,10 @@ void DynamicAABBTreeCollisionManager::setup()
{
if(!setup_)
{
- int height = dtree.getMaxHeight(dtree.getRoot());
+ int height = dtree.getMaxHeight();
int num = dtree.size();
- if(height - std::log((BVH_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
+ if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
dtree.balanceIncremental(10);
else
dtree.balanceTopdown(tree_topdown_balance_threshold);
@@ -2173,7 +2173,7 @@ bool DynamicAABBTreeCollisionManager::selfCollisionRecurse(DynamicAABBNode* root
return false;
}
-bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(root1->isLeaf() && root2->isLeaf())
{
@@ -2184,8 +2184,8 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, Dy
if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
{
- BVH_REAL d1 = root2->bv.distance(root1->childs[0]->bv);
- BVH_REAL d2 = root2->bv.distance(root1->childs[1]->bv);
+ FCL_REAL d1 = root2->bv.distance(root1->childs[0]->bv);
+ FCL_REAL d2 = root2->bv.distance(root1->childs[1]->bv);
if(d2 < d1)
{
@@ -2218,8 +2218,8 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, Dy
}
else
{
- BVH_REAL d1 = root1->bv.distance(root2->childs[0]->bv);
- BVH_REAL d2 = root1->bv.distance(root2->childs[1]->bv);
+ FCL_REAL d1 = root1->bv.distance(root2->childs[0]->bv);
+ FCL_REAL d2 = root1->bv.distance(root2->childs[1]->bv);
if(d2 < d1)
{
@@ -2254,7 +2254,7 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, Dy
return false;
}
-bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(root->isLeaf())
{
@@ -2262,8 +2262,8 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, Col
return callback(root_obj, query, cdata, min_dist);
}
- BVH_REAL d1 = query->getAABB().distance(root->childs[0]->bv);
- BVH_REAL d2 = query->getAABB().distance(root->childs[1]->bv);
+ FCL_REAL d1 = query->getAABB().distance(root->childs[0]->bv);
+ FCL_REAL d2 = query->getAABB().distance(root->childs[1]->bv);
if(d2 < d1)
{
@@ -2297,7 +2297,7 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root, Col
return false;
}
-bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
if(root->isLeaf()) return false;
@@ -2313,4 +2313,325 @@ bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root,
return false;
}
+
+
+
+
+
+
+
+
+void DynamicAABBTreeCollisionManager2::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ if(size() > 0)
+ {
+ BroadPhaseCollisionManager::registerObjects(other_objs);
+ }
+ else
+ {
+ DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()];
+ table.rehash(other_objs.size());
+ for(size_t i = 0; i < other_objs.size(); ++i)
+ {
+ leaves[i].bv = other_objs[i]->getAABB();
+ leaves[i].parent = dtree.NULL_NODE;
+ leaves[i].childs[1] = dtree.NULL_NODE;
+ leaves[i].data = other_objs[i];
+ table[other_objs[i]] = i;
+ }
+
+ int n_leaves = other_objs.size();
+ dtree.init(leaves, n_leaves, tree_topdown_balance_threshold);
+
+ setup_ = true;
+ }
+}
+
+void DynamicAABBTreeCollisionManager2::registerObject(CollisionObject* obj)
+{
+ size_t node = dtree.insert(obj->getAABB(), obj);
+ table[obj] = node;
+
+}
+
+void DynamicAABBTreeCollisionManager2::unregisterObject(CollisionObject* obj)
+{
+ size_t node = table[obj];
+ table.erase(obj);
+ dtree.remove(node);
+}
+
+void DynamicAABBTreeCollisionManager2::setup()
+{
+ if(!setup_)
+ {
+ int height = dtree.getMaxHeight();
+ int num = dtree.size();
+
+ if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
+ dtree.balanceIncremental(10);
+ else
+ dtree.balanceTopdown(tree_topdown_balance_threshold);
+
+ setup_ = true;
+ }
+}
+
+
+void DynamicAABBTreeCollisionManager2::update()
+{
+ for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
+ {
+ CollisionObject* obj = it->first;
+ size_t node = it->second;
+ dtree.getNodes()[node].bv = obj->getAABB();
+ }
+
+ dtree.refit();
+ setup_ = false;
+
+ setup();
+}
+
+void DynamicAABBTreeCollisionManager2::update_(CollisionObject* updated_obj)
+{
+ DynamicAABBTable::const_iterator it = table.find(updated_obj);
+ if(it != table.end())
+ {
+ size_t node = it->second;
+ if(!dtree.getNodes()[node].bv.equal(updated_obj->getAABB()))
+ dtree.update(node, updated_obj->getAABB());
+ }
+ setup_ = false;
+}
+
+void DynamicAABBTreeCollisionManager2::update(CollisionObject* updated_obj)
+{
+ update_(updated_obj);
+ setup();
+}
+
+void DynamicAABBTreeCollisionManager2::update(const std::vector<CollisionObject*>& updated_objs)
+{
+ for(size_t i = 0; i < updated_objs.size(); ++i)
+ update_(updated_objs[i]);
+ setup();
+}
+
+bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id,
+ DynamicAABBNode* nodes2, size_t root2_id,
+ void* cdata, CollisionCallBack callback) const
+{
+ DynamicAABBNode* root1 = nodes1 + root1_id;
+ DynamicAABBNode* root2 = nodes2 + root2_id;
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ if(!root1->bv.overlap(root2->bv)) return false;
+ return callback(static_cast<CollisionObject*>(root1->data), static_cast<CollisionObject*>(root2->data), cdata);
+ }
+
+ if(!root1->bv.overlap(root2->bv)) return false;
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ if(collisionRecurse(nodes1, root1->childs[0], nodes2, root2_id, cdata, callback))
+ return true;
+ if(collisionRecurse(nodes1, root1->childs[1], nodes2, root2_id, cdata, callback))
+ return true;
+ }
+ else
+ {
+ if(collisionRecurse(nodes1, root1_id, nodes2, root2->childs[0], cdata, callback))
+ return true;
+ if(collisionRecurse(nodes1, root1_id, nodes2, root2->childs[1], cdata, callback))
+ return true;
+ }
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf())
+ {
+ if(!root->bv.overlap(query->getAABB())) return false;
+ return callback(static_cast<CollisionObject*>(root->data), query, cdata);
+ }
+
+ if(!root->bv.overlap(query->getAABB())) return false;
+
+ int select_res = alternative::select(query->getAABB(), root->childs[0], root->childs[1], nodes);
+
+ if(collisionRecurse(nodes, root->childs[select_res], query, cdata, callback))
+ return true;
+
+ if(collisionRecurse(nodes, root->childs[1-select_res], query, cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager2::selfCollisionRecurse(DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf()) return false;
+
+ if(selfCollisionRecurse(nodes, root->childs[0], cdata, callback))
+ return true;
+
+ if(selfCollisionRecurse(nodes, root->childs[1], cdata, callback))
+ return true;
+
+ if(collisionRecurse(nodes, root->childs[0], nodes, root->childs[1], cdata, callback))
+ return true;
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id,
+ DynamicAABBNode* nodes2, size_t root2_id,
+ void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ DynamicAABBNode* root1 = nodes1 + root1_id;
+ DynamicAABBNode* root2 = nodes2 + root2_id;
+ if(root1->isLeaf() && root2->isLeaf())
+ {
+ CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
+ CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
+ return callback(root1_obj, root2_obj, cdata, min_dist);
+ }
+
+ if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
+ {
+ FCL_REAL d1 = root2->bv.distance((nodes1 + root1->childs[0])->bv);
+ FCL_REAL d2 = root2->bv.distance((nodes1 + root1->childs[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[1], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[0], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[0], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1->childs[1], nodes2, root2_id, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ FCL_REAL d1 = root1->bv.distance((nodes2 + root2->childs[0])->bv);
+ FCL_REAL d2 = root1->bv.distance((nodes2 + root2->childs[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[1], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[0], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[0], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes1, root1_id, nodes2, root2->childs[1], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf())
+ {
+ CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
+ return callback(root_obj, query, cdata, min_dist);
+ }
+
+ FCL_REAL d1 = query->getAABB().distance((nodes + root->childs[0])->bv);
+ FCL_REAL d2 = query->getAABB().distance((nodes + root->childs[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->childs[1], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->childs[0], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->childs[0], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(nodes, root->childs[1], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager2::selfDistanceRecurse(DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ DynamicAABBNode* root = nodes + root_id;
+ if(root->isLeaf()) return false;
+
+ if(selfDistanceRecurse(nodes, root->childs[0], cdata, callback, min_dist))
+ return true;
+
+ if(selfDistanceRecurse(nodes, root->childs[1], cdata, callback, min_dist))
+ return true;
+
+ if(distanceRecurse(nodes, root->childs[0], nodes, root->childs[1], cdata, callback, min_dist))
+ return true;
+
+ return false;
+}
+
}
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index 2435cee8..31dc75f1 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -53,7 +53,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MotionBase<BV>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
- BVH_REAL& toc)
+ FCL_REAL& toc)
{
if(num_max_contacts <= 0 && !exhaustive)
{
@@ -125,7 +125,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
relativeTransform(R1_t, T1_t, R2_t, T2_t, node.R, node.T);
node.delta_t = 1;
- node.min_distance = std::numeric_limits<BVH_REAL>::max();
+ node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
@@ -162,7 +162,7 @@ template int conservativeAdvancement<RSS>(const CollisionGeometry* o1,
MotionBase<RSS>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
- BVH_REAL& toc);
+ FCL_REAL& toc);
}
diff --git a/trunk/fcl/src/distance.cpp b/trunk/fcl/src/distance.cpp
index e9cfdda4..2a4f6502 100644
--- a/trunk/fcl/src/distance.cpp
+++ b/trunk/fcl/src/distance.cpp
@@ -51,13 +51,13 @@ DistanceFunctionMatrix<GJKSolver>& getDistanceFunctionLookTable()
}
template<typename NarrowPhaseSolver>
-BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver)
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver)
{
return distance<NarrowPhaseSolver>(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver);
}
template<typename NarrowPhaseSolver>
-BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
+FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver_)
{
@@ -72,7 +72,7 @@ BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
OBJECT_TYPE object_type2 = o2->getObjectType();
NODE_TYPE node_type2 = o2->getNodeType();
- BVH_REAL res;
+ FCL_REAL res;
if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
{
@@ -105,18 +105,18 @@ BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
return res;
}
-template BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver);
-template BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver);
-template BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver);
-template BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver);
+template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver);
+template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver);
+template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver);
+template FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver);
-BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2)
+FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2)
{
GJKSolver_libccd solver;
return distance<GJKSolver_libccd>(o1, o2, &solver);
}
-BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
+FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2)
{
GJKSolver_libccd solver;
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index 504f3ab3..e79cd523 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -44,7 +44,7 @@ namespace fcl
{
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
-BVH_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
{
ShapeDistanceTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
@@ -58,7 +58,7 @@ BVH_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform&
template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer
{
- static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
{
MeshShapeDistanceTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -77,7 +77,7 @@ struct BVHShapeDistancer
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
{
- static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
{
MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
@@ -94,7 +94,7 @@ struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
{
- static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
{
MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
@@ -110,7 +110,7 @@ struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
template<typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
{
- static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ static FCL_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
{
MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
@@ -125,7 +125,7 @@ struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
template<typename T_BVH>
-BVH_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
{
MeshDistanceTraversalNode<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -142,7 +142,7 @@ BVH_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, co
}
template<>
-BVH_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
{
MeshDistanceTraversalNodeRSS node;
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
@@ -155,7 +155,7 @@ BVH_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf
}
template<>
-BVH_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
{
MeshDistanceTraversalNodekIOS node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
@@ -169,7 +169,7 @@ BVH_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& t
template<>
-BVH_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
{
MeshDistanceTraversalNodeOBBRSS node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
@@ -183,7 +183,7 @@ BVH_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform&
template<typename T_BVH, typename NarrowPhaseSolver>
-BVH_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver)
{
return BVHDistance<T_BVH>(o1, tf1, o2, tf2);
diff --git a/trunk/fcl/src/geometric_shapes.cpp b/trunk/fcl/src/geometric_shapes.cpp
index ecef677a..c594a7f8 100644
--- a/trunk/fcl/src/geometric_shapes.cpp
+++ b/trunk/fcl/src/geometric_shapes.cpp
@@ -99,10 +99,10 @@ void Convex::fillEdges()
void Plane::unitNormalTest()
{
- BVH_REAL l = n.length();
+ FCL_REAL l = n.length();
if(l > 0)
{
- BVH_REAL inv_l = 1.0 / l;
+ FCL_REAL inv_l = 1.0 / l;
n *= inv_l;
d *= inv_l;
}
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 4e52249c..09bdd178 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -47,9 +47,9 @@ namespace details
std::vector<Vec3f> getBoundVertices(const Box& box, const SimpleTransform& tf)
{
std::vector<Vec3f> result(8);
- BVH_REAL a = box.side[0] / 2;
- BVH_REAL b = box.side[1] / 2;
- BVH_REAL c = box.side[2] / 2;
+ FCL_REAL a = box.side[0] / 2;
+ FCL_REAL b = box.side[1] / 2;
+ FCL_REAL c = box.side[2] / 2;
result[0] = tf.transform(Vec3f(a, b, c));
result[1] = tf.transform(Vec3f(a, b, -c));
result[2] = tf.transform(Vec3f(a, -b, c));
@@ -66,11 +66,11 @@ std::vector<Vec3f> getBoundVertices(const Box& box, const SimpleTransform& tf)
std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const SimpleTransform& tf)
{
std::vector<Vec3f> result(12);
- const BVH_REAL m = (1 + sqrt(5.0)) / 2.0;
- BVH_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
+ const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
+ FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
- BVH_REAL a = edge_size;
- BVH_REAL b = m * edge_size;
+ FCL_REAL a = edge_size;
+ FCL_REAL b = m * edge_size;
result[0] = tf.transform(Vec3f(0, a, b));
result[1] = tf.transform(Vec3f(0, -a, b));
result[2] = tf.transform(Vec3f(0, a, -b));
@@ -90,13 +90,13 @@ std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const SimpleTransform&
std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const SimpleTransform& tf)
{
std::vector<Vec3f> result(36);
- const BVH_REAL m = (1 + sqrt(5.0)) / 2.0;
+ const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
- BVH_REAL hl = capsule.lz * 0.5;
- BVH_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
- BVH_REAL a = edge_size;
- BVH_REAL b = m * edge_size;
- BVH_REAL r2 = capsule.radius * 2 / sqrt(3.0);
+ FCL_REAL hl = capsule.lz * 0.5;
+ FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
+ FCL_REAL a = edge_size;
+ FCL_REAL b = m * edge_size;
+ FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0);
result[0] = tf.transform(Vec3f(0, a, b + hl));
@@ -125,8 +125,8 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const SimpleTransfor
result[22] = tf.transform(Vec3f(-b, 0, a - hl));
result[23] = tf.transform(Vec3f(-b, 0, -a - hl));
- BVH_REAL c = 0.5 * r2;
- BVH_REAL d = capsule.radius;
+ FCL_REAL c = 0.5 * r2;
+ FCL_REAL d = capsule.radius;
result[24] = tf.transform(Vec3f(r2, 0, hl));
result[25] = tf.transform(Vec3f(c, d, hl));
result[26] = tf.transform(Vec3f(-c, d, hl));
@@ -149,10 +149,10 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const SimpleTransform& tf)
{
std::vector<Vec3f> result(7);
- BVH_REAL hl = cone.lz * 0.5;
- BVH_REAL r2 = cone.radius * 2 / sqrt(3.0);
- BVH_REAL a = 0.5 * r2;
- BVH_REAL b = cone.radius;
+ FCL_REAL hl = cone.lz * 0.5;
+ FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
+ FCL_REAL a = 0.5 * r2;
+ FCL_REAL b = cone.radius;
result[0] = tf.transform(Vec3f(r2, 0, -hl));
result[1] = tf.transform(Vec3f(a, b, -hl));
@@ -170,10 +170,10 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const SimpleTransf
{
std::vector<Vec3f> result(12);
- BVH_REAL hl = cylinder.lz * 0.5;
- BVH_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
- BVH_REAL a = 0.5 * r2;
- BVH_REAL b = cylinder.radius;
+ FCL_REAL hl = cylinder.lz * 0.5;
+ FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
+ FCL_REAL a = 0.5 * r2;
+ FCL_REAL b = cylinder.radius;
result[0] = tf.transform(Vec3f(r2, 0, -hl));
result[1] = tf.transform(Vec3f(a, b, -hl));
@@ -222,9 +222,9 @@ void computeBV<AABB, Box>(const Box& s, const SimpleTransform& tf, AABB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- BVH_REAL x_range = 0.5 * (fabs(R[0][0] * s.side[0]) + fabs(R[0][1] * s.side[1]) + fabs(R[0][2] * s.side[2]));
- BVH_REAL y_range = 0.5 * (fabs(R[1][0] * s.side[0]) + fabs(R[1][1] * s.side[1]) + fabs(R[1][2] * s.side[2]));
- BVH_REAL z_range = 0.5 * (fabs(R[2][0] * s.side[0]) + fabs(R[2][1] * s.side[1]) + fabs(R[2][2] * s.side[2]));
+ FCL_REAL x_range = 0.5 * (fabs(R[0][0] * s.side[0]) + fabs(R[0][1] * s.side[1]) + fabs(R[0][2] * s.side[2]));
+ FCL_REAL y_range = 0.5 * (fabs(R[1][0] * s.side[0]) + fabs(R[1][1] * s.side[1]) + fabs(R[1][2] * s.side[2]));
+ FCL_REAL z_range = 0.5 * (fabs(R[2][0] * s.side[0]) + fabs(R[2][1] * s.side[1]) + fabs(R[2][2] * s.side[2]));
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
@@ -245,9 +245,9 @@ void computeBV<AABB, Capsule>(const Capsule& s, const SimpleTransform& tf, AABB&
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- BVH_REAL x_range = 0.5 * fabs(R[0][2] * s.lz) + s.radius;
- BVH_REAL y_range = 0.5 * fabs(R[1][2] * s.lz) + s.radius;
- BVH_REAL z_range = 0.5 * fabs(R[2][2] * s.lz) + s.radius;
+ FCL_REAL x_range = 0.5 * fabs(R[0][2] * s.lz) + s.radius;
+ FCL_REAL y_range = 0.5 * fabs(R[1][2] * s.lz) + s.radius;
+ FCL_REAL z_range = 0.5 * fabs(R[2][2] * s.lz) + s.radius;
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
@@ -259,9 +259,9 @@ void computeBV<AABB, Cone>(const Cone& s, const SimpleTransform& tf, AABB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
- BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
- BVH_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
+ FCL_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
+ FCL_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
+ FCL_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
@@ -273,9 +273,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, AAB
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
- BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
- BVH_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
+ FCL_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
+ FCL_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
+ FCL_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
@@ -316,19 +316,19 @@ void computeBV<AABB, Plane>(const Plane& s, const SimpleTransform& tf, AABB& bv)
Vec3f n = R * s.n;
AABB bv_;
- if(n[1] == (BVH_REAL)0.0 && n[2] == (BVH_REAL)0.0)
+ if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
{
// normal aligned with x axis
if(n[0] < 0) bv_.min_[0] = -s.d;
else if(n[0] > 0) bv_.max_[0] = s.d;
}
- else if(n[0] == (BVH_REAL)0.0 && n[2] == (BVH_REAL)0.0)
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
{
// normal aligned with y axis
if(n[1] < 0) bv_.min_[1] = -s.d;
else if(n[1] > 0) bv_.max_[1] = s.d;
}
- else if(n[0] == (BVH_REAL)0.0 && n[1] == (BVH_REAL)0.0)
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
{
// normal aligned with z axis
if(n[2] < 0) bv_.min_[2] = -s.d;
@@ -349,7 +349,7 @@ void computeBV<OBB, Box>(const Box& s, const SimpleTransform& tf, OBB& bv)
bv.axis[0] = R.getColumn(0);
bv.axis[1] = R.getColumn(1);
bv.axis[2] = R.getColumn(2);
- bv.extent = s.side * (BVH_REAL)0.5;
+ bv.extent = s.side * (FCL_REAL)0.5;
}
template<>
@@ -428,7 +428,7 @@ void computeBV<OBB, Plane>(const Plane& s, const SimpleTransform& tf, OBB& bv)
generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
bv.axis[0] = s.n;
- bv.extent.setValue(0, std::numeric_limits<BVH_REAL>::max(), std::numeric_limits<BVH_REAL>::max());
+ bv.extent.setValue(0, std::numeric_limits<FCL_REAL>::max(), std::numeric_limits<FCL_REAL>::max());
Vec3f p = s.n * s.d;
bv.To = R * p + T;
@@ -443,10 +443,10 @@ void computeBV<RSS, Plane>(const Plane& s, const SimpleTransform& tf, RSS& bv)
generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
bv.axis[0] = s.n;
- bv.l[0] = std::numeric_limits<BVH_REAL>::max();
- bv.l[1] = std::numeric_limits<BVH_REAL>::max();
+ bv.l[0] = std::numeric_limits<FCL_REAL>::max();
+ bv.l[1] = std::numeric_limits<FCL_REAL>::max();
- bv.r = std::numeric_limits<BVH_REAL>::max();
+ bv.r = std::numeric_limits<FCL_REAL>::max();
}
template<>
diff --git a/trunk/fcl/src/gjk.cpp b/trunk/fcl/src/gjk.cpp
index 29f5fcea..f5daef36 100644
--- a/trunk/fcl/src/gjk.cpp
+++ b/trunk/fcl/src/gjk.cpp
@@ -49,9 +49,9 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
case GEOM_TRIANGLE:
{
const Triangle2* triangle = static_cast<const Triangle2*>(shape);
- BVH_REAL dota = dir.dot(triangle->a);
- BVH_REAL dotb = dir.dot(triangle->b);
- BVH_REAL dotc = dir.dot(triangle->c);
+ FCL_REAL dota = dir.dot(triangle->a);
+ FCL_REAL dotb = dir.dot(triangle->b);
+ FCL_REAL dotc = dir.dot(triangle->c);
if(dota > dotb)
{
if(dotc > dota)
@@ -85,7 +85,7 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
case GEOM_CAPSULE:
{
const Capsule* capsule = static_cast<const Capsule*>(shape);
- BVH_REAL half_h = capsule->lz * 0.5;
+ FCL_REAL half_h = capsule->lz * 0.5;
Vec3f pos1(0, 0, half_h);
Vec3f pos2(0, 0, -half_h);
Vec3f v = dir * capsule->radius;
@@ -99,20 +99,20 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
case GEOM_CONE:
{
const Cone* cone = static_cast<const Cone*>(shape);
- BVH_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
- BVH_REAL len = zdist + dir[2] * dir[2];
+ FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
+ FCL_REAL len = zdist + dir[2] * dir[2];
zdist = std::sqrt(zdist);
len = std::sqrt(len);
- BVH_REAL half_h = cone->lz * 0.5;
- BVH_REAL radius = cone->radius;
+ FCL_REAL half_h = cone->lz * 0.5;
+ FCL_REAL radius = cone->radius;
- BVH_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
+ FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
if(dir[2] > len * sin_a)
return Vec3f(0, 0, half_h);
else if(zdist > 0)
{
- BVH_REAL rad = radius / zdist;
+ FCL_REAL rad = radius / zdist;
return Vec3f(rad * dir[0], rad * dir[1], -half_h);
}
else
@@ -122,15 +122,15 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
case GEOM_CYLINDER:
{
const Cylinder* cylinder = static_cast<const Cylinder*>(shape);
- BVH_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
- BVH_REAL half_h = cylinder->lz * 0.5;
+ FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
+ FCL_REAL half_h = cylinder->lz * 0.5;
if(zdist == 0.0)
{
return Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
}
else
{
- BVH_REAL d = cylinder->radius / zdist;
+ FCL_REAL d = cylinder->radius / zdist;
return Vec3f(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h);
}
}
@@ -138,12 +138,12 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
case GEOM_CONVEX:
{
const Convex* convex = static_cast<const Convex*>(shape);
- BVH_REAL maxdot = - std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL maxdot = - std::numeric_limits<FCL_REAL>::max();
Vec3f* curp = convex->points;
Vec3f bestv;
for(size_t i = 0; i < convex->num_points; ++i, curp+=1)
{
- BVH_REAL dot = dir.dot(*curp);
+ FCL_REAL dot = dir.dot(*curp);
if(dot > maxdot)
{
bestv = *curp;
@@ -164,13 +164,13 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
}
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m)
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m)
{
const Vec3f d = b - a;
- const BVH_REAL l = d.sqrLength();
+ const FCL_REAL l = d.sqrLength();
if(l > 0)
{
- const BVH_REAL t(l > 0 ? - a.dot(d) / l : 0);
+ const FCL_REAL t(l > 0 ? - a.dot(d) / l : 0);
if(t >= 1) { w[0] = 0; w[1] = 1; m = 2; return b.sqrLength(); } // m = 0x10
else if(t <= 0) { w[0] = 1; w[1] = 0; m = 1; return a.sqrLength(); } // m = 0x01
else { w[0] = 1 - (w[1] = t); m = 3; return (a + d * t).sqrLength(); } // m = 0x11
@@ -179,25 +179,25 @@ BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m)
return -1;
}
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL* w, size_t& m)
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m)
{
static const size_t nexti[3] = {1, 2, 0};
const Vec3f* vt[] = {&a, &b, &c};
const Vec3f dl[] = {a - b, b - c, c - a};
const Vec3f& n = dl[0].cross(dl[1]);
- const BVH_REAL l = n.sqrLength();
+ const FCL_REAL l = n.sqrLength();
if(l > 0)
{
- BVH_REAL mindist = -1;
- BVH_REAL subw[2] = {0, 0};
+ FCL_REAL mindist = -1;
+ FCL_REAL subw[2] = {0, 0};
size_t subm = 0;
for(size_t i = 0; i < 3; ++i)
{
if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
{
size_t j = nexti[i];
- BVH_REAL subd = projectOrigin(*vt[i], *vt[j], subw, subm);
+ FCL_REAL subd = projectOrigin(*vt[i], *vt[j], subw, subm);
if(mindist < 0 || subd < mindist)
{
mindist = subd;
@@ -211,8 +211,8 @@ BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL*
if(mindist < 0) // the origin project is within the triangle
{
- BVH_REAL d = a.dot(n);
- BVH_REAL s = sqrt(l);
+ FCL_REAL d = a.dot(n);
+ FCL_REAL s = sqrt(l);
Vec3f p = n * (d / l);
mindist = p.sqrLength();
m = 7; // m = 0x111
@@ -226,25 +226,25 @@ BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL*
return -1;
}
-BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, BVH_REAL* w, size_t& m)
+FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m)
{
static const size_t nexti[] = {1, 2, 0};
const Vec3f* vt[] = {&a, &b, &c, &d};
const Vec3f dl[3] = {a-d, b-d, c-d};
- BVH_REAL vl = triple(dl[0], dl[1], dl[2]);
+ FCL_REAL vl = triple(dl[0], dl[1], dl[2]);
bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0;
if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
{
- BVH_REAL mindist = -1;
- BVH_REAL subw[3] = {0, 0, 0};
+ FCL_REAL mindist = -1;
+ FCL_REAL subw[3] = {0, 0, 0};
size_t subm = 0;
for(size_t i = 0; i < 3; ++i)
{
size_t j = nexti[i];
- BVH_REAL s = vl * d.dot(dl[i].cross(dl[j]));
+ FCL_REAL s = vl * d.dot(dl[i].cross(dl[j]));
if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
{
- BVH_REAL subd = projectOrigin(*vt[i], *vt[j], d, subw, subm);
+ FCL_REAL subd = projectOrigin(*vt[i], *vt[j], d, subw, subm);
if(mindist < 0 || subd < mindist)
{
mindist = subd;
@@ -285,8 +285,8 @@ void GJK::initialize()
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
{
size_t iterations = 0;
- BVH_REAL sqdist = 0;
- BVH_REAL alpha = 0;
+ FCL_REAL sqdist = 0;
+ FCL_REAL alpha = 0;
Vec3f lastw[4];
size_t clastw = 0;
@@ -303,7 +303,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
simplices[0].rank = 0;
ray = guess;
- BVH_REAL sqrl = ray.sqrLength();
+ FCL_REAL sqrl = ray.sqrLength();
appendVertex(simplices[0], (sqrl>0) ? -ray : Vec3f(1, 0, 0));
simplices[0].p[0] = 1;
ray = simplices[0].c[0]->w;
@@ -316,7 +316,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
Simplex& curr_simplex = simplices[current];
Simplex& next_simplex = simplices[next];
- BVH_REAL rl = ray.sqrLength();
+ FCL_REAL rl = ray.sqrLength();
if(rl < tolerance) // means origin is near the face of original simplex, return touch
{
status = Inside;
@@ -346,7 +346,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
}
// check for termination, from bullet
- BVH_REAL omega = ray.dot(w) / rl;
+ FCL_REAL omega = ray.dot(w) / rl;
alpha = std::max(alpha, omega);
if((rl - alpha) - tolerance * rl <= 0)
{
@@ -355,7 +355,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
}
// reduce simplex and decide the extend direction
- BVH_REAL weights[4];
+ FCL_REAL weights[4];
size_t mask = 0; // decide the simplex vertices that compose the minimal distance
switch(curr_simplex.rank)
{
@@ -499,20 +499,20 @@ void EPA::initialize()
stock.append(&fc_store[max_face_num-i-1]);
}
-bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist)
+bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist)
{
Vec3f ba = b->w - a->w;
Vec3f n_ab = ba.cross(face->n);
- BVH_REAL a_dot_nab = a->w.dot(n_ab);
+ FCL_REAL a_dot_nab = a->w.dot(n_ab);
if(a_dot_nab < 0) // the origin is on the outside part of ab
{
- BVH_REAL ba_l2 = ba.sqrLength();
+ FCL_REAL ba_l2 = ba.sqrLength();
// following is similar to projectOrigin for two points
// however, as we dont need to compute the parameterization, dont need to compute 0 or 1
- BVH_REAL a_dot_ba = a->w.dot(ba);
- BVH_REAL b_dot_ba = b->w.dot(ba);
+ FCL_REAL a_dot_ba = a->w.dot(ba);
+ FCL_REAL b_dot_ba = b->w.dot(ba);
if(a_dot_ba > 0)
dist = a->w.length();
@@ -520,8 +520,8 @@ bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist)
dist = b->w.length();
else
{
- BVH_REAL a_dot_b = a->w.dot(b->w);
- dist = std::sqrt(std::max(a->w.sqrLength() * b->w.sqrLength() - a_dot_b * a_dot_b, (BVH_REAL)0));
+ FCL_REAL a_dot_b = a->w.dot(b->w);
+ dist = std::sqrt(std::max(a->w.sqrLength() * b->w.sqrLength() - a_dot_b * a_dot_b, (FCL_REAL)0));
}
return true;
@@ -542,7 +542,7 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
face->c[1] = b;
face->c[2] = c;
face->n = (b->w - a->w).cross(c->w - a->w);
- BVH_REAL l = face->n.length();
+ FCL_REAL l = face->n.length();
if(l > tolerance)
{
@@ -575,10 +575,10 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
EPA::SimplexF* EPA::findBest()
{
SimplexF* minf = hull.root;
- BVH_REAL mind = minf->d * minf->d;
+ FCL_REAL mind = minf->d * minf->d;
for(SimplexF* f = minf->l[1]; f; f = f->l[1])
{
- BVH_REAL sqd = f->d * f->d;
+ FCL_REAL sqd = f->d * f->d;
if(sqd < mind)
{
minf = f;
@@ -609,7 +609,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
simplex.c[0] = simplex.c[1];
simplex.c[1] = tmp;
- BVH_REAL tmpv = simplex.p[0];
+ FCL_REAL tmpv = simplex.p[0];
simplex.p[0] = simplex.p[1];
simplex.p[1] = tmpv;
}
@@ -644,7 +644,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
bool valid = true;
best->pass = ++pass;
gjk.getSupport(best->n, *w);
- BVH_REAL wdist = best->n.dot(w->w) - best->d;
+ FCL_REAL wdist = best->n.dot(w->w) - best->d;
if(wdist > tolerance)
{
for(size_t j = 0; (j < 3) && valid; ++j)
@@ -689,7 +689,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).length();
result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).length();
- BVH_REAL sum = result.p[0] + result.p[1] + result.p[2];
+ FCL_REAL sum = result.p[0] + result.p[1] + result.p[2];
result.p[0] /= sum;
result.p[1] /= sum;
result.p[2] /= sum;
@@ -699,7 +699,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
status = FallBack;
normal = -guess;
- BVH_REAL nl = normal.length();
+ FCL_REAL nl = normal.length();
if(nl > 0) normal /= nl;
else normal = Vec3f(1, 0, 0);
depth = 0;
diff --git a/trunk/fcl/src/gjk_libccd.cpp b/trunk/fcl/src/gjk_libccd.cpp
index c5515e62..c445f592 100644
--- a/trunk/fcl/src/gjk_libccd.cpp
+++ b/trunk/fcl/src/gjk_libccd.cpp
@@ -686,8 +686,8 @@ static void centerTriangle(const void* obj, ccd_vec3_t* c)
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
- unsigned int max_iterations, BVH_REAL tolerance,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal)
+ unsigned int max_iterations, FCL_REAL tolerance,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
ccd_t ccd;
int res;
@@ -724,8 +724,8 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
- unsigned int max_iterations, BVH_REAL tolerance_,
- BVH_REAL* res)
+ unsigned int max_iterations, FCL_REAL tolerance_,
+ FCL_REAL* res)
{
ccd_t ccd;
ccd_real_t dist;
diff --git a/trunk/fcl/src/hierarchy_tree.cpp b/trunk/fcl/src/hierarchy_tree.cpp
index 5a0f6858..015ce41d 100644
--- a/trunk/fcl/src/hierarchy_tree.cpp
+++ b/trunk/fcl/src/hierarchy_tree.cpp
@@ -48,8 +48,8 @@ size_t select(const NodeBase<AABB>& node, const NodeBase<AABB>& node1, const Nod
Vec3f v = bv.min_ + bv.max_;
Vec3f v1 = v - (bv1.min_ + bv1.max_);
Vec3f v2 = v - (bv2.min_ + bv2.max_);
- BVH_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
- BVH_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+ FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+ FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
}
@@ -62,13 +62,13 @@ size_t select(const AABB& query, const NodeBase<AABB>& node1, const NodeBase<AAB
Vec3f v = bv.min_ + bv.max_;
Vec3f v1 = v - (bv1.min_ + bv1.max_);
Vec3f v2 = v - (bv2.min_ + bv2.max_);
- BVH_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
- BVH_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+ FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+ FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
}
template<>
-bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, BVH_REAL margin)
+bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin)
{
AABB bv(bv_);
if(leaf->bv.contain(bv)) return false;
@@ -100,4 +100,36 @@ bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Ve
return true;
}
+namespace alternative
+{
+template<>
+size_t select(size_t query, size_t node1, size_t node2, NodeBase<AABB>* nodes)
+{
+ const AABB& bv = nodes[query].bv;
+ const AABB& bv1 = nodes[node1].bv;
+ const AABB& bv2 = nodes[node2].bv;
+ Vec3f v = bv.min_ + bv.max_;
+ Vec3f v1 = v - (bv1.min_ + bv1.max_);
+ Vec3f v2 = v - (bv2.min_ + bv2.max_);
+ FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+ FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+ return (d1 < d2) ? 0 : 1;
+}
+
+template<>
+size_t select(const AABB& query, size_t node1, size_t node2, NodeBase<AABB>* nodes)
+{
+ const AABB& bv = query;
+ const AABB& bv1 = nodes[node1].bv;
+ const AABB& bv2 = nodes[node2].bv;
+ Vec3f v = bv.min_ + bv.max_;
+ Vec3f v1 = v - (bv1.min_ + bv1.max_);
+ Vec3f v2 = v - (bv2.min_ + bv2.max_);
+ FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
+ FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
+ return (d1 < d2) ? 0 : 1;
+}
+
+}
+
}
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 436833b8..9185aeba 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -41,20 +41,20 @@
namespace fcl
{
-const BVH_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9;
+const FCL_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9;
-bool PolySolver::isZero(BVH_REAL v)
+bool PolySolver::isZero(FCL_REAL v)
{
return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
}
-bool PolySolver::cbrt(BVH_REAL v)
+bool PolySolver::cbrt(FCL_REAL v)
{
return powf(v, 1.0 / 3.0);
}
-int PolySolver::solveLinear(BVH_REAL c[2], BVH_REAL s[1])
+int PolySolver::solveLinear(FCL_REAL c[2], FCL_REAL s[1])
{
if(isZero(c[1]))
return 0;
@@ -62,9 +62,9 @@ int PolySolver::solveLinear(BVH_REAL c[2], BVH_REAL s[1])
return 1;
}
-int PolySolver::solveQuadric(BVH_REAL c[3], BVH_REAL s[2])
+int PolySolver::solveQuadric(FCL_REAL c[3], FCL_REAL s[2])
{
- BVH_REAL p, q, D;
+ FCL_REAL p, q, D;
// make sure we have a d2 equation
@@ -78,7 +78,7 @@ int PolySolver::solveQuadric(BVH_REAL c[3], BVH_REAL s[2])
if(isZero(D))
{
- // one BVH_REAL root
+ // one FCL_REAL root
s[0] = s[1] = -p;
return 1;
}
@@ -89,19 +89,19 @@ int PolySolver::solveQuadric(BVH_REAL c[3], BVH_REAL s[2])
else
{
// two real roots
- BVH_REAL sqrt_D = sqrt(D);
+ FCL_REAL sqrt_D = sqrt(D);
s[0] = sqrt_D - p;
s[1] = -sqrt_D - p;
return 2;
}
}
-int PolySolver::solveCubic(BVH_REAL c[4], BVH_REAL s[3])
+int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3])
{
int i, num;
- BVH_REAL sub, A, B, C, sq_A, p, q, cb_p, D;
- const BVH_REAL ONE_OVER_THREE = 1 / 3.0;
- const BVH_REAL PI = 3.14159265358979323846;
+ 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]))
@@ -131,8 +131,8 @@ int PolySolver::solveCubic(BVH_REAL c[4], BVH_REAL s[3])
}
else
{
- // one single and one BVH_REAL solution
- BVH_REAL u = cbrt(-q);
+ // one single and one FCL_REAL solution
+ FCL_REAL u = cbrt(-q);
s[0] = 2.0 * u;
s[1] = -u;
num = 2;
@@ -143,8 +143,8 @@ int PolySolver::solveCubic(BVH_REAL c[4], BVH_REAL s[3])
if(D < 0.0)
{
// three real solutions
- BVH_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p));
- BVH_REAL t = 2.0 * sqrt(-p);
+ 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);
@@ -153,8 +153,8 @@ int PolySolver::solveCubic(BVH_REAL c[4], BVH_REAL s[3])
else
{
// one real solution
- BVH_REAL sqrt_D = sqrt(D);
- BVH_REAL u = cbrt(sqrt_D + fabs(q));
+ FCL_REAL sqrt_D = sqrt(D);
+ FCL_REAL u = cbrt(sqrt_D + fabs(q));
if(q > 0.0)
s[0] = - u + p / u ;
else
@@ -280,12 +280,12 @@ CloudClassifierParam::CloudClassifierParam()
#endif
-const BVH_REAL Intersect::EPSILON = 1e-5;
-const BVH_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7;
-const BVH_REAL Intersect::CCD_RESOLUTION = 1e-7;
+const FCL_REAL Intersect::EPSILON = 1e-5;
+const FCL_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7;
+const FCL_REAL Intersect::CCD_RESOLUTION = 1e-7;
-bool Intersect::isZero(BVH_REAL v)
+bool Intersect::isZero(FCL_REAL v)
{
return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
}
@@ -295,11 +295,11 @@ bool Intersect::isZero(BVH_REAL v)
*/
bool Intersect::solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL& l, BVH_REAL& r, bool bVF, BVH_REAL coeffs[], Vec3f* data)
+ FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data)
{
- BVH_REAL v2[2]= {l*l,r*r};
- BVH_REAL v[2]= {l,r};
- BVH_REAL r_backup;
+ FCL_REAL v2[2]= {l*l,r*r};
+ FCL_REAL v[2]= {l,r};
+ FCL_REAL r_backup;
unsigned char min3, min2, min1, max3, max2, max1;
@@ -309,25 +309,25 @@ bool Intersect::solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, c
// bound the cubic
- BVH_REAL minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0];
- BVH_REAL major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0];
+ FCL_REAL minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0];
+ FCL_REAL major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0];
if(major<0) return false;
if(minor>0) return false;
// starting here, the bounds have opposite values
- BVH_REAL m = 0.5 * (r + l);
+ FCL_REAL m = 0.5 * (r + l);
// bound the derivative
- BVH_REAL dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1];
- BVH_REAL dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1];
+ FCL_REAL dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1];
+ FCL_REAL dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1];
if((dminor > 0)||(dmajor < 0)) // we can use Newton
{
- BVH_REAL m2 = m*m;
- BVH_REAL fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0];
- BVH_REAL nl = m;
- BVH_REAL nu = m;
+ FCL_REAL m2 = m*m;
+ FCL_REAL fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0];
+ FCL_REAL nl = m;
+ FCL_REAL nu = m;
if(fm>0)
{
nl-=(fm/dminor);
@@ -402,7 +402,7 @@ bool Intersect::insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p
Return FALSE if no solution exists.
*/
bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, BVH_REAL* mua, BVH_REAL* mub)
+ Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub)
{
Vec3f p31 = p1 - p3;
Vec3f p34 = p4 - p3;
@@ -413,16 +413,16 @@ bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f&
if(fabs(p12[0]) < EPSILON && fabs(p12[1]) < EPSILON && fabs(p12[2]) < EPSILON)
return false;
- BVH_REAL d3134 = p31.dot(p34);
- BVH_REAL d3412 = p34.dot(p12);
- BVH_REAL d3112 = p31.dot(p12);
- BVH_REAL d3434 = p34.dot(p34);
- BVH_REAL d1212 = p12.dot(p12);
+ FCL_REAL d3134 = p31.dot(p34);
+ FCL_REAL d3412 = p34.dot(p12);
+ FCL_REAL d3112 = p31.dot(p12);
+ FCL_REAL d3434 = p34.dot(p34);
+ FCL_REAL d1212 = p12.dot(p12);
- BVH_REAL denom = d1212 * d3434 - d3412 * d3412;
+ FCL_REAL denom = d1212 * d3434 - d3412 * d3412;
if(fabs(denom) < EPSILON)
return false;
- BVH_REAL numer = d3134 * d3412 - d3112 * d3434;
+ FCL_REAL numer = d3134 * d3412 - d3112 * d3434;
*mua = numer / denom;
if(*mua < 0 || *mua > 1)
@@ -439,21 +439,21 @@ bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f&
bool Intersect::checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL t)
+ FCL_REAL t)
{
return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t);
}
bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL t, Vec3f* q_i)
+ FCL_REAL t, Vec3f* q_i)
{
Vec3f a = a0 + va * t;
Vec3f b = b0 + vb * t;
Vec3f c = c0 + vc * t;
Vec3f d = d0 + vd * t;
Vec3f p1, p2;
- BVH_REAL t_ab, t_cd;
+ FCL_REAL t_ab, t_cd;
if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd))
{
if(q_i) *q_i = p1;
@@ -465,26 +465,26 @@ bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec
bool Intersect::checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- BVH_REAL t)
+ FCL_REAL t)
{
return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t);
}
-bool Intersect::solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
+bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
bool bVF,
- BVH_REAL* ret)
+ FCL_REAL* ret)
{
- BVH_REAL discriminant = b * b - 4 * a * c;
+ FCL_REAL discriminant = b * b - 4 * a * c;
if(discriminant < 0)
return false;
- BVH_REAL sqrt_dis = sqrt(discriminant);
- BVH_REAL r1 = (-b + sqrt_dis) / (2 * a);
+ FCL_REAL sqrt_dis = sqrt(discriminant);
+ FCL_REAL r1 = (-b + sqrt_dis) / (2 * a);
bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false;
- BVH_REAL r2 = (-b - sqrt_dis) / (2 * a);
+ FCL_REAL r2 = (-b - sqrt_dis) / (2 * a);
bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false;
if(v1 && v2)
@@ -506,27 +506,27 @@ bool Intersect::solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
return false;
}
-bool Intersect::solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
+bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp)
{
if(isZero(a))
{
- BVH_REAL t = -c/b;
+ FCL_REAL t = -c/b;
return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false;
}
- BVH_REAL discriminant = b*b-4*a*c;
+ FCL_REAL discriminant = b*b-4*a*c;
if(discriminant < 0)
return false;
- BVH_REAL sqrt_dis = sqrt(discriminant);
+ FCL_REAL sqrt_dis = sqrt(discriminant);
- BVH_REAL r1 = (-b+sqrt_dis) / (2 * a);
+ FCL_REAL r1 = (-b+sqrt_dis) / (2 * a);
bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false;
if(v1) return true;
- BVH_REAL r2 = (-b-sqrt_dis) / (2 * a);
+ FCL_REAL r2 = (-b-sqrt_dis) / (2 * a);
bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false;
return v2;
}
@@ -538,7 +538,7 @@ bool Intersect::solveSquare(BVH_REAL a, BVH_REAL b, BVH_REAL c,
*/
void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d)
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
{
Vec3f vavb = vb - va;
Vec3f vavc = vc - va;
@@ -560,7 +560,7 @@ void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec
void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c, BVH_REAL* d)
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
{
Vec3f vavb = vb - va;
Vec3f vcvd = vd - vc;
@@ -582,7 +582,7 @@ void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec
void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
const Vec3f& L,
- BVH_REAL* a, BVH_REAL* b, BVH_REAL* c)
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c)
{
Vec3f vbva = va - vb;
Vec3f vbvp = vp - vb;
@@ -600,7 +600,7 @@ void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec
bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
*collision_time = 2.0;
@@ -610,7 +610,7 @@ bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
vb = b1 - b0;
vc = c1 - c0;
- BVH_REAL a, b, c, d;
+ FCL_REAL a, b, c, d;
computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d);
if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
@@ -626,13 +626,13 @@ bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
*/
- BVH_REAL coeffs[4];
+ FCL_REAL coeffs[4];
coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
if(useNewton)
{
- BVH_REAL l = 0;
- BVH_REAL r = 1;
+ FCL_REAL l = 0;
+ FCL_REAL r = 1;
if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs))
{
@@ -641,11 +641,11 @@ bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
}
else
{
- BVH_REAL roots[3];
+ FCL_REAL roots[3];
int num = PolySolver::solveCubic(coeffs, roots);
for(int i = 0; i < num; ++i)
{
- BVH_REAL r = roots[i];
+ FCL_REAL r = roots[i];
if(r < 0 || r > 1) continue;
if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r))
{
@@ -666,7 +666,7 @@ bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
*collision_time = 2.0;
@@ -676,7 +676,7 @@ bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
vc = c1 - c0;
vd = d1 - d0;
- BVH_REAL a, b, c, d;
+ FCL_REAL a, b, c, d;
computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d);
if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
@@ -690,13 +690,13 @@ bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
}
*/
- BVH_REAL coeffs[4];
+ FCL_REAL coeffs[4];
coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
if(useNewton)
{
- BVH_REAL l = 0;
- BVH_REAL r = 1;
+ FCL_REAL l = 0;
+ FCL_REAL r = 1;
if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i))
{
@@ -705,11 +705,11 @@ bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
}
else
{
- BVH_REAL roots[3];
+ FCL_REAL roots[3];
int num = PolySolver::solveCubic(coeffs, roots);
for(int i = 0; i < num; ++i)
{
- BVH_REAL r = roots[i];
+ FCL_REAL r = roots[i];
if(r < 0 || r > 1) continue;
if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i))
@@ -738,7 +738,7 @@ bool Intersect::intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
vb = b1 - b0;
vp = p1 - p0;
- BVH_REAL a, b, c;
+ FCL_REAL a, b, c;
computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c);
if(isZero(a) && isZero(b) && isZero(c))
@@ -764,12 +764,12 @@ bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Ve
Vec3f a0d0 = d0 - a0;
Vec3f a1d1 = d1 - a1;
- BVH_REAL A = n0.dot(a0d0);
- BVH_REAL B = n1.dot(a1d1);
- BVH_REAL C = nx.dot(a0d0);
- BVH_REAL D = nx.dot(a1d1);
- BVH_REAL E = n1.dot(a0d0);
- BVH_REAL F = n0.dot(a1d1);
+ FCL_REAL A = n0.dot(a0d0);
+ FCL_REAL B = n1.dot(a1d1);
+ FCL_REAL C = nx.dot(a0d0);
+ FCL_REAL D = nx.dot(a1d1);
+ FCL_REAL E = n1.dot(a0d0);
+ FCL_REAL F = n0.dot(a1d1);
if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0)
return false;
@@ -781,7 +781,7 @@ bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Ve
bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1))
{
@@ -793,7 +793,7 @@ bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Ve
bool Intersect::intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- BVH_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1))
{
@@ -808,7 +808,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points,
unsigned int* num_contact_points,
- BVH_REAL* penetration_depth,
+ FCL_REAL* penetration_depth,
Vec3f* normal)
{
Vec3f Q1_ = R * Q1 + T;
@@ -823,18 +823,18 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
Vec3f* contact_points,
unsigned int* num_contact_points,
- BVH_REAL* penetration_depth,
+ FCL_REAL* penetration_depth,
Vec3f* normal)
{
Vec3f n1;
- BVH_REAL t1;
+ FCL_REAL t1;
bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1);
if(!b1) return false;
Vec3f n2;
- BVH_REAL t2;
+ FCL_REAL t2;
bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
if(!b2) return false;
@@ -853,7 +853,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
unsigned int num_deepest_points1 = 0;
Vec3f deepest_points2[MAX_TRIANGLE_CLIPS];
unsigned int num_deepest_points2 = 0;
- BVH_REAL penetration_depth1 = -1, penetration_depth2 = -1;
+ FCL_REAL penetration_depth1 = -1, penetration_depth2 = -1;
clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2);
@@ -907,7 +907,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
Vec3f* contact_points,
unsigned int* num_contact_points,
- BVH_REAL* penetration_depth,
+ FCL_REAL* penetration_depth,
Vec3f* normal)
{
Vec3f p1 = P1 - P1;
@@ -977,7 +977,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
if(contact_points && num_contact_points && penetration_depth && normal)
{
Vec3f n1, n2;
- BVH_REAL t1, t2;
+ FCL_REAL t1, t2;
buildTrianglePlane(P1, P2, P3, &n1, &t1);
buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
@@ -985,7 +985,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
unsigned int num_deepest_points1 = 0;
Vec3f deepest_points2[3];
unsigned int num_deepest_points2 = 0;
- BVH_REAL penetration_depth1, penetration_depth2;
+ FCL_REAL penetration_depth1, penetration_depth2;
Vec3f P[3] = {P1, P2, P3};
Vec3f Q[3] = {Q1, Q2, Q3};
@@ -1023,10 +1023,10 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
#endif
-void Intersect::computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, BVH_REAL t, BVH_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points)
+void Intersect::computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points)
{
*num_deepest_points = 0;
- BVH_REAL max_depth = -std::numeric_limits<BVH_REAL>::max();
+ FCL_REAL max_depth = -std::numeric_limits<FCL_REAL>::max();
unsigned int num_deepest_points_ = 0;
unsigned int num_neg = 0;
unsigned int num_pos = 0;
@@ -1034,7 +1034,7 @@ void Intersect::computeDeepestPoints(Vec3f* clipped_points, unsigned int num_cli
for(unsigned int i = 0; i < num_clipped_points; ++i)
{
- BVH_REAL dist = -distanceToPlane(n, t, clipped_points[i]);
+ FCL_REAL dist = -distanceToPlane(n, t, clipped_points[i]);
if(dist > EPSILON) num_pos++;
else if(dist < -EPSILON) num_neg++;
else num_zero++;
@@ -1063,7 +1063,7 @@ void Intersect::computeDeepestPoints(Vec3f* clipped_points, unsigned int num_cli
void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
- const Vec3f& tn, BVH_REAL to,
+ const Vec3f& tn, FCL_REAL to,
Vec3f clipped_points[], unsigned int* num_clipped_points,
bool clip_triangle)
{
@@ -1075,7 +1075,7 @@ void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f
Vec3f v[3] = {v1, v2, v3};
Vec3f plane_n;
- BVH_REAL plane_dist;
+ FCL_REAL plane_dist;
if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist))
{
@@ -1109,7 +1109,7 @@ void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f
}
}
-void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, BVH_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points)
+void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points)
{
*num_clipped_points = 0;
@@ -1120,7 +1120,7 @@ void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polyg
for(unsigned int i = 0; i <= num_polygon_points; ++i)
{
vi = (i % num_polygon_points);
- BVH_REAL d = distanceToPlane(n, t, polygon_points[i]);
+ FCL_REAL d = distanceToPlane(n, t, polygon_points[i]);
classify = ((d > EPSILON) ? 1 : 0);
if(classify == 0)
{
@@ -1191,20 +1191,20 @@ void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polyg
*num_clipped_points = num_clipped_points_;
}
-void Intersect::clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, BVH_REAL t, Vec3f* clipped_point)
+void Intersect::clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point)
{
- BVH_REAL dist1 = distanceToPlane(n, t, v1);
+ FCL_REAL dist1 = distanceToPlane(n, t, v1);
Vec3f tmp = v2 - v1;
- BVH_REAL dist2 = tmp.dot(n);
+ FCL_REAL dist2 = tmp.dot(n);
*clipped_point = tmp * (-dist1 / dist2) + v1;
}
-BVH_REAL Intersect::distanceToPlane(const Vec3f& n, BVH_REAL t, const Vec3f& v)
+FCL_REAL Intersect::distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v)
{
return n.dot(v) - t;
}
-bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, BVH_REAL* t)
+bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t)
{
Vec3f n_ = (v2 - v1).cross(v3 - v1);
if(n_.normalize())
@@ -1217,7 +1217,7 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f
return false;
}
-bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, BVH_REAL* t)
+bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t)
{
Vec3f n_ = (v2 - v1).cross(tn);
if(n_.normalize())
@@ -1230,11 +1230,11 @@ bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn
return false;
}
-bool Intersect::sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, BVH_REAL t)
+bool Intersect::sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t)
{
- BVH_REAL dist1 = distanceToPlane(n, t, v1);
- BVH_REAL dist2 = dist1 * distanceToPlane(n, t, v2);
- BVH_REAL dist3 = dist1 * distanceToPlane(n, t, v3);
+ FCL_REAL dist1 = distanceToPlane(n, t, v1);
+ FCL_REAL dist2 = dist1 * distanceToPlane(n, t, v2);
+ FCL_REAL dist3 = dist1 * distanceToPlane(n, t, v3);
if((dist2 > 0) && (dist3 > 0))
return true;
return false;
@@ -1244,19 +1244,19 @@ int Intersect::project6(const Vec3f& ax,
const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
const Vec3f& q1, const Vec3f& q2, const Vec3f& q3)
{
- BVH_REAL P1 = ax.dot(p1);
- BVH_REAL P2 = ax.dot(p2);
- BVH_REAL P3 = ax.dot(p3);
- BVH_REAL Q1 = ax.dot(q1);
- BVH_REAL Q2 = ax.dot(q2);
- BVH_REAL Q3 = ax.dot(q3);
-
- BVH_REAL mn1 = std::min(P1, std::min(P2, P3));
- BVH_REAL mx2 = std::max(Q1, std::max(Q2, Q3));
+ FCL_REAL P1 = ax.dot(p1);
+ FCL_REAL P2 = ax.dot(p2);
+ FCL_REAL P3 = ax.dot(p3);
+ FCL_REAL Q1 = ax.dot(q1);
+ FCL_REAL Q2 = ax.dot(q2);
+ FCL_REAL Q3 = ax.dot(q3);
+
+ FCL_REAL mn1 = std::min(P1, std::min(P2, P3));
+ FCL_REAL mx2 = std::max(Q1, std::max(Q2, Q3));
if(mn1 > mx2) return 0;
- BVH_REAL mx1 = std::max(P1, std::max(P2, P3));
- BVH_REAL mn2 = std::min(Q1, std::min(Q2, Q3));
+ FCL_REAL mx1 = std::max(P1, std::max(P2, P3));
+ FCL_REAL mn2 = std::min(Q1, std::min(Q2, Q3));
if(mn2 > mx1) return 0;
return 1;
@@ -1319,7 +1319,7 @@ void Intersect::kernelGradient(KERNEL_PARM *kernel_parm, DOC *a, DOC *b, Vec3f&
}
}
-BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
const CloudClassifierParam& solver, bool scaling)
{
@@ -1422,7 +1422,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
}
}
- BVH_REAL S[3];
+ FCL_REAL S[3];
S[0] = 1 / (bbmax[0] - bbmin[0]);
S[1] = 1 / (bbmax[1] - bbmin[1]);
S[2] = 1 / (bbmax[2] - bbmin[2]);
@@ -1431,7 +1431,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
{
for(int i = 0; i < totdoc; ++i)
{
- BVH_REAL f = docs[i]->fvec->words[0].weight;
+ FCL_REAL f = docs[i]->fvec->words[0].weight;
docs[i]->fvec->words[0].weight = (f - bbmin[0]) * S[0];
f = docs[i]->fvec->words[1].weight;
docs[i]->fvec->words[1].weight = (f - bbmin[1]) * S[1];
@@ -1474,14 +1474,14 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
double sigma = uc1[i].Sigma.quadraticForm(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
+ FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
else
{
double sigma = uc2[i - nPositiveExamples].Sigma.quadraticForm(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
+ FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
@@ -1506,7 +1506,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
return max_collision_prob;
}
-BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling)
{
@@ -1610,7 +1610,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
}
}
- BVH_REAL S[3];
+ FCL_REAL S[3];
S[0] = 1 / (bbmax[0] - bbmin[0]);
S[1] = 1 / (bbmax[1] - bbmin[1]);
S[2] = 1 / (bbmax[2] - bbmin[2]);
@@ -1619,7 +1619,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
{
for(int i = 0; i < totdoc; ++i)
{
- BVH_REAL f = docs[i]->fvec->words[0].weight;
+ FCL_REAL f = docs[i]->fvec->words[0].weight;
docs[i]->fvec->words[0].weight = (f - bbmin[0]) * S[0];
f = docs[i]->fvec->words[1].weight;
docs[i]->fvec->words[1].weight = (f - bbmin[1]) * S[1];
@@ -1662,7 +1662,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
if (i < nPositiveExamples)
{
double sigma = uc1[i].Sigma.quadraticForm(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
+ FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
@@ -1670,7 +1670,7 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
{
Matrix3f rotatedSigma = R.tensorTransform(uc2[i - nPositiveExamples].Sigma);
double sigma = rotatedSigma.quadraticForm(fgrad);
- BVH_REAL col_prob = gaussianCDF(f / sqrt(sigma));
+ FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
if(max_collision_prob < col_prob)
max_collision_prob = col_prob;
}
@@ -1696,11 +1696,11 @@ BVH_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
}
-BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3)
{
// get the plane x * n - t = 0 and the compute the projection matrix according to (I - nn^t) y + t * n = y'
- BVH_REAL t;
+ FCL_REAL t;
Vec3f n;
bool b_plane = buildTrianglePlane(Q1, Q2, Q3, &n, &t);
if(!b_plane)
@@ -1709,7 +1709,7 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
return 0.0;
}
- BVH_REAL edge_t[3];
+ FCL_REAL edge_t[3];
Vec3f edge_n[3];
bool b_edge_plane1 = buildEdgePlane(Q1, Q2, n, edge_n + 0, edge_t + 0);
@@ -1739,7 +1739,7 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
Vec3f delta = n * t;
- BVH_REAL max_prob = 0;
+ FCL_REAL max_prob = 0;
for(int i = 0; i < size_cloud1; ++i)
{
Vec3f projected_p = P * cloud1[i] + delta;
@@ -1753,17 +1753,17 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(b_inside)
{
- BVH_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(newS.quadraticForm(edge_n[0])));
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(newS.quadraticForm(edge_n[1])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(newS.quadraticForm(edge_n[2])));
- BVH_REAL prob = 1.0 - prob1 - prob2 - prob3;
+ FCL_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(newS.quadraticForm(edge_n[0])));
+ FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(newS.quadraticForm(edge_n[1])));
+ FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(newS.quadraticForm(edge_n[2])));
+ FCL_REAL prob = 1.0 - prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
}
else
{
- BVH_REAL d1 = projected_p.dot(edge_n[0]) - edge_t[0];
- BVH_REAL d2 = projected_p.dot(edge_n[1]) - edge_t[1];
- BVH_REAL d3 = projected_p.dot(edge_n[2]) - edge_t[2];
+ FCL_REAL d1 = projected_p.dot(edge_n[0]) - edge_t[0];
+ FCL_REAL d2 = projected_p.dot(edge_n[1]) - edge_t[1];
+ FCL_REAL d3 = projected_p.dot(edge_n[2]) - edge_t[2];
std::vector<int> pos_plane;
std::vector<int> neg_plane;
@@ -1774,29 +1774,29 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
if(pos_plane.size() == 1)
{
int pos_id = pos_plane[0];
- BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(newS.quadraticForm(edge_n[pos_id])));
+ FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(newS.quadraticForm(edge_n[pos_id])));
int neg_id1 = neg_plane[0];
int neg_id2 = neg_plane[1];
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
+ FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
+ FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(newS.quadraticForm(edge_n[neg_id2])));
- BVH_REAL prob = prob1 - prob2 - prob3;
+ FCL_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
}
else if(pos_plane.size() == 2)
{
int neg_id = neg_plane[0];
- BVH_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(newS.quadraticForm(edge_n[neg_id])));
+ FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(newS.quadraticForm(edge_n[neg_id])));
int pos_id1 = pos_plane[0];
int pos_id2 = pos_plane[1];
- BVH_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(newS.quadraticForm(edge_n[pos_id1])));
- BVH_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(newS.quadraticForm(edge_n[pos_id2])));
+ FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(newS.quadraticForm(edge_n[pos_id1])));
+ FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(newS.quadraticForm(edge_n[pos_id2])));
- BVH_REAL prob = prob1 - prob2 - prob3;
+ FCL_REAL prob = prob1 - prob2 - prob3;
if(prob > max_prob) max_prob = prob;
}
else
@@ -1810,7 +1810,7 @@ BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
}
-BVH_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
const Matrix3f& R, const Vec3f& T)
{
@@ -1828,7 +1828,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
Vec3f& VEC, Vec3f& X, Vec3f& Y)
{
Vec3f T;
- BVH_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
+ FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
Vec3f TMP;
T = Q - P;
@@ -1841,12 +1841,12 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
// t parameterizes ray P,A
// u parameterizes ray Q,B
- BVH_REAL t, u;
+ FCL_REAL t, u;
// compute t for the closest point on ray P,A to
// ray Q,B
- BVH_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
+ FCL_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
@@ -1939,7 +1939,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
}
-BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q)
+FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q)
{
// Compute vectors along the 6 sides
@@ -1964,7 +1964,7 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
// points found, and whether the triangles were shown disjoint
Vec3f V, Z, minP, minQ;
- BVH_REAL mindd;
+ FCL_REAL mindd;
int shown_disjoint = 0;
mindd = (S[0] - T[0]).sqrLength() + 1; // Set first minimum safely high
@@ -1978,7 +1978,7 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q);
V = Q - P;
- BVH_REAL dd = V.dot(V);
+ FCL_REAL dd = V.dot(V);
// Verify this closest point pair only if the distance
// squared is less than the minimum found thus far.
@@ -1990,13 +1990,13 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
mindd = dd;
Z = S[(i+2)%3] - P;
- BVH_REAL a = Z.dot(VEC);
+ FCL_REAL a = Z.dot(VEC);
Z = T[(j+2)%3] - Q;
- BVH_REAL b = Z.dot(VEC);
+ FCL_REAL b = Z.dot(VEC);
if((a <= 0) && (b >= 0)) return sqrt(dd);
- BVH_REAL p = V.dot(VEC);
+ FCL_REAL p = V.dot(VEC);
if(a < 0) a = 0;
if(b > 0) b = 0;
@@ -2022,7 +2022,7 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
// First check for case 1
Vec3f Sn;
- BVH_REAL Snl;
+ FCL_REAL Snl;
Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle
Snl = Sn.dot(Sn); // Compute square of length of normal
@@ -2092,7 +2092,7 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
}
Vec3f Tn;
- BVH_REAL Tnl;
+ FCL_REAL Tnl;
Tn = Tv[0].cross(Tv[1]);
Tnl = Tn.dot(Tn);
@@ -2162,7 +2162,7 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f
}
-BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
Vec3f& P, Vec3f& Q)
{
@@ -2174,7 +2174,7 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const V
return triDistance(S, T, P, Q);
}
-BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
+FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q)
{
@@ -2186,7 +2186,7 @@ BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
return triDistance(S, T_transformed, P, Q);
}
-BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
+FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q)
diff --git a/trunk/fcl/src/interval.cpp b/trunk/fcl/src/interval.cpp
index 7cb9a3e0..e9bde63d 100644
--- a/trunk/fcl/src/interval.cpp
+++ b/trunk/fcl/src/interval.cpp
@@ -58,18 +58,18 @@ Interval Interval::operator * (const Interval& other) const
if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[1] * other.i_[1]);
if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[0] * other.i_[0]);
- BVH_REAL v00 = i_[0] * other.i_[0];
- BVH_REAL v11 = i_[1] * other.i_[1];
+ FCL_REAL v00 = i_[0] * other.i_[0];
+ FCL_REAL v11 = i_[1] * other.i_[1];
if(v00 <= v11)
{
- BVH_REAL v01 = i_[0] * other.i_[1];
- BVH_REAL v10 = i_[1] * other.i_[0];
+ FCL_REAL v01 = i_[0] * other.i_[1];
+ FCL_REAL v10 = i_[1] * other.i_[0];
if(v01 < v10) return Interval(v01, v11);
return Interval(v10, v11);
}
- BVH_REAL v01 = i_[0] * other.i_[1];
- BVH_REAL v10 = i_[1] * other.i_[0];
+ FCL_REAL v01 = i_[0] * other.i_[1];
+ FCL_REAL v10 = i_[1] * other.i_[0];
if(v01 < v10) return Interval(v01, v00);
return Interval(v10, v00);
}
diff --git a/trunk/fcl/src/interval_matrix.cpp b/trunk/fcl/src/interval_matrix.cpp
index 9390353b..896a5821 100644
--- a/trunk/fcl/src/interval_matrix.cpp
+++ b/trunk/fcl/src/interval_matrix.cpp
@@ -45,7 +45,7 @@ IMatrix3::IMatrix3()
i_[0][0] = i_[0][1] = i_[0][2] = i_[1][0] = i_[1][1] = i_[1][2] = i_[2][0] = i_[2][1] = i_[2][2] = 0;
}
-IMatrix3::IMatrix3(BVH_REAL v)
+IMatrix3::IMatrix3(FCL_REAL v)
{
i_[0][0] = i_[0][1] = i_[0][2] = i_[1][0] = i_[1][1] = i_[1][2] = i_[2][0] = i_[2][1] = i_[2][2] = v;
}
@@ -65,7 +65,7 @@ IMatrix3::IMatrix3(const Matrix3f& m)
i_[2][2] = m[2][2];
}
-IMatrix3::IMatrix3(BVH_REAL m[3][3][2])
+IMatrix3::IMatrix3(FCL_REAL m[3][3][2])
{
i_[0][0].setValue(m[0][0][0], m[0][0][1]);
i_[0][1].setValue(m[0][1][0], m[0][1][1]);
@@ -80,7 +80,7 @@ IMatrix3::IMatrix3(BVH_REAL m[3][3][2])
i_[2][2].setValue(m[2][2][0], m[2][2][1]);
}
-IMatrix3::IMatrix3(BVH_REAL m[3][3])
+IMatrix3::IMatrix3(FCL_REAL m[3][3])
{
i_[0][0].setValue(m[0][0]);
i_[0][1].setValue(m[0][1]);
@@ -147,7 +147,7 @@ Vec3f IMatrix3::getRealRow(size_t i) const
IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
{
- BVH_REAL res[3][3][2];
+ FCL_REAL res[3][3][2];
if(m[0][0] < 0)
{
@@ -325,7 +325,7 @@ IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
IVector3 IMatrix3::operator * (const Vec3f& v) const
{
- BVH_REAL res[3][2];
+ FCL_REAL res[3][2];
if(v[0] < 0)
{
@@ -390,7 +390,7 @@ IVector3 IMatrix3::operator * (const Vec3f& v) const
IMatrix3 IMatrix3::nonIntervalAddMatrix(const IMatrix3& m) const
{
- BVH_REAL res[3][3];
+ FCL_REAL res[3][3];
res[0][0] = i_[0][0][0] + m.i_[0][0][0];
res[0][1] = i_[0][1][0] + m.i_[0][1][0];
@@ -409,8 +409,8 @@ IMatrix3 IMatrix3::nonIntervalAddMatrix(const IMatrix3& m) const
IVector3 IMatrix3::operator * (const IVector3& v) const
{
- BVH_REAL xl, xu, yl, yu, zl, zu;
- register BVH_REAL temp, vmin, vmax;
+ FCL_REAL xl, xu, yl, yu, zl, zu;
+ register FCL_REAL temp, vmin, vmax;
// r.v.i_[0]
vmin = vmax = i_[0][0][0] * v.i_[0][0];
@@ -532,8 +532,8 @@ IVector3 IMatrix3::operator * (const IVector3& v) const
IMatrix3 IMatrix3::operator * (const IMatrix3& m) const
{
- register BVH_REAL temp, vmin, vmax;
- BVH_REAL res[3][3][2];
+ register FCL_REAL temp, vmin, vmax;
+ FCL_REAL res[3][3][2];
// res[0][0]
@@ -720,7 +720,7 @@ IMatrix3 IMatrix3::operator * (const IMatrix3& m) const
IMatrix3 IMatrix3::operator + (const IMatrix3& m) const
{
- BVH_REAL res[3][3][2];
+ FCL_REAL res[3][3][2];
res[0][0][0] = i_[0][0][0] + m.i_[0][0][0]; res[0][0][1] = i_[0][0][1] + m.i_[0][0][1]; res[0][1][0] = i_[0][1][0] + m.i_[0][1][0]; res[0][1][1] = i_[0][1][1] + m.i_[0][1][1]; res[0][2][0] = i_[0][2][0] + m.i_[0][2][0]; res[0][2][1] = i_[0][2][1] + m.i_[0][2][1];
res[1][0][0] = i_[1][0][0] + m.i_[1][0][0]; res[1][0][1] = i_[1][0][1] + m.i_[1][0][1]; res[1][1][0] = i_[1][1][0] + m.i_[1][1][0]; res[1][1][1] = i_[1][1][1] + m.i_[1][1][1]; res[1][2][0] = i_[1][2][0] + m.i_[1][2][0]; res[1][2][1] = i_[1][2][1] + m.i_[1][2][1];
res[2][0][0] = i_[2][0][0] + m.i_[2][0][0]; res[2][0][1] = i_[2][0][1] + m.i_[2][0][1]; res[2][1][0] = i_[2][1][0] + m.i_[2][1][0]; res[2][1][1] = i_[2][1][1] + m.i_[2][1][1]; res[2][2][0] = i_[2][2][0] + m.i_[2][2][0]; res[2][2][1] = i_[2][2][1] + m.i_[2][2][1];
diff --git a/trunk/fcl/src/interval_vector.cpp b/trunk/fcl/src/interval_vector.cpp
index 5074850d..78f7ebe6 100644
--- a/trunk/fcl/src/interval_vector.cpp
+++ b/trunk/fcl/src/interval_vector.cpp
@@ -43,23 +43,23 @@ namespace fcl
IVector3::IVector3() {}
-IVector3::IVector3(BVH_REAL v) { i_[0] = i_[1] = i_[2] = v; }
+IVector3::IVector3(FCL_REAL v) { i_[0] = i_[1] = i_[2] = v; }
-IVector3::IVector3(BVH_REAL x, BVH_REAL y, BVH_REAL z)
+IVector3::IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z)
{
i_[0].setValue(x);
i_[1].setValue(y);
i_[2].setValue(z);
}
-IVector3::IVector3(BVH_REAL xl, BVH_REAL xu, BVH_REAL yl, BVH_REAL yu, BVH_REAL zl, BVH_REAL zu)
+IVector3::IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu)
{
i_[0].setValue(xl, xu);
i_[1].setValue(yl, yu);
i_[2].setValue(zl, zu);
}
-IVector3::IVector3(BVH_REAL v[3][2])
+IVector3::IVector3(FCL_REAL v[3][2])
{
i_[0].setValue(v[0][0], v[0][1]);
i_[1].setValue(v[1][0], v[1][1]);
@@ -142,7 +142,7 @@ IVector3 IVector3::cross(const IVector3& other) const
return IVector3(res);
}
-BVH_REAL IVector3::volumn() const
+FCL_REAL IVector3::volumn() const
{
return i_[0].diameter() * i_[1].diameter() * i_[2].diameter();
}
diff --git a/trunk/fcl/src/matrix_3f.cpp b/trunk/fcl/src/matrix_3f.cpp
index 0c674991..93cfefb3 100644
--- a/trunk/fcl/src/matrix_3f.cpp
+++ b/trunk/fcl/src/matrix_3f.cpp
@@ -50,7 +50,7 @@ Matrix3f& Matrix3f::operator *= (const Matrix3f& other)
return *this;
}
-Matrix3f& Matrix3f::operator += (BVH_REAL c)
+Matrix3f& Matrix3f::operator += (FCL_REAL c)
{
setValue(v_[0][0] + c, v_[0][1] + c, v_[0][2] + c,
v_[1][0] + c, v_[1][1] + c, v_[1][2] + c,
@@ -59,7 +59,7 @@ Matrix3f& Matrix3f::operator += (BVH_REAL c)
}
-BVH_REAL Matrix3f::determinant() const
+FCL_REAL Matrix3f::determinant() const
{
return triple(v_[0], v_[1], v_[2]);
}
@@ -73,8 +73,8 @@ Matrix3f Matrix3f::transpose() const
Matrix3f Matrix3f::inverse() const
{
- BVH_REAL det = determinant();
- BVH_REAL inv_det = 1.0 / det;
+ FCL_REAL det = determinant();
+ FCL_REAL inv_det = 1.0 / det;
return Matrix3f((v_[1][1] * v_[2][2] - v_[1][2] * v_[2][1]) * inv_det,
(v_[0][2] * v_[2][1] - v_[0][1] * v_[2][2]) * inv_det,
@@ -139,17 +139,17 @@ void relativeTransform(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2,
T = R1.transposeTimes(temp);
}
-void matEigen(const Matrix3f& m, BVH_REAL dout[3], Vec3f vout[3])
+void matEigen(const Matrix3f& m, FCL_REAL dout[3], Vec3f vout[3])
{
Matrix3f R(m);
int n = 3;
int j, iq, ip, i;
- BVH_REAL tresh, theta, tau, t, sm, s, h, g, c;
+ FCL_REAL tresh, theta, tau, t, sm, s, h, g, c;
int nrot;
- BVH_REAL b[3];
- BVH_REAL z[3];
- BVH_REAL v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- BVH_REAL d[3];
+ FCL_REAL b[3];
+ FCL_REAL z[3];
+ FCL_REAL v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ FCL_REAL d[3];
for(ip = 0; ip < n; ++ip)
{
diff --git a/trunk/fcl/src/motion.cpp b/trunk/fcl/src/motion.cpp
index 09adcdb4..af50a3cd 100644
--- a/trunk/fcl/src/motion.cpp
+++ b/trunk/fcl/src/motion.cpp
@@ -41,10 +41,10 @@ namespace fcl
{
template<>
-BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
+FCL_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
{
- BVH_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
- BVH_REAL tmp;
+ FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
+ FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
@@ -54,18 +54,18 @@ BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) co
c_proj_max = sqrt(c_proj_max);
- BVH_REAL v_dot_n = linear_vel.dot(n);
- BVH_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- BVH_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
+ FCL_REAL v_dot_n = linear_vel.dot(n);
+ FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
+ FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
return mu;
}
template<>
-BVH_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
+FCL_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
{
- BVH_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
- BVH_REAL tmp;
+ FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
+ FCL_REAL tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
@@ -75,28 +75,28 @@ BVH_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) con
c_proj_max = sqrt(c_proj_max);
- BVH_REAL v_dot_n = axis.dot(n) * linear_vel;
- BVH_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
- BVH_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
+ FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
+ FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
+ FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();
- BVH_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
+ FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
return mu;
}
template<>
-BVH_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
+FCL_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
{
- BVH_REAL T_bound = computeTBound(n);
+ FCL_REAL T_bound = computeTBound(n);
Vec3f c1 = bv.Tr;
Vec3f c2 = bv.Tr + bv.axis[0] * bv.l[0];
Vec3f c3 = bv.Tr + bv.axis[1] * bv.l[1];
Vec3f c4 = bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1];
- BVH_REAL tmp;
+ FCL_REAL tmp;
// max_i |c_i * n|
- BVH_REAL cn_max = fabs(c1.dot(n));
+ FCL_REAL cn_max = fabs(c1.dot(n));
tmp = fabs(c2.dot(n));
if(tmp > cn_max) cn_max = tmp;
tmp = fabs(c3.dot(n));
@@ -105,7 +105,7 @@ BVH_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) co
if(tmp > cn_max) cn_max = tmp;
// max_i ||c_i||
- BVH_REAL cmax = c1.sqrLength();
+ FCL_REAL cmax = c1.sqrLength();
tmp = c2.sqrLength();
if(tmp > cmax) cmax = tmp;
tmp = c3.sqrLength();
@@ -115,7 +115,7 @@ BVH_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) co
cmax = sqrt(cmax);
// max_i ||c_i x n||
- BVH_REAL cxn_max = (c1.cross(n)).sqrLength();
+ FCL_REAL cxn_max = (c1.cross(n)).sqrLength();
tmp = (c2.cross(n)).sqrLength();
if(tmp > cxn_max) cxn_max = tmp;
tmp = (c3.cross(n)).sqrLength();
@@ -124,10 +124,10 @@ BVH_REAL SplineMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) co
if(tmp > cxn_max) cxn_max = tmp;
cxn_max = sqrt(cxn_max);
- BVH_REAL dWdW_max = computeDWMax();
- BVH_REAL ratio = std::min(1 - tf_t, dWdW_max);
+ FCL_REAL dWdW_max = computeDWMax();
+ FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
- BVH_REAL R_bound = 2 * (cn_max + cmax + cxn_max + 3 * bv.r) * ratio;
+ FCL_REAL R_bound = 2 * (cn_max + cmax + cxn_max + 3 * bv.r) * ratio;
// std::cout << R_bound << " " << T_bound << std::endl;
diff --git a/trunk/fcl/src/narrowphase.cpp b/trunk/fcl/src/narrowphase.cpp
index 160232fa..785f584f 100644
--- a/trunk/fcl/src/narrowphase.cpp
+++ b/trunk/fcl/src/narrowphase.cpp
@@ -46,10 +46,10 @@ namespace details
bool sphereSphereIntersect(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal)
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
- BVH_REAL len = diff.length();
+ FCL_REAL len = diff.length();
if(len > s1.radius + s2.radius)
return false;
@@ -72,10 +72,10 @@ bool sphereSphereIntersect(const Sphere& s1, const SimpleTransform& tf1,
bool sphereSphereDistance(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- BVH_REAL* dist)
+ FCL_REAL* dist)
{
Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
- BVH_REAL len = diff.length();
+ FCL_REAL len = diff.length();
if(len > s1.radius + s2.radius)
{
*dist = len - (s1.radius + s2.radius);
@@ -87,15 +87,15 @@ bool sphereSphereDistance(const Sphere& s1, const SimpleTransform& tf1,
}
/** \brief the minimum distance from a point to a line */
-BVH_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest)
+FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest)
{
Vec3f diff = p - from;
Vec3f v = to - from;
- BVH_REAL t = v.dot(diff);
+ FCL_REAL t = v.dot(diff);
if(t > 0)
{
- BVH_REAL dotVV = v.dot(v);
+ FCL_REAL dotVV = v.dot(v);
if(t < dotVV)
{
t /= dotVV;
@@ -129,7 +129,7 @@ bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const
Vec3f edge2_normal(edge2.cross(normal));
Vec3f edge3_normal(edge3.cross(normal));
- BVH_REAL r1, r2, r3;
+ FCL_REAL r1, r2, r3;
r1 = edge1_normal.dot(p1_to_p);
r2 = edge2_normal.dot(p2_to_p);
r3 = edge3_normal.dot(p3_to_p);
@@ -141,15 +141,15 @@ bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const
bool sphereTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal_)
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
{
Vec3f normal = (P2 - P1).cross(P3 - P1);
normal.normalize();
const Vec3f& center = tf.getTranslation();
- const BVH_REAL& radius = s.radius;
- BVH_REAL radius_with_threshold = radius + std::numeric_limits<BVH_REAL>::epsilon();
+ const FCL_REAL& radius = s.radius;
+ FCL_REAL radius_with_threshold = radius + std::numeric_limits<FCL_REAL>::epsilon();
Vec3f p1_to_center = center - P1;
- BVH_REAL distance_from_plane = p1_to_center.dot(normal);
+ FCL_REAL distance_from_plane = p1_to_center.dot(normal);
if(distance_from_plane < 0)
{
@@ -170,9 +170,9 @@ bool sphereTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
}
else
{
- BVH_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
+ FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
Vec3f nearest_on_edge;
- BVH_REAL distance_sqr;
+ FCL_REAL distance_sqr;
distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge);
if(distance_sqr < contact_capsule_radius_sqr)
{
@@ -199,20 +199,20 @@ bool sphereTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
if(has_contact)
{
Vec3f contact_to_center = center - contact_point;
- BVH_REAL distance_sqr = contact_to_center.sqrLength();
+ FCL_REAL distance_sqr = contact_to_center.sqrLength();
if(distance_sqr < radius_with_threshold * radius_with_threshold)
{
if(distance_sqr > 0)
{
- BVH_REAL distance = std::sqrt(distance_sqr);
+ FCL_REAL distance = std::sqrt(distance_sqr);
if(normal_) *normal_ = contact_to_center.normalized();
if(contact_points) *contact_points = contact_point;
if(penetration_depth) *penetration_depth = -(radius - distance);
}
else
{
- BVH_REAL distance = 0;
+ FCL_REAL distance = 0;
if(normal_) *normal_ = normal;
if(contact_points) *contact_points = contact_point;
if(penetration_depth) *penetration_depth = -radius;
@@ -227,26 +227,26 @@ bool sphereTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
bool sphereTriangleDistance(const Sphere& sp, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- BVH_REAL* dist)
+ FCL_REAL* dist)
{
// from geometric tools, very different from the collision code.
const Vec3f& center = tf.getTranslation();
- BVH_REAL radius = sp.radius;
+ FCL_REAL radius = sp.radius;
Vec3f diff = P1 - center;
Vec3f edge0 = P2 - P1;
Vec3f edge1 = P3 - P1;
- BVH_REAL a00 = edge0.sqrLength();
- BVH_REAL a01 = edge0.dot(edge1);
- BVH_REAL a11 = edge1.sqrLength();
- BVH_REAL b0 = diff.dot(edge0);
- BVH_REAL b1 = diff.dot(edge1);
- BVH_REAL c = diff.sqrLength();
- BVH_REAL det = fabs(a00*a11 - a01*a01);
- BVH_REAL s = a01*b1 - a11*b0;
- BVH_REAL t = a01*b0 - a00*b1;
-
- BVH_REAL sqr_dist;
+ FCL_REAL a00 = edge0.sqrLength();
+ FCL_REAL a01 = edge0.dot(edge1);
+ FCL_REAL a11 = edge1.sqrLength();
+ FCL_REAL b0 = diff.dot(edge0);
+ FCL_REAL b1 = diff.dot(edge1);
+ FCL_REAL c = diff.sqrLength();
+ FCL_REAL det = fabs(a00*a11 - a01*a01);
+ FCL_REAL s = a01*b1 - a11*b0;
+ FCL_REAL t = a01*b0 - a00*b1;
+
+ FCL_REAL sqr_dist;
if(s + t <= det)
{
@@ -330,7 +330,7 @@ bool sphereTriangleDistance(const Sphere& sp, const SimpleTransform& tf,
else // region 0
{
// minimum at interior point
- BVH_REAL inv_det = (1)/det;
+ FCL_REAL inv_det = (1)/det;
s *= inv_det;
t *= inv_det;
sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
@@ -338,7 +338,7 @@ bool sphereTriangleDistance(const Sphere& sp, const SimpleTransform& tf,
}
else
{
- BVH_REAL tmp0, tmp1, numer, denom;
+ FCL_REAL tmp0, tmp1, numer, denom;
if(s < 0) // region 2
{
@@ -472,21 +472,21 @@ struct ContactPoint
{
Vec3f normal;
Vec3f point;
- BVH_REAL depth;
- ContactPoint(const Vec3f& n, const Vec3f& p, BVH_REAL d) : normal(n), point(p), depth(d) {}
+ FCL_REAL depth;
+ ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {}
};
static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
const Vec3f& pb, const Vec3f& ub,
- BVH_REAL* alpha, BVH_REAL* beta)
+ FCL_REAL* alpha, FCL_REAL* beta)
{
Vec3f p = pb - pa;
- BVH_REAL uaub = ua.dot(ub);
- BVH_REAL q1 = ua.dot(p);
- BVH_REAL q2 = -ub.dot(p);
- BVH_REAL d = 1 - uaub * uaub;
- if(d <= (BVH_REAL)(0.0001f))
+ FCL_REAL uaub = ua.dot(ub);
+ FCL_REAL q1 = ua.dot(p);
+ FCL_REAL q2 = -ub.dot(p);
+ FCL_REAL d = 1 - uaub * uaub;
+ if(d <= (FCL_REAL)(0.0001f))
{
*alpha = 0;
*beta = 0;
@@ -506,22 +506,22 @@ static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
// the intersection points are returned as x,y pairs in the 'ret' array.
// the number of intersection points is returned by the function (this will
// be in the range 0 to 8).
-static int intersectRectQuad2(BVH_REAL h[2], BVH_REAL p[8], BVH_REAL ret[16])
+static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
{
// q (and r) contain nq (and nr) coordinate points for the current (and
// chopped) polygons
int nq = 4, nr = 0;
- BVH_REAL buffer[16];
- BVH_REAL* q = p;
- BVH_REAL* r = ret;
+ FCL_REAL buffer[16];
+ FCL_REAL* q = p;
+ FCL_REAL* r = ret;
for(int dir = 0; dir <= 1; ++dir)
{
// direction notation: xy[0] = x axis, xy[1] = y axis
for(int sign = -1; sign <= 1; sign += 2)
{
// chop q along the line xy[dir] = sign*h[dir]
- BVH_REAL* pq = q;
- BVH_REAL* pr = r;
+ FCL_REAL* pq = q;
+ FCL_REAL* pr = r;
nr = 0;
for(int i = nq; i > 0; --i)
{
@@ -539,7 +539,7 @@ static int intersectRectQuad2(BVH_REAL h[2], BVH_REAL p[8], BVH_REAL ret[16])
goto done;
}
}
- BVH_REAL* nextq = (i > 1) ? pq+2 : q;
+ FCL_REAL* nextq = (i > 1) ? pq+2 : q;
if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
{
// this line crosses the chopping line
@@ -563,7 +563,7 @@ static int intersectRectQuad2(BVH_REAL h[2], BVH_REAL p[8], BVH_REAL ret[16])
}
done:
- if(q != ret) memcpy(ret, q, nr*2*sizeof(BVH_REAL));
+ if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL));
return nr;
}
@@ -574,10 +574,10 @@ static int intersectRectQuad2(BVH_REAL h[2], BVH_REAL p[8], BVH_REAL ret[16])
// array iret (of size m). 'i0' is always the first entry in the array.
// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
// in the range [0..n-1].
-static inline void cullPoints2(int n, BVH_REAL p[], int m, int i0, int iret[])
+static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
- BVH_REAL a, cx, cy, q;
+ FCL_REAL a, cx, cy, q;
switch(n)
{
case 1:
@@ -600,7 +600,7 @@ static inline void cullPoints2(int n, BVH_REAL p[], int m, int i0, int iret[])
cy += q*(p[i*2+1]+p[i*2+3]);
}
q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
- if(std::abs(a+q) > std::numeric_limits<BVH_REAL>::epsilon())
+ if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
a = 1/(3*(a+q));
else
a= 1e18f;
@@ -611,7 +611,7 @@ static inline void cullPoints2(int n, BVH_REAL p[], int m, int i0, int iret[])
// compute the angle of each point w.r.t. the centroid
- BVH_REAL A[8];
+ FCL_REAL A[8];
for(int i = 0; i < n; ++i)
A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
@@ -621,12 +621,12 @@ static inline void cullPoints2(int n, BVH_REAL p[], int m, int i0, int iret[])
avail[i0] = 0;
iret[0] = i0;
iret++;
- const double pi = boost::math::constants::pi<BVH_REAL>();
+ const double pi = boost::math::constants::pi<FCL_REAL>();
for(int j = 1; j < m; ++j)
{
a = j*(2*pi/m) + A[i0];
if (a > pi) a -= 2*pi;
- BVH_REAL maxdiff= 1e9, diff;
+ FCL_REAL maxdiff= 1e9, diff;
*iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
for(int i = 0; i < n; ++i)
@@ -651,12 +651,12 @@ static inline void cullPoints2(int n, BVH_REAL p[], int m, int i0, int iret[])
int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2,
- Vec3f& normal, BVH_REAL* depth, int* return_code,
+ Vec3f& normal, FCL_REAL* depth, int* return_code,
int maxc, std::vector<ContactPoint>& contacts)
{
- const BVH_REAL fudge_factor = BVH_REAL(1.05);
+ const FCL_REAL fudge_factor = FCL_REAL(1.05);
Vec3f normalC;
- BVH_REAL s, s2, l;
+ FCL_REAL s, s2, l;
int invert_normal, code;
Vec3f p = T2 - T1; // get vector from centers of box 1 to box 2, relative to box 1
@@ -682,9 +682,9 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// the normal should be flipped.
int best_col_id = -1;
- BVH_REAL tmp = 0;
+ FCL_REAL tmp = 0;
- s = - std::numeric_limits<BVH_REAL>::max();
+ s = - std::numeric_limits<FCL_REAL>::max();
invert_normal = 0;
code = 0;
@@ -754,11 +754,11 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
- BVH_REAL fudge2(1.0e-6);
+ FCL_REAL fudge2(1.0e-6);
Q += fudge2;
Vec3f n;
- BVH_REAL eps = std::numeric_limits<BVH_REAL>::epsilon();
+ FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
// separating axis = u1 x (v1,v2,v3)
tmp = pp[2] * R[1][0] - pp[1] * R[2][0];
@@ -948,7 +948,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
Vec3f pa = T1;
- BVH_REAL sign;
+ FCL_REAL sign;
for(int j = 0; j < 3; ++j)
{
@@ -965,7 +965,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
pb += R2.getColumn(j) * (B[j] * sign);
}
- BVH_REAL alpha, beta;
+ FCL_REAL alpha, beta;
Vec3f ua, ub;
ua = R1.getColumn((code-7)/3);
ub = R2.getColumn((code-7)%3);
@@ -1085,8 +1085,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
// find the four corners of the incident face, in reference-face coordinates
- BVH_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
- BVH_REAL c1, c2, m11, m12, m21, m22;
+ FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
+ FCL_REAL c1, c2, m11, m12, m21, m22;
c1 = Ra->transposeDot(code1, center);
c2 = Ra->transposeDot(code2, center);
// optimize this? - we have already computed this data above, but it is not
@@ -1099,10 +1099,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
m21 = Rb->transposeDot(a1, tempRac);
m22 = Rb->transposeDot(a2, tempRac);
- BVH_REAL k1 = m11 * (*Sb)[a1];
- BVH_REAL k2 = m21 * (*Sb)[a1];
- BVH_REAL k3 = m12 * (*Sb)[a2];
- BVH_REAL k4 = m22 * (*Sb)[a2];
+ FCL_REAL k1 = m11 * (*Sb)[a1];
+ FCL_REAL k2 = m21 * (*Sb)[a1];
+ FCL_REAL k3 = m12 * (*Sb)[a2];
+ FCL_REAL k4 = m22 * (*Sb)[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
@@ -1113,12 +1113,12 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
quad[7] = c2 + k2 - k4;
// find the size of the reference face
- BVH_REAL rect[2];
+ FCL_REAL rect[2];
rect[0] = (*Sa)[code1];
rect[1] = (*Sa)[code2];
// intersect the incident and reference faces
- BVH_REAL ret[16];
+ FCL_REAL ret[16];
int n_intersect = intersectRectQuad2(rect, quad, ret);
if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen
@@ -1127,8 +1127,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
Vec3f points[8]; // penetrating contact points
- BVH_REAL dep[8]; // depths for those points
- BVH_REAL det1 = 1.f/(m11*m22 - m12*m21);
+ FCL_REAL dep[8]; // depths for those points
+ FCL_REAL det1 = 1.f/(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
@@ -1136,8 +1136,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
int cnum = 0; // number of penetrating contact points found
for(int j = 0; j < n_intersect; ++j)
{
- BVH_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
- BVH_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
+ FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
+ FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
points[cnum] = center + Rb->getColumn(a1) * k1 + Rb->getColumn(a2) * k2;
dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
if(dep[cnum] >= 0)
@@ -1179,7 +1179,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
- BVH_REAL maxdepth = dep[0];
+ FCL_REAL maxdepth = dep[0];
for(int i = 1; i < cnum; ++i)
{
if(dep[i] > maxdepth)
@@ -1211,12 +1211,12 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
bool boxBoxIntersect(const Box& s1, const SimpleTransform& tf1,
const Box& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth_, Vec3f* normal_)
+ Vec3f* contact_points, FCL_REAL* penetration_depth_, Vec3f* normal_)
{
std::vector<ContactPoint> contacts;
int return_code;
Vec3f normal;
- BVH_REAL depth;
+ FCL_REAL depth;
int cnum = boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(),
s2.side, tf2.getRotation(), tf2.getTranslation(),
normal, &depth, &return_code,
@@ -1233,7 +1233,7 @@ bool boxBoxIntersect(const Box& s1, const SimpleTransform& tf1,
contact_point += contacts[i].point;
}
- contact_point = contact_point / (BVH_REAL)contacts.size();
+ contact_point = contact_point / (FCL_REAL)contacts.size();
*contact_points = contact_point;
}
@@ -1250,21 +1250,21 @@ bool boxBoxIntersect(const Box& s1, const SimpleTransform& tf1,
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
}
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, contact_points, penetration_depth, normal);
}
@@ -1272,7 +1272,7 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTrans
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
}
@@ -1280,7 +1280,7 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Sim
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
}
@@ -1288,7 +1288,7 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Simp
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
return details::sphereTriangleDistance(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, dist);
}
@@ -1300,21 +1300,21 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Simp
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereSphereIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal);
}
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::sphereTriangleIntersect(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, contact_points, penetration_depth, normal);
}
@@ -1323,7 +1323,7 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const SimpleTransf
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
return details::sphereSphereDistance(s1, tf1, s2, tf2, dist);
}
@@ -1332,7 +1332,7 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Simp
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist);
}
@@ -1340,7 +1340,7 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Simpl
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
- BVH_REAL* dist) const
+ FCL_REAL* dist) const
{
return details::sphereTriangleDistance(s, tf, R * P1 + T, R * P2 + T, R * P3 + T, dist);
}
@@ -1349,7 +1349,7 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Simpl
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
const Box& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
@@ -1357,7 +1357,7 @@ bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const SimpleTrans
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
const Box& s2, const SimpleTransform& tf2,
- Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
}
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 6aa46a9d..613959fb 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -118,12 +118,12 @@ template<typename BV, typename OrientedNode>
static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
BVHModel<BV>& model1, const SimpleTransform& tf1,
BVHModel<BV>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
bool exhaustive,
bool enable_contact,
- BVH_REAL expand_r)
+ FCL_REAL expand_r)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
|| !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
@@ -162,12 +162,12 @@ static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
bool initialize(PointCloudCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
BVHModel<OBB>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
bool exhaustive,
bool enable_contact,
- BVH_REAL expand_r)
+ FCL_REAL expand_r)
{
return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
}
@@ -176,12 +176,12 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node,
bool initialize(PointCloudCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
BVHModel<RSS>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
bool exhaustive,
bool enable_contact,
- BVH_REAL expand_r)
+ FCL_REAL expand_r)
{
return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
}
@@ -193,12 +193,12 @@ template<typename BV, typename OrientedNode>
static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
BVHModel<BV>& model1, const SimpleTransform& tf1,
const BVHModel<BV>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
bool exhaustive,
bool enable_contact,
- BVH_REAL expand_r)
+ FCL_REAL expand_r)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -233,12 +233,12 @@ static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
BVHModel<OBB>& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
bool exhaustive,
bool enable_contact,
- BVH_REAL expand_r)
+ FCL_REAL expand_r)
{
return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
}
@@ -247,12 +247,12 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
int leaf_size_threshold,
int num_max_contacts,
bool exhaustive,
bool enable_contact,
- BVH_REAL expand_r)
+ FCL_REAL expand_r)
{
return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
}
diff --git a/trunk/fcl/src/taylor_matrix.cpp b/trunk/fcl/src/taylor_matrix.cpp
index 8d33939f..77801e08 100644
--- a/trunk/fcl/src/taylor_matrix.cpp
+++ b/trunk/fcl/src/taylor_matrix.cpp
@@ -115,7 +115,7 @@ TaylorModel& TMatrix3::operator () (size_t i, size_t j)
TMatrix3 TMatrix3::operator * (const Matrix3f& m) const
{
TaylorModel res[3][3];
- register BVH_REAL temp;
+ register FCL_REAL temp;
temp = m[0][0];
res[0][0].coeffs_[0] = i_[0][0].coeffs_[0] * temp; res[0][0].coeffs_[1] = i_[0][0].coeffs_[1] * temp; res[0][0].coeffs_[2] = i_[0][0].coeffs_[2] * temp; res[0][0].coeffs_[3] = i_[0][0].coeffs_[3] * temp; res[1][0].coeffs_[0] = i_[1][0].coeffs_[0] * temp; res[1][0].coeffs_[1] = i_[1][0].coeffs_[1] * temp; res[1][0].coeffs_[2] = i_[1][0].coeffs_[2] * temp; res[1][0].coeffs_[3] = i_[1][0].coeffs_[3] * temp; res[2][0].coeffs_[0] = i_[2][0].coeffs_[0] * temp; res[2][0].coeffs_[1] = i_[2][0].coeffs_[1] * temp; res[2][0].coeffs_[2] = i_[2][0].coeffs_[2] * temp; res[2][0].coeffs_[3] = i_[2][0].coeffs_[3] * temp;
@@ -224,7 +224,7 @@ TVector3 TMatrix3::operator * (const Vec3f& v) const
{
TaylorModel res[3];
- register BVH_REAL temp;
+ register FCL_REAL temp;
temp = v[0];
res[0].coeffs_[0] = i_[0][0].coeffs_[0] * temp; res[0].coeffs_[1] = i_[0][0].coeffs_[1] * temp; res[0].coeffs_[2] = i_[0][0].coeffs_[2] * temp; res[0].coeffs_[3] = i_[0][0].coeffs_[3] * temp; res[1].coeffs_[0] = i_[1][0].coeffs_[0] * temp; res[1].coeffs_[1] = i_[1][0].coeffs_[1] * temp; res[1].coeffs_[2] = i_[1][0].coeffs_[2] * temp; res[1].coeffs_[3] = i_[1][0].coeffs_[3] * temp; res[2].coeffs_[0] = i_[2][0].coeffs_[0] * temp; res[2].coeffs_[1] = i_[2][0].coeffs_[1] * temp; res[2].coeffs_[2] = i_[2][0].coeffs_[2] * temp; res[2].coeffs_[3] = i_[2][0].coeffs_[3] * temp;
@@ -354,12 +354,12 @@ IMatrix3 TMatrix3::getBound() const
return IMatrix3(res);
}
-BVH_REAL TMatrix3::diameter() const
+FCL_REAL TMatrix3::diameter() const
{
- BVH_REAL d = 0;
+ FCL_REAL d = 0;
- BVH_REAL tmp = i_[0][0].r_.diameter();
+ FCL_REAL tmp = i_[0][0].r_.diameter();
if(tmp > d) d = tmp;
tmp = i_[0][1].r_.diameter();
if(tmp > d) d = tmp;
diff --git a/trunk/fcl/src/taylor_model.cpp b/trunk/fcl/src/taylor_model.cpp
index d512f518..955ae050 100644
--- a/trunk/fcl/src/taylor_model.cpp
+++ b/trunk/fcl/src/taylor_model.cpp
@@ -42,17 +42,17 @@
namespace fcl
{
-const BVH_REAL TaylorModel::PI_ = 3.1415626535;
+const FCL_REAL TaylorModel::PI_ = 3.1415626535;
TaylorModel::TaylorModel() {}
-TaylorModel::TaylorModel(BVH_REAL coeff)
+TaylorModel::TaylorModel(FCL_REAL coeff)
{
coeffs_[0] = coeff;
coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0;
}
-TaylorModel::TaylorModel(BVH_REAL coeffs[3], const Interval& r)
+TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r)
{
coeffs_[0] = coeffs[0];
coeffs_[1] = coeffs[1];
@@ -62,7 +62,7 @@ TaylorModel::TaylorModel(BVH_REAL coeffs[3], const Interval& r)
r_ = r;
}
-TaylorModel::TaylorModel(BVH_REAL c0, BVH_REAL c1, BVH_REAL c2, BVH_REAL c3, const Interval& r)
+TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r)
{
coeffs_[0] = c0;
coeffs_[1] = c1;
@@ -72,7 +72,7 @@ TaylorModel::TaylorModel(BVH_REAL c0, BVH_REAL c1, BVH_REAL c2, BVH_REAL c3, con
r_ = r;
}
-void TaylorModel::setTimeInterval(BVH_REAL l, BVH_REAL r)
+void TaylorModel::setTimeInterval(FCL_REAL l, FCL_REAL r)
{
t_.setValue(l, r);
t2_.setValue(l * t_[0], r * t_[1]);
@@ -133,8 +133,8 @@ TaylorModel& TaylorModel::operator -= (const TaylorModel& other)
TaylorModel TaylorModel::operator * (const TaylorModel& other) const
{
assert(other.t_ == t_);
- register BVH_REAL c0, c1, c2, c3;
- register BVH_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];
+ register FCL_REAL c0, c1, c2, c3;
+ register FCL_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];
const Interval& rb = other.r_;
@@ -144,7 +144,7 @@ TaylorModel TaylorModel::operator * (const TaylorModel& other) const
c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b;
Interval remainder(r_ * rb);
- register BVH_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
+ register FCL_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
remainder += t4_ * tempVal;
tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
@@ -159,7 +159,7 @@ TaylorModel TaylorModel::operator * (const TaylorModel& other) const
return TaylorModel(c0, c1, c2, c3, remainder);
}
-TaylorModel TaylorModel::operator * (BVH_REAL d) const
+TaylorModel TaylorModel::operator * (FCL_REAL d) const
{
return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d);
}
@@ -175,12 +175,12 @@ void TaylorModel::print() const
std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl;
}
-Interval TaylorModel::getBound(BVH_REAL t) const
+Interval TaylorModel::getBound(FCL_REAL t) const
{
return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_;
}
-Interval TaylorModel::getBound(BVH_REAL t0, BVH_REAL t1) const
+Interval TaylorModel::getBound(FCL_REAL t0, FCL_REAL t1) const
{
Interval t(t0, t1);
Interval t2(t0 * t0, t1 * t1);
@@ -194,7 +194,7 @@ Interval TaylorModel::getBound() const
return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + t_ * coeffs_[1] + t2_ * coeffs_[2] + t3_ * coeffs_[3];
}
-Interval TaylorModel::getTightBound(BVH_REAL t0, BVH_REAL t1) const
+Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
{
if(t0 < t_[0]) t0 = t_[0];
@@ -202,17 +202,17 @@ Interval TaylorModel::getTightBound(BVH_REAL t0, BVH_REAL t1) const
if(coeffs_[3] == 0)
{
- register BVH_REAL a = -coeffs_[1] / (2 * coeffs_[2]);
+ register FCL_REAL a = -coeffs_[1] / (2 * coeffs_[2]);
Interval polybounds;
if(a <= t1 && a >= t0)
{
- BVH_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
- register BVH_REAL t = t0;
- BVH_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+ FCL_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
+ register FCL_REAL t = t0;
+ FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
t = t1;
- BVH_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+ FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
- BVH_REAL minQ = LQ, maxQ = RQ;
+ FCL_REAL minQ = LQ, maxQ = RQ;
if(LQ > RQ)
{
minQ = RQ;
@@ -226,10 +226,10 @@ Interval TaylorModel::getTightBound(BVH_REAL t0, BVH_REAL t1) const
}
else
{
- register BVH_REAL t = t0;
- BVH_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+ register FCL_REAL t = t0;
+ FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
t = t1;
- BVH_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
+ FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
if(LQ > RQ) polybounds.setValue(RQ, LQ);
else polybounds.setValue(LQ, RQ);
@@ -239,37 +239,37 @@ Interval TaylorModel::getTightBound(BVH_REAL t0, BVH_REAL t1) const
}
else
{
- register BVH_REAL t = t0;
- BVH_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
+ register FCL_REAL t = t0;
+ FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
t = t1;
- BVH_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
+ FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
if(LQ > RQ)
{
- BVH_REAL tmp = LQ;
+ FCL_REAL tmp = LQ;
LQ = RQ;
RQ = tmp;
}
// derivative: c1+2*c2*t+3*c3*t^2
- BVH_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
+ FCL_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
if(delta < 0)
return Interval(LQ, RQ) + r_;
- BVH_REAL r1=(-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
- BVH_REAL r2=(-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
+ FCL_REAL r1=(-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
+ FCL_REAL r2=(-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);
if(r1 <= t1 && r1 >= t0)
{
- BVH_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
+ FCL_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
if(Q < LQ) LQ = Q;
else if(Q > RQ) RQ = Q;
}
if(r2 <= t1 && r2 >= t0)
{
- BVH_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
+ FCL_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
if(Q < LQ) LQ = Q;
else if(Q > RQ) RQ = Q;
}
@@ -290,15 +290,15 @@ void TaylorModel::setZero()
}
-void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
+void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
{
- BVH_REAL a = tm.t_.center();
- BVH_REAL t = w * a + q0;
- BVH_REAL w2 = w * w;
- BVH_REAL fa = cos(t);
- BVH_REAL fda = -w*sin(t);
- BVH_REAL fdda = -w2*fa;
- BVH_REAL fddda = -w2*fda;
+ FCL_REAL a = tm.t_.center();
+ FCL_REAL t = w * a + q0;
+ FCL_REAL w2 = w * w;
+ FCL_REAL fa = cos(t);
+ FCL_REAL fda = -w*sin(t);
+ FCL_REAL fdda = -w2*fa;
+ FCL_REAL fddda = -w2*fda;
tm.coeffs_[0] = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
tm.coeffs_[1] = fda-a*fdda+0.5*a*a*fddda;
@@ -310,8 +310,8 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
if(w == 0) fddddBounds.setValue(0);
else
{
- BVH_REAL cosQL = cos(tm.t_[0] * w + q0);
- BVH_REAL cosQR = cos(tm.t_[1] * w + q0);
+ FCL_REAL cosQL = cos(tm.t_[0] * w + q0);
+ FCL_REAL cosQR = cos(tm.t_[1] * w + q0);
if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR);
else fddddBounds.setValue(cosQR, cosQL);
@@ -323,8 +323,8 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
// cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi];
// cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi]
- BVH_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_);
- BVH_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_);
+ FCL_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_);
+ FCL_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_);
if(w > 0)
@@ -343,12 +343,12 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
}
}
- BVH_REAL w4 = w2 * w2;
+ FCL_REAL w4 = w2 * w2;
fddddBounds *= w4;
- BVH_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
- BVH_REAL midSize2 = midSize * midSize;
- BVH_REAL midSize4 = midSize2 * midSize2;
+ FCL_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
+ FCL_REAL midSize2 = midSize * midSize;
+ FCL_REAL midSize4 = midSize2 * midSize2;
// [0, midSize4] * fdddBounds
if(fddddBounds[0] > 0)
@@ -359,15 +359,15 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
tm.r_.setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
}
-void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
+void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
{
- BVH_REAL a = tm.t_.center();
- BVH_REAL t = w * a + q0;
- BVH_REAL w2 = w * w;
- BVH_REAL fa = sin(t);
- BVH_REAL fda = w*cos(t);
- BVH_REAL fdda = -w2*fa;
- BVH_REAL fddda = -w2*fda;
+ FCL_REAL a = tm.t_.center();
+ FCL_REAL t = w * a + q0;
+ FCL_REAL w2 = w * w;
+ FCL_REAL fa = sin(t);
+ FCL_REAL fda = w*cos(t);
+ FCL_REAL fdda = -w2*fa;
+ FCL_REAL fddda = -w2*fda;
tm.coeffs_[0] = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
tm.coeffs_[1] = fda-a*fdda+0.5*a*a*fddda;
@@ -381,8 +381,8 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
if(w == 0) fddddBounds.setValue(0);
else
{
- BVH_REAL sinQL = sin(w * tm.t_[0] + q0);
- BVH_REAL sinQR = sin(w * tm.t_[1] + q0);
+ FCL_REAL sinQL = sin(w * tm.t_[0] + q0);
+ FCL_REAL sinQR = sin(w * tm.t_[1] + q0);
if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR);
else fddddBounds.setValue(sinQR, sinQL);
@@ -394,8 +394,8 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
// sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi];
// sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi]
- BVH_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
- BVH_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
+ FCL_REAL k1 = (tm.t_[0] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
+ FCL_REAL k2 = (tm.t_[1] * w + q0) / (2 * TaylorModel::PI_) - 0.25;
if(w > 0)
{
@@ -412,12 +412,12 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
}
- BVH_REAL w4 = w2 * w2;
+ FCL_REAL w4 = w2 * w2;
fddddBounds *= w4;
- BVH_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
- BVH_REAL midSize2 = midSize * midSize;
- BVH_REAL midSize4 = midSize2 * midSize2;
+ FCL_REAL midSize = 0.5 * (tm.t_[1] - tm.t_[0]);
+ FCL_REAL midSize2 = midSize * midSize;
+ FCL_REAL midSize4 = midSize2 * midSize2;
// [0, midSize4] * fdddBounds
if(fddddBounds[0] > 0)
@@ -429,7 +429,7 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, BVH_REAL w, BVH_REAL q0)
}
}
-void generateTaylorModelForLinearFunc(TaylorModel& tm, BVH_REAL p, BVH_REAL v)
+void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v)
{
tm.coeffs_[0] = p;
tm.coeffs_[1] = v;
diff --git a/trunk/fcl/src/taylor_vector.cpp b/trunk/fcl/src/taylor_vector.cpp
index d81393d8..9d8694c4 100644
--- a/trunk/fcl/src/taylor_vector.cpp
+++ b/trunk/fcl/src/taylor_vector.cpp
@@ -78,7 +78,7 @@ TVector3 TVector3::operator + (const TVector3& other) const
return TVector3(res);
}
-TVector3 TVector3::operator + (BVH_REAL d) const
+TVector3 TVector3::operator + (FCL_REAL d) const
{
TaylorModel res[3];
res[0] = i_[0];
@@ -147,7 +147,7 @@ TVector3 TVector3::cross(const TVector3& other) const
return TVector3(res);
}
-BVH_REAL TVector3::volumn() const
+FCL_REAL TVector3::volumn() const
{
return i_[0].getBound().diameter() * i_[1].getBound().diameter() * i_[2].getBound().diameter();
}
@@ -169,7 +169,7 @@ void TVector3::print() const
i_[2].print();
}
-IVector3 TVector3::getBound(BVH_REAL t) const
+IVector3 TVector3::getBound(FCL_REAL t) const
{
return IVector3(i_[0].getBound(t), i_[1].getBound(t), i_[2].getBound(t));
}
diff --git a/trunk/fcl/src/transform.cpp b/trunk/fcl/src/transform.cpp
index 06304322..68b9ecc7 100644
--- a/trunk/fcl/src/transform.cpp
+++ b/trunk/fcl/src/transform.cpp
@@ -44,8 +44,8 @@ void SimpleQuaternion::fromRotation(const Matrix3f& R)
{
const int next[3] = {1, 2, 0};
- BVH_REAL trace = R[0][0] + R[1][1] + R[2][2];
- BVH_REAL root;
+ FCL_REAL trace = R[0][0] + R[1][1] + R[2][2];
+ FCL_REAL root;
if(trace > 0.0)
{
@@ -73,7 +73,7 @@ void SimpleQuaternion::fromRotation(const Matrix3f& R)
int k = next[j];
root = sqrt(R[i][i] - R[j][j] - R[k][k] + 1.0);
- BVH_REAL* quat[3] = { &data[1], &data[2], &data[3] };
+ FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
*quat[i] = 0.5 * root;
root = 0.5 / root;
data[0] = (R[k][j] - R[j][k]) * root;
@@ -84,18 +84,18 @@ void SimpleQuaternion::fromRotation(const Matrix3f& R)
void SimpleQuaternion::toRotation(Matrix3f& R) const
{
- BVH_REAL twoX = 2.0*data[1];
- BVH_REAL twoY = 2.0*data[2];
- BVH_REAL twoZ = 2.0*data[3];
- BVH_REAL twoWX = twoX*data[0];
- BVH_REAL twoWY = twoY*data[0];
- BVH_REAL twoWZ = twoZ*data[0];
- BVH_REAL twoXX = twoX*data[1];
- BVH_REAL twoXY = twoY*data[1];
- BVH_REAL twoXZ = twoZ*data[1];
- BVH_REAL twoYY = twoY*data[2];
- BVH_REAL twoYZ = twoZ*data[2];
- BVH_REAL twoZZ = twoZ*data[3];
+ FCL_REAL twoX = 2.0*data[1];
+ FCL_REAL twoY = 2.0*data[2];
+ FCL_REAL twoZ = 2.0*data[3];
+ FCL_REAL twoWX = twoX*data[0];
+ FCL_REAL twoWY = twoY*data[0];
+ FCL_REAL twoWZ = twoZ*data[0];
+ FCL_REAL twoXX = twoX*data[1];
+ FCL_REAL twoXY = twoY*data[1];
+ FCL_REAL twoXZ = twoZ*data[1];
+ FCL_REAL twoYY = twoY*data[2];
+ FCL_REAL twoYZ = twoZ*data[2];
+ FCL_REAL twoZZ = twoZ*data[3];
R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
@@ -110,8 +110,8 @@ void SimpleQuaternion::fromAxes(const Vec3f axis[3])
const int next[3] = {1, 2, 0};
- BVH_REAL trace = axis[0][0] + axis[1][1] + axis[2][2];
- BVH_REAL root;
+ FCL_REAL trace = axis[0][0] + axis[1][1] + axis[2][2];
+ FCL_REAL root;
if(trace > 0.0)
{
@@ -139,7 +139,7 @@ void SimpleQuaternion::fromAxes(const Vec3f axis[3])
int k = next[j];
root = sqrt(axis[i][i] - axis[j][j] - axis[k][k] + 1.0);
- BVH_REAL* quat[3] = { &data[1], &data[2], &data[3] };
+ FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
*quat[i] = 0.5 * root;
root = 0.5 / root;
data[0] = (axis[j][k] - axis[k][j]) * root;
@@ -150,18 +150,18 @@ void SimpleQuaternion::fromAxes(const Vec3f axis[3])
void SimpleQuaternion::toAxes(Vec3f axis[3]) const
{
- BVH_REAL twoX = 2.0*data[1];
- BVH_REAL twoY = 2.0*data[2];
- BVH_REAL twoZ = 2.0*data[3];
- BVH_REAL twoWX = twoX*data[0];
- BVH_REAL twoWY = twoY*data[0];
- BVH_REAL twoWZ = twoZ*data[0];
- BVH_REAL twoXX = twoX*data[1];
- BVH_REAL twoXY = twoY*data[1];
- BVH_REAL twoXZ = twoZ*data[1];
- BVH_REAL twoYY = twoY*data[2];
- BVH_REAL twoYZ = twoZ*data[2];
- BVH_REAL twoZZ = twoZ*data[3];
+ FCL_REAL twoX = 2.0*data[1];
+ FCL_REAL twoY = 2.0*data[2];
+ FCL_REAL twoZ = 2.0*data[3];
+ FCL_REAL twoWX = twoX*data[0];
+ FCL_REAL twoWY = twoY*data[0];
+ FCL_REAL twoWZ = twoZ*data[0];
+ FCL_REAL twoXX = twoX*data[1];
+ FCL_REAL twoXY = twoY*data[1];
+ FCL_REAL twoXZ = twoZ*data[1];
+ FCL_REAL twoYY = twoY*data[2];
+ FCL_REAL twoYZ = twoZ*data[2];
+ FCL_REAL twoZZ = twoZ*data[3];
axis[0].setValue(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
axis[1].setValue(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX);
@@ -169,17 +169,17 @@ void SimpleQuaternion::toAxes(Vec3f axis[3]) const
}
-void SimpleQuaternion::fromAxisAngle(const Vec3f& axis, BVH_REAL angle)
+void SimpleQuaternion::fromAxisAngle(const Vec3f& axis, FCL_REAL angle)
{
- BVH_REAL half_angle = 0.5 * angle;
- BVH_REAL sn = sin((double)half_angle);
+ FCL_REAL half_angle = 0.5 * angle;
+ FCL_REAL sn = sin((double)half_angle);
data[0] = cos((double)half_angle);
data[1] = sn * axis[0];
data[2] = sn * axis[1];
data[3] = sn * axis[2];
}
-void SimpleQuaternion::toAxisAngle(Vec3f& axis, BVH_REAL& angle) const
+void SimpleQuaternion::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
{
double sqr_length = data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
if(sqr_length > 0)
@@ -199,7 +199,7 @@ void SimpleQuaternion::toAxisAngle(Vec3f& axis, BVH_REAL& angle) const
}
}
-BVH_REAL SimpleQuaternion::dot(const SimpleQuaternion& other) const
+FCL_REAL SimpleQuaternion::dot(const SimpleQuaternion& other) const
{
return data[0] * other.data[0] + data[1] * other.data[1] + data[2] * other.data[2] + data[3] * other.data[3];
}
@@ -247,10 +247,10 @@ SimpleQuaternion SimpleQuaternion::operator * (const SimpleQuaternion& other) co
const SimpleQuaternion& SimpleQuaternion::operator *= (const SimpleQuaternion& other)
{
- BVH_REAL a = data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3];
- BVH_REAL b = data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2];
- BVH_REAL c = data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1];
- BVH_REAL d = data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0];
+ FCL_REAL a = data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3];
+ FCL_REAL b = data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2];
+ FCL_REAL c = data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1];
+ FCL_REAL d = data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0];
data[0] = a;
data[1] = b;
@@ -264,12 +264,12 @@ SimpleQuaternion SimpleQuaternion::operator - () const
return SimpleQuaternion(-data[0], -data[1], -data[2], -data[3]);
}
-SimpleQuaternion SimpleQuaternion::operator * (BVH_REAL t) const
+SimpleQuaternion SimpleQuaternion::operator * (FCL_REAL t) const
{
return SimpleQuaternion(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
}
-const SimpleQuaternion& SimpleQuaternion::operator *= (BVH_REAL t)
+const SimpleQuaternion& SimpleQuaternion::operator *= (FCL_REAL t)
{
data[0] *= t;
data[1] *= t;
diff --git a/trunk/fcl/src/traversal_node_base.cpp b/trunk/fcl/src/traversal_node_base.cpp
index 1f8a3939..ebcb1a6a 100644
--- a/trunk/fcl/src/traversal_node_base.cpp
+++ b/trunk/fcl/src/traversal_node_base.cpp
@@ -103,16 +103,16 @@ DistanceTraversalNodeBase::~DistanceTraversalNodeBase()
{
}
-BVH_REAL DistanceTraversalNodeBase::BVTesting(int b1, int b2) const
+FCL_REAL DistanceTraversalNodeBase::BVTesting(int b1, int b2) const
{
- return std::numeric_limits<BVH_REAL>::max();
+ return std::numeric_limits<FCL_REAL>::max();
}
void DistanceTraversalNodeBase::leafTesting(int b1, int b2) const
{
}
-bool DistanceTraversalNodeBase::canStop(BVH_REAL c) const
+bool DistanceTraversalNodeBase::canStop(FCL_REAL c) const
{
return false;
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index ca74672a..f4d4af3a 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -73,7 +73,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
- BVH_REAL penetration;
+ FCL_REAL penetration;
Vec3f normal;
int n_contacts;
Vec3f contacts[2];
@@ -117,7 +117,7 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
Vec3f& p2,
int& last_tri_id1,
int& last_tri_id2,
- BVH_REAL& min_distance)
+ FCL_REAL& min_distance)
{
if(enable_statistics) num_leaf_tests++;
@@ -141,7 +141,7 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
// nearest point pair
Vec3f P1, P2;
- BVH_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
+ FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
R, T,
P1, P2);
@@ -288,11 +288,11 @@ static inline void pointCloudCollisionOrientedNodeLeafTesting(int b1, int b2,
Vec3f* vertices1, Vec3f* vertices2,
const Matrix3f& R, const Vec3f& T,
bool enable_statistics,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
const boost::shared_arry<Uncertainty>& uc1, const boost::shared_array<Uncertainty>& uc2,
const CloudClassifierParam classifier_param,
int& num_leaf_tests,
- BVH_REAL& max_collision_prob,
+ FCL_REAL& max_collision_prob,
std::vector<BVHPointCollisionPair>& pairs)
{
if(enable_statistics) num_leaf_tests++;
@@ -300,7 +300,7 @@ static inline void pointCloudCollisionOrientedNodeLeafTesting(int b1, int b2,
const BVNode<BV>& node1 = model1->getBV(b1);
const BVNode<BV>& node2 = model2->getBV(b2);
- BVH_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
+ FCL_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
node1.num_primitives,
vertices2 + node2.first_primitive, uc2.get() + node2.first_primitive,
node2.num_primitives,
@@ -378,10 +378,10 @@ static inline void pointCloudMeshCollisionOrientedNodeLeafTesting(int b1, int b2
Triangle* tri_indices2,
const Matrix3f& R, const Vec3f& T,
bool enable_statistics,
- BVH_REAL collision_prob_threshold,
+ FCL_REAL collision_prob_threshold,
const boost::shared_array<Uncertainty>& uc1,
int& num_leaf_tests,
- BVH_REAL& max_collision_prob,
+ FCL_REAL& max_collision_prob,
std::vector<BVHPointCollisionPair>& pairs)
{
if(enable_statistics) num_leaf_tests++;
@@ -396,7 +396,7 @@ static inline void pointCloudMeshCollisionOrientedNodeLeafTesting(int b1, int b2
const Vec3f& q2 = vertices2[tri_id2[1]];
const Vec3f& q3 = vertices2[tri_id2[2]];
- BVH_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
+ FCL_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
node1.num_primitives,
q1, q2, q3,
R, T);
@@ -466,7 +466,7 @@ static inline void distance_preprocess(Vec3f* vertices1, Vec3f* vertices2,
Triangle* tri_indices1, Triangle* tri_indices2,
int last_tri_id1, int last_tri_id2,
const Matrix3f& R, const Vec3f& T,
- BVH_REAL& min_distance,
+ FCL_REAL& min_distance,
Vec3f& p1,
Vec3f& p2)
{
@@ -516,7 +516,7 @@ void MeshDistanceTraversalNodeRSS::postprocess()
distance_postprocess(R, T, p2);
}
-BVH_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
+FCL_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
@@ -545,7 +545,7 @@ void MeshDistanceTraversalNodekIOS::postprocess()
distance_postprocess(R, T, p2);
}
-BVH_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
+FCL_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
@@ -574,7 +574,7 @@ void MeshDistanceTraversalNodeOBBRSS::postprocess()
distance_postprocess(R, T, p2);
}
-BVH_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
+FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
@@ -590,12 +590,12 @@ void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
/** for OBB and RSS, there is local coordinate of BV, so normal need to be transformed */
template<>
-bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
+bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
Vec3f n;
int c1, c2;
@@ -619,12 +619,12 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
- BVH_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
+ FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
- BVH_REAL bound = bound1 + bound2;
+ FCL_REAL bound = bound1 + bound2;
- BVH_REAL cur_delta_t;
+ FCL_REAL cur_delta_t;
if(bound <= c) cur_delta_t = 1;
else cur_delta_t = c / bound;
@@ -638,7 +638,7 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
else
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
if(d > c)
stack[stack.size() - 2] = stack[stack.size() - 1];
@@ -650,12 +650,12 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
}
template<>
-bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
+bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
Vec3f n;
int c1, c2;
@@ -679,12 +679,12 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
- BVH_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
+ FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
- BVH_REAL bound = bound1 + bound2;
+ FCL_REAL bound = bound1 + bound2;
- BVH_REAL cur_delta_t;
+ FCL_REAL cur_delta_t;
if(bound <= c) cur_delta_t = 1;
else cur_delta_t = c / bound;
@@ -698,7 +698,7 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
else
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
if(d > c)
stack[stack.size() - 2] = stack[stack.size() - 1];
@@ -710,17 +710,17 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
}
-MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(BVH_REAL w_) : MeshConservativeAdvancementTraversalNode<RSS>(w_)
+MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_) : MeshConservativeAdvancementTraversalNode<RSS>(w_)
{
R.setIdentity();
// default T is 0
}
-BVH_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
+FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
Vec3f P1, P2;
- BVH_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
+ FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
@@ -752,7 +752,7 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
// nearest point pair
Vec3f P1, P2;
- BVH_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
+ FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
R, T,
P1, P2);
@@ -775,12 +775,12 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
motion1->getCurrentRotation(R0);
Vec3f n_transformed = R0 * n;
n_transformed.normalize();
- BVH_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, -n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
+ FCL_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, -n_transformed);
- BVH_REAL bound = bound1 + bound2;
+ FCL_REAL bound = bound1 + bound2;
- BVH_REAL cur_delta_t;
+ FCL_REAL cur_delta_t;
if(bound <= d) cur_delta_t = 1;
else cur_delta_t = d / bound;
@@ -788,12 +788,12 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
delta_t = cur_delta_t;
}
-bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
+bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const
{
if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
Vec3f n;
int c1, c2;
@@ -822,12 +822,12 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
n_transformed = R0 * n_transformed;
n_transformed.normalize();
- BVH_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
+ FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
- BVH_REAL bound = bound1 + bound2;
+ FCL_REAL bound = bound1 + bound2;
- BVH_REAL cur_delta_t;
+ FCL_REAL cur_delta_t;
if(bound <= c) cur_delta_t = 1;
else cur_delta_t = c / bound;
@@ -841,7 +841,7 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
else
{
const ConservativeAdvancementStackData& data = stack.back();
- BVH_REAL d = data.d;
+ FCL_REAL d = data.d;
if(d > c)
stack[stack.size() - 2] = stack[stack.size() - 1];
diff --git a/trunk/fcl/src/traversal_recurse.cpp b/trunk/fcl/src/traversal_recurse.cpp
index ba876831..aa1343c5 100644
--- a/trunk/fcl/src/traversal_recurse.cpp
+++ b/trunk/fcl/src/traversal_recurse.cpp
@@ -222,8 +222,8 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontLi
c2 = node->getSecondRightChild(b2);
}
- BVH_REAL d1 = node->BVTesting(a1, a2);
- BVH_REAL d2 = node->BVTesting(c1, c2);
+ FCL_REAL d1 = node->BVTesting(a1, a2);
+ FCL_REAL d2 = node->BVTesting(c1, c2);
if(d2 < d1)
{
@@ -256,7 +256,7 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontLi
struct BVT
{
/** \brief distance between bvs */
- BVH_REAL d;
+ FCL_REAL d;
/** \brief bv indices for a pair of bvs in two models */
int b1, b2;
--
GitLab