diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index 45b78080ccd341937ec8a26b12601dc312ef0929..02066caca5c2ede771894729ac9eff71d67cb15c 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -69,6 +69,12 @@ public:
max_ = max(a, b);
}
+ AABB(const AABB& core, const Vec3f& delta)
+ {
+ min_ = core.min_ - delta;
+ max_ = core.max_ + delta;
+ }
+
/** \brief Check whether two AABB are overlap */
inline bool overlap(const AABB& other) const
{
@@ -186,6 +192,23 @@ public:
{
return min_.equal(other.min_) && max_.equal(other.max_);
}
+
+ inline AABB& expand(const Vec3f& delta)
+ {
+ min_ -= delta;
+ max_ += delta;
+ return *this;
+ }
+
+ /**\brief expand the aabb by increase the 'thickness of the plate by a ratio */
+ inline AABB& expand(const AABB& core, BVH_REAL ratio)
+ {
+ min_ = min_ * ratio - core.min_;
+ max_ = max_ * ratio - core.max_;
+ return *this;
+ }
+
+
};
}
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index cb9f1a30df240f2ffbe92dd9542a5616098d4bf5..4228b5bc9728d64e5ae5981f7b8fa53e22736785 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -445,8 +445,7 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
if(min_dist < std::numeric_limits<BVH_REAL>::max())
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb.min_ -= min_dist_delta;
- aabb.max_ += min_dist_delta;
+ aabb.expand(min_dist_delta);
}
AABB overlap_aabb;
@@ -500,22 +499,15 @@ bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj,
if(min_dist < old_min_distance)
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb.min_ = obj->getAABB().min_ - min_dist_delta;
- aabb.max_ = obj->getAABB().max_ + min_dist_delta;
+ aabb = AABB(obj->getAABB(), min_dist_delta);
status = 0;
}
else
{
if(aabb.equal(obj->getAABB()))
- {
- aabb.min_ -= delta;
- aabb.max_ += delta;
- }
+ aabb.expand(delta);
else
- {
- aabb.min_ = aabb.min_ * 2 - obj->getAABB().min_;
- aabb.max_ = aabb.max_ * 2 - obj->getAABB().max_;
- }
+ aabb.expand(obj->getAABB(), 2.0);
}
}
}
@@ -643,6 +635,7 @@ public:
elist[0] = NULL;
elist[1] = NULL;
elist[2] = NULL;
+ enable_tested_set = true;
}
~SaPCollisionManager()
@@ -834,6 +827,11 @@ protected:
std::list<SaPPair> overlap_pairs;
size_t optimal_axis;
+
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
+
+public:
+ bool enable_tested_set;
};
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 4878e24345ea86ba182c9a4bfc91e800baf2b817..b72cb8e927b027668964ff78b3dedf51432f1cd3 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -1141,7 +1141,7 @@ void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCa
EndPoint* pos = *res_it;
- while(pos != NULL & pos->getVal(axis) <= max_val)
+ while((pos != NULL) && (pos->getVal(axis) <= max_val))
{
if(pos->aabb->cached.overlap(obj->getAABB()))
{
@@ -1152,19 +1152,110 @@ void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCa
}
}
-void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
{
- if(size() == 0) return;
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ AABB aabb = obj->getAABB();
- BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
+ if(min_dist < std::numeric_limits<BVH_REAL>::max())
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb.expand(min_dist_delta);
+ }
+
+ size_t axis = optimal_axis;
+
+ int status = 1;
+ BVH_REAL old_min_distance;
+
+ EndPoint* start_pos = elist[axis];
+
+ std::set<CollisionObject*> tested;
+
+ while(1)
{
- if((*it)->obj->getAABB().distance(obj->getAABB()) < min_dist)
+ old_min_distance = min_dist;
+ BVH_REAL min_val = aabb.min_[axis];
+ BVH_REAL max_val = aabb.max_[axis];
+
+ EndPoint dummy;
+ SaPAABB dummy_aabb;
+ dummy_aabb.cached = aabb;
+ dummy.minmax = 1;
+ dummy.aabb = &dummy_aabb;
+
+
+ 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)));
+
+ EndPoint* end_pos = NULL;
+ if(res_it != velist[axis].end())
+ end_pos = *res_it;
+
+ EndPoint* pos = start_pos;
+
+ while(pos != end_pos)
{
- if(callback(obj, (*it)->obj, cdata, min_dist))
- return;
+ // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos.
+ // but this seems slower.
+ if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
+ {
+ CollisionObject* curr_obj = pos->aabb->obj;
+ if(!enable_tested_set || (tested.find(curr_obj) == tested.end()))
+ {
+ if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(curr_obj, obj, cdata, min_dist))
+ return true;
+ }
+ if(enable_tested_set) tested.insert(curr_obj);
+ }
+ }
+
+ pos = pos->next[axis];
+ }
+
+ if(status == 1)
+ {
+ if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ {
+ if(min_dist < old_min_distance) break;
+ else
+ min_dist = std::numeric_limits<BVH_REAL>::max();
+ }
+ else
+ {
+ if(min_dist < old_min_distance)
+ {
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb = AABB(obj->getAABB(), min_dist_delta);
+ status = 0;
+ }
+ else
+ {
+ if(aabb.equal(obj->getAABB()))
+ aabb.expand(delta);
+ else
+ aabb.expand(obj->getAABB(), 2.0);
+ }
+ }
}
+ else if(status == 0)
+ break;
}
+
+ return false;
+}
+
+void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+
+ distance_(obj, cdata, callback, min_dist);
}
void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
@@ -1186,16 +1277,11 @@ void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
if(size() == 0) return;
BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+
for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
{
- std::list<SaPAABB*>::const_iterator it2 = it; it2++;
- for(; it2 != AABB_arr.end(); ++it2)
- {
- if((*it)->obj->getAABB().distance((*it2)->obj->getAABB()) < min_dist)
- {
- if(callback((*it)->obj, (*it2)->obj, cdata, min_dist)) return;
- }
- }
+ if(distance_((*it)->obj, cdata, callback, min_dist))
+ return;
}
}
@@ -1503,8 +1589,7 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata,
if(min_dist < std::numeric_limits<BVH_REAL>::max())
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb.min_ -= min_dist_delta;
- aabb.max_ += min_dist_delta;
+ aabb.expand(min_dist_delta);
}
int status = 1;
@@ -1566,22 +1651,15 @@ bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata,
if(min_dist < old_min_distance)
{
Vec3f min_dist_delta(min_dist, min_dist, min_dist);
- aabb.min_ = obj->getAABB().min_ - min_dist_delta;
- aabb.max_ = obj->getAABB().max_ + min_dist_delta;
+ aabb = AABB(obj->getAABB(), min_dist_delta);
status = 0;
}
else
{
if(aabb.equal(obj->getAABB()))
- {
- aabb.min_ -= delta;
- aabb.max_ += delta;
- }
+ aabb.expand(delta);
else
- {
- aabb.min_ = aabb.min_ * 2 - obj->getAABB().min_;
- aabb.max_ = aabb.max_ * 2 - obj->getAABB().max_;
- }
+ aabb.expand(obj->getAABB(), 2.0);
}
}
}