Commit 868f9ee8 authored by jpan's avatar jpan
Browse files

a faster (only a bit) implementation of dynamicAABBTree.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@122 253336fb-580f-4252-a368-f3cef5a2a82b
parent 9c1c39eb
......@@ -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_;
......
......@@ -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:
......
......@@ -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);
}
......
......@@ -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);
......
......@@ -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];
};
......
......@@ -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);
}
......
......@@ -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 -> ......
* |
......
......@@ -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);
}
......
......@@ -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);
}
......
......@@ -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);