diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index cdd6a3032b4810ffac8715489ba23744da437c77..248d2c1364d2f918066b9e2617bb4f5f2846a9cd 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 619902bb2bf1994d782573f944406eec32f2692f..a3978113408fe29fae070a434dd999f57b2f1442 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 d8d9050b0050eee5622b57433e887c729cff268d..7d9b42dbfa8db760c650669602b616a0d76d6331 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 83cf4281f4c9146312012d769354944e15153f3e..130fcf047770aeeace3610f7d1f2c0ffb7089d50 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 6c59b40fc2698519e0a3ea0deb9876e82e619383..474730dc4e35e81ce791607f96a9934345f69afc 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 8532702e83989fd3bf3f01648f3133b50c2688b2..99044ce09dbce9c5b36665cc201c2575abb52fcc 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 3aa681947180118b817b78792e8838cc41860147..958973747643d16b5636a4836127e3d8180561af 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 5dc2cdcf3e27fd18533b5193646d43449acedebf..059d94e5659679c4ad0482156001b578b719c739 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 bfafb8353072234a1eecaf3f10de252c977eb745..1341a6234b5baca685df5f4c06fa8ab5625173ba 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 192dfeb6df8f5ad9385bedc12aa8e3718aa1eb9a..d3e1025a16d991f9c05bd0ea4242f3187a779791 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 c1a2db2c9bf5556d301e54e0a16ee79e0d432a1d..b75bfd46f9f5182bb09038d1b3102c98e08369e5 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 f543be0a8a3ff55eccc132d7e768be60176ec85e..c22eec13e0f159798ea551699f1dde440f9293bf 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 9a2740852f90ffbea2042608ce6eeade7af001df..5b5229454c77e8ea2262a453f443aa7eae2602c8 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 9ef9f3225d051aa3025a988a0c68a9cc5808010e..df2fc0fd37266ae6c14a2f2f7286477af547f1a6 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 46dfe608052e2d8e70bea1869fc5cc611c7ea33c..6158e1fc1bf99d1ce1b2af5f930cdf9e424d7f3f 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 a6ed5b387243a38c1afde19db7bd611d0c03e1ad..78e5a51361702737ae9c84ae3122245c6d3d7d8e 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 595f298d28fac8526d188ec56cdb43e9e7be921b..b5946cb48ec91f1210cfe91c47e17400143bd973 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 52594d30b33c5516fb39d658e736bde27d0107a8..ead30c2d1ae30fc770725c5958ae95adaf327df1 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 5e5285b731e1e49b2cfbf1abe1065f085d31cff2..3a05c814659d5c1aa76207918add92f94a7bc349 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 6f9e05035352e2cbbe4d26f3df64f532c78e7768..f909505c5abfddcdad8a6ff630e9abc46d351ccf 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 0a317ab7bfe05a91b5f89089dc13a4a64fe58267..2757df853645123b7cf358e2c93fd208367825e3 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 cfe7b5234be207390e0b886fa9beb1983fb4faf8..76099e402b4632e020b7f6145b1cde2a0d10e3f6 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 87f3044216f7fb19eff452f33258a1627cfaf765..b2ae42eb25872da1a4112cc54752ed9eb4aa68e6 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 06eeb0d17cf02bc2e8a03b362a702e80ef6d62d8..fe6a3b0289e76459dfdcef77879bb28f5f8b3871 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 c7c4981e962cbcdaed79b6e616ef9fbc2a8aba1a..147ad61394ae35488e7cdb68ade46e866d88d4ad 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 dba1f789d920c632b1e14f6c327e27e9a747087c..2e3d3c61c24e2a190c9fd9610713423da5f831c4 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 ae29c78c1e8e2608f48c0bc9e59eb96cd5563f06..5726d369a4b2e02190902738a57917397ee56953 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 d71196d48beffe7a4c20676fe27f1557190881d3..37ea501d2108fc2e656a8f9db2542e257ea833eb 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 944e5aa45b093579cc8a14c2c15bb017b462aa43..4e6e3be2eb9003e6ebafd6691d252a3c2329ecc8 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 e8af515f202812c9379e41188afd7ff22276949b..357e52949cf7e6bdb0d6ec3aec790befcec5a2c7 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 1b4f9443429328ba9287d332cf6101cf0ef66e81..6a310d44eee98bcd79b4ef0748974e7bcb9ca7ae 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 fc38a1199ff01fe873e24eac3c42f4eaf34e66d9..1cacbdf93090a940b7fb1c9dd2f6a48932f728bf 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 cc2658dc3164b36dbbc2b24694a976cae7d0c182..dcd6dea6aa6b2caba2c8cfb00eeaa437b5de37e5 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 7f72db427f442435f9aa7f2788714b35275d4a28..6091684a23f7f7eee663c94dd081c95d6f70a3ac 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 0352506db559c025bee038fd29cdc3737e1fb690..9b25878b8c77305dca4905196072b36ac43d8364 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 79c90c9186b582b3b81dafbd38636c6fd25a9f04..d35d87df0a53f40a829e58705c9733e29fa52337 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 59bc0d3e813a48b589b3e3ef1aba809e90d360b0..1acb7e96a088911e3865271e490993a92a87479c 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 81674d8cc0aed144226aa730632c5619b9c007a5..ef545b2b5b1f07ccadc2c4deb435c0d8dc7b5d69 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 c7c414289062af77ee5ef763d4ad53851e91dcef..5d73c672fd809506a9bc0020e126c8d3ef21303b 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 c43a6a1ee760d473128a70904d8fa835414ee385..50a98e4485d3d6c5fee4d89b4ea25b0e9099c488 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 f72038ff54c70ddd36f8006724a7d70c91b81677..260af6db82116c6eea6e2f4e44fe7b827e1a24d0 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 18a7b8a00e008546ca866955480af202f7fa4dfa..6557ef168d5fda12daadea7a91d31486300af188 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 72782ba7d6a93e59f7394781c1aeb8cdbc6bf347..da72cc0ded989d989163eb5207eed8f065c6bfa2 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 bda78846cddd2f08dbd925346b0bbfadaca54709..f9df09ffb78d6dd7d3d6bc670e13b6e0af9ed5ad 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 8063fea9f60ade1614d61642df6d4216152a1970..e413ecc999be330daa674b33bcb13f075eff30f1 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 002f610496d0f69c572da12e97f056f838a66998..d4d202f006334f98580857149994514f0357e9ea 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 b645d52fabff6137f8d31c306e150daaa759ae87..ed3e0e92c998e7701da8a8757f1ca3a8a6bb1ae6 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 c2c619ba6707ac1b43960a6c7bdec7c519fcde49..ddd9a0322b1141a85fcbdc25b989bea7139656a8 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 c821df0523324d2cfadcd8da72f4a21bf256e2b0..5df9821b3c55fef7ebbae7e9f5d8147a6c84dd83 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 6c6253f6c32fc01e3779d1480c4d93c7bf7a1d38..31ff3cb0f9144348cbdf0d8874ec16f084df47fb 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 29d3677b6738fb2a806eb20bec88fcb51202355b..6e808cf0e944f435e5e4c6e5f19f6e339165c909 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 a323783da66f9313d061c9e72e685930afe8401c..060995e63f15a035737dc20cae5e5900e0579d6b 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 acfd7c7ffd8eb56005dcde15835419c466962a75..d7b3cf19f82a3fd65b52d4a2a7ef45e4969756c5 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 e491ec51cc6a3f93aae3b528f1a908c1266ae23a..c3e57ee0163a125ee79ec89fb520bb030de9cc4a 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 530bc1466a79a61f5b853d12d60e761e067c5bf9..f66a8bc9bd7698db1608d180358db8d611f18007 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 bc82b9b9bc1de00cc0f2a80bd51f271f13fb324d..a853aad886cf6322c402e394bccaea96379bd99a 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 2435cee8d6794758c3ed239fdf781593e50e5d7a..31dc75f1a8cd1b159c8749cef57712fd55755a80 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 e9cfdda42a3defe0b333be2cbbc72b5a1a52e94f..2a4f650203e92e97d345740e2c3bd7e1b12570de 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 504f3ab3a84df2d63320818d83c0651112f5b533..e79cd523865c100af017c6677116f3cd4b88145c 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 ecef677a45dd47ca13ba3d0074291de3afc271cb..c594a7f85c71059c9f4fbf730f9210a8b30aec57 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 4e52249c5044e5e8780fc5b7a7ddaefc9e2cbb49..09bdd1783c2f5b40b9c07ccf8099dd3d82326181 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 29f5fcea2de2bbf2c2e66bfd74251aa9aa41ac8f..f5daef36e6c31725d7dd23f70305f3cd043c3468 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 c5515e626da467284cae5a3c4efc8111c8eb3d5a..c445f59230c50569e98b19b962a9965094293a71 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 5a0f68584179bc9233bf68ebd848b77488424195..015ce41d245a0fa1fbae40d487c2a2cc69657822 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 436833b8b56bdf59957057ec583a9964ee13ca00..9185aeba4086c36eeb4f0912435bd8c08a8fb070 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 7cb9a3e0971442191cfc28164e52272ff119d803..e9bde63d1545c3c74f869f25e767b4d30fc3a223 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 9390353b8c2a8c8585e575562c8c9a7483b0d19e..896a5821c9131d6b6b38fcca1a1fd881a9d0a5b5 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 5074850d33e4aa422decf5f5bca1239a749faffb..78f7ebe6104b51c96306cac4c5ab1625fd1f1430 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 0c674991014cbf3962ea5b5dc84378e87e23ee6f..93cfefb3c51c8e517a85fc802552922c3cde3c8c 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 09adcdb4e37f608767fb1aaa0637de5cb65ab603..af50a3cd4356b5a5299bda50d35841bd3e24e2bf 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 160232fa1a6b87d328e8c511c7a250fc4a8e0d34..785f584f5c13715f885a3726e73f0c394076fc76 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 6aa46a9dfe9ac066c9b9c6ef1324a107732eb372..613959fb86dd6fd50cff55634925b64c0444c31c 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 8d33939fbd881f047ec1c18c425886c92349eb02..77801e08e012753898afc4a6952fb61e6ff77784 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 d512f5189fd8cfc438ca57073220f095fa71ebbf..955ae050bd3178db3207a8a7de3431b03df3ef5b 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 d81393d8032eb05e63c019905dc4fb0067986848..9d8694c4d7a93ebdbb72346e5ea7a8152f84f601 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 063043229cbb6eb208da84ca6ca4c65b0b043d86..68b9ecc7a421d0f661fe2d43a6ca7195ee641608 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 1f8a39391538176725a6ec96cf093b28d5f1b2d5..ebcb1a6a67aa7658dbd2f7afe1c64225e16c7216 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 ca74672a23b8e84afa0dc4fd86e0d94993df530d..f4d4af3af78eeeccece71173c72b6b7edfbcceca 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 ba876831c793f094e27794f256ba860b6d27af0a..aa1343c521ba36d87ca1785ef8d81c870ecbb009 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;