From 46dd3294156664b9103488b10924c9ecaac36a06 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Sun, 3 Jun 2012 06:50:01 +0000
Subject: [PATCH] Add the broad phase distance
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@89 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/AABB.h | 8 +-
trunk/fcl/include/fcl/broad_phase_collision.h | 204 ++++++----
trunk/fcl/include/fcl/collision_data.h | 13 +
trunk/fcl/include/fcl/vec_3f.h | 10 +-
trunk/fcl/src/AABB.cpp | 51 ++-
trunk/fcl/src/broad_phase_collision.cpp | 369 +++++++++++++++---
6 files changed, 520 insertions(+), 135 deletions(-)
diff --git a/trunk/fcl/include/fcl/AABB.h b/trunk/fcl/include/fcl/AABB.h
index 620bb79b..45b78080 100644
--- a/trunk/fcl/include/fcl/AABB.h
+++ b/trunk/fcl/include/fcl/AABB.h
@@ -180,10 +180,12 @@ public:
return (min_ + max_) * 0.5;
}
- /** \brief The distance between two AABB
- * Not implemented.
- */
BVH_REAL distance(const AABB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+
+ inline bool equal(const AABB& other) const
+ {
+ return min_.equal(other.min_) && max_.equal(other.max_);
+ }
};
}
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index de15ffc1..f9a424d5 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -55,11 +55,13 @@ namespace fcl
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
-BVH_REAL defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
+bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
/** \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);
+
/** \brief Base class for broad phase collision */
class BroadPhaseCollisionManager
{
@@ -87,6 +89,9 @@ public:
/** \brief perform collision test between one object and all the objects belonging to the manager */
virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0;
+ /** \brief perform distance computation between one object and all the objects belonging to the manager */
+ virtual void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0;
+
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
@@ -125,6 +130,9 @@ public:
/** \brief perform collision test between one object and all the objects belonging to the manager */
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+ /** \brief perform distance computation between one object and all the objects belonging to the manager */
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
void collide(void* cdata, CollisionCallBack callback) const;
@@ -140,7 +148,7 @@ protected:
std::list<CollisionObject*> objs;
};
-
+/** \brief Spatial hash function: hash an AABB to a set of integer values */
struct SpatialHash
{
SpatialHash(const AABB& scene_limit_, BVH_REAL cell_size_)
@@ -201,33 +209,45 @@ public:
delete hash_table;
}
+ /** \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 clear the manager */
void clear();
+ /** \brief return the objects managed by the manager */
void getObjects(std::vector<CollisionObject*>& objs) const;
+ /** \brief perform collision test between one object and all the objects belonging to the manager */
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+ /** \brief perform distance computation between one object and all the objects belonging ot the manager */
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
+ /** \brief perform collision test for the objects belonging to the manager (i.e, N^2 self collision) */
void collide(void* cdata, CollisionCallBack callback) const;
+ /** \brief whether the manager is empty */
bool empty() const;
+ /** \brief the number of objects managed by the manager */
size_t size() const;
+ /** \brief compute the bound for the environent */
static void computeBound(std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u)
{
AABB bound;
for(unsigned int i = 0; i < objs.size(); ++i)
- {
bound += objs[i]->getAABB();
- }
l = bound.min_;
u = bound.max_;
@@ -243,6 +263,7 @@ protected:
// objects outside the scene limit are in another list
std::list<CollisionObject*> objs_outside_scene_limit;
+ // the size of the scene
AABB scene_limit;
};
@@ -263,9 +284,7 @@ void SpatialHashingCollisionManager<HashTable>::registerObject(CollisionObject*
hash_table->insert(overlap_aabb, obj);
}
else
- {
objs_outside_scene_limit.push_back(obj);
- }
}
template<typename HashTable>
@@ -284,9 +303,7 @@ void SpatialHashingCollisionManager<HashTable>::unregisterObject(CollisionObject
hash_table->remove(overlap_aabb, obj);
}
else
- {
objs_outside_scene_limit.remove(obj);
- }
}
template<typename HashTable>
@@ -301,9 +318,7 @@ void SpatialHashingCollisionManager<HashTable>::update()
std::list<CollisionObject*>::const_iterator it;
for(it = objs.begin(); it != objs.end(); ++it)
- {
registerObject(*it);
- }
}
template<typename HashTable>
@@ -354,6 +369,90 @@ void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, vo
}
}
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+{
+ DistanceData* cdata = static_cast<DistanceData*>(cdata_);
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ AABB aabb = obj->getAABB();
+ if(cdata->min_distance < std::numeric_limits<BVH_REAL>::max())
+ {
+ BVH_REAL d = cdata->min_distance;
+ aabb.min_ -= Vec3f(d, d, d);
+ aabb.max_ += Vec3f(d, d, d);
+ }
+ AABB overlap_aabb;
+
+ int status = 1;
+ BVH_REAL old_min_distance;
+
+ while(1)
+ {
+ old_min_distance = cdata->min_distance;
+
+ if(scene_limit.overlap(aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(aabb))
+ {
+ std::list<CollisionObject*>::const_iterator it;
+ for(it = objs_outside_scene_limit.begin(); it != objs_outside_scene_limit.end(); ++it)
+ {
+ if(callback(obj, *it, cdata)) break;
+ }
+ }
+
+ std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
+ for(unsigned int i = 0; i < query_result.size(); ++i)
+ {
+ if(callback(obj, query_result[i], cdata)) break;
+ }
+ }
+ else
+ {
+ std::list<CollisionObject*>::const_iterator it;
+ for(it = objs_outside_scene_limit.begin(); it != objs_outside_scene_limit.end(); ++it)
+ {
+ if(callback(obj, *it, cdata)) break;
+ }
+ }
+
+ if(status == 1)
+ {
+ if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ {
+ if(cdata->min_distance < old_min_distance) break;
+ else
+ cdata->min_distance = std::numeric_limits<BVH_REAL>::max();
+ }
+ else
+ {
+ if(cdata->min_distance < old_min_distance)
+ {
+ BVH_REAL d = cdata->min_distance;
+ aabb.min_ = obj->getAABB().min_ - Vec3f(d, d, d);
+ aabb.max_ = obj->getAABB().max_ + Vec3f(d, d, d);
+ status = 0;
+ }
+ else
+ {
+ if(aabb.equal(obj->getAABB()))
+ {
+ aabb.min_ -= delta;
+ aabb.max_ += delta;
+ }
+ else
+ {
+ aabb.min_ = aabb.min_ * 2 - obj->getAABB().min_;
+ aabb.max_ = aabb.max_ * 2 - obj->getAABB().max_;
+ }
+ }
+ }
+ }
+ else if(status == 0)
+ break;
+ }
+}
+
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCallBack callback) const
{
@@ -370,20 +469,14 @@ void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCa
std::list<CollisionObject*>::const_iterator it2;
for(it2 = objs_outside_scene_limit.begin(); it2 != objs_outside_scene_limit.end(); ++it2)
{
- if(*it1 < *it2)
- {
- if(callback(*it1, *it2, cdata)) return;
- }
+ if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
}
}
std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
for(unsigned int i = 0; i < query_result.size(); ++i)
{
- if(*it1 < query_result[i])
- {
- if(callback(*it1, query_result[i], cdata)) return;
- }
+ if(*it1 < query_result[i]) { if(callback(*it1, query_result[i], cdata)) return; }
}
}
else
@@ -391,10 +484,7 @@ void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCa
std::list<CollisionObject*>::const_iterator it2;
for(it2 = objs_outside_scene_limit.begin(); it2 != objs_outside_scene_limit.end(); ++it2)
{
- if(*it1 < *it2)
- {
- if(callback(*it1, *it2, cdata)) return;
- }
+ if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; }
}
}
}
@@ -451,6 +541,9 @@ public:
/** \brief perform collision test between one object and all the objects belonging to the manager */
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+ /** \brief perform distance computation between one object and all the objects belonging to the manager */
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
void collide(void* cdata, CollisionCallBack callback) const;
@@ -568,6 +661,9 @@ protected:
std::list<SaPPair> overlap_pairs;
};
+
+
+
/** Simple SAP collision manager */
class SSaPCollisionManager : public BroadPhaseCollisionManager
{
@@ -598,6 +694,9 @@ public:
/** \brief perform collision test between one object and all the objects belonging to the manager */
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+ /** \brief perform distance computation between one object and all the objects belonging to the manager */
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
void collide(void* cdata, CollisionCallBack callback) const;
@@ -608,56 +707,13 @@ public:
inline size_t size() const { return objs_x.size(); }
protected:
-
- /** \brief Functor sorting objects according to the AABB lower x bound */
- struct SortByXLow
- {
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getAABB().min_[0] < b->getAABB().min_[0])
- return true;
- return false;
- }
- };
-
- /** \brief Functor sorting objects according to the AABB lower y bound */
- struct SortByYLow
- {
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getAABB().min_[1] < b->getAABB().min_[1])
- return true;
- return false;
- }
- };
-
- /** \brief Functor sorting objects according to the AABB lower z bound */
- struct SortByZLow
- {
- bool operator()(const CollisionObject* a, const CollisionObject* b) const
- {
- if(a->getAABB().min_[2] < b->getAABB().min_[2])
- return true;
- return false;
- }
- };
-
- /** \brief Dummy collision object with a point AABB */
- class DummyCollisionObject : public CollisionObject
- {
- public:
- DummyCollisionObject(const AABB& aabb_)
- {
- aabb = aabb_;
- }
-
- void computeLocalAABB() {}
- };
-
/** \brief check collision between one object and a list of objects */
void checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+ void checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
+ CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
/** \brief Objects sorted according to lower x value */
std::vector<CollisionObject*> objs_x;
@@ -704,6 +760,9 @@ public:
/** \brief perform collision test between one object and all the objects belonging to the manager */
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+ /** \brief perform distance computation between one object and all the objects belonging to the manager */
+ void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
void collide(void* cdata, CollisionCallBack callback) const;
@@ -715,10 +774,6 @@ public:
protected:
- /** \brief check collision between one object and a list of objects */
- void checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
-
/** \brief SAP end point */
struct EndPoint
@@ -756,6 +811,11 @@ protected:
}
};
+
+ void checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ void checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
/** \brief vector stores all the end points */
std::vector<EndPoint> endpoints[3];
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index c39e1151..f543be0a 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -4,6 +4,7 @@
#include "fcl/collision_object.h"
#include "fcl/vec_3f.h"
#include <vector>
+#include <limits>
namespace fcl
@@ -68,6 +69,18 @@ struct CollisionData
std::vector<Contact> contacts;
};
+struct DistanceData
+{
+ DistanceData()
+ {
+ min_distance = std::numeric_limits<BVH_REAL>::max();
+ done = false;
+ }
+
+ BVH_REAL min_distance;
+ bool done;
+};
+
}
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 58a2b284..35adfa02 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -41,6 +41,7 @@
#include <cmath>
#include <cstdlib>
#include <algorithm>
+#include <cstring>
/** \brief Main namespace */
namespace fcl
@@ -310,11 +311,14 @@ namespace fcl
Vec3f() { v_[0] = 0; v_[1] = 0; v_[2] = 0; }
+ Vec3f(const Vec3f& other)
+ {
+ memcpy(v_, other.v_, sizeof(BVH_REAL) * 3);
+ }
+
Vec3f(const BVH_REAL* v)
{
- v_[0] = v[0];
- v_[1] = v[1];
- v_[2] = v[2];
+ memcpy(v_, v, sizeof(BVH_REAL) * 3);
}
Vec3f(BVH_REAL x, BVH_REAL y, BVH_REAL z)
diff --git a/trunk/fcl/src/AABB.cpp b/trunk/fcl/src/AABB.cpp
index 2cd8ebd2..39ba0c09 100644
--- a/trunk/fcl/src/AABB.cpp
+++ b/trunk/fcl/src/AABB.cpp
@@ -51,8 +51,55 @@ AABB::AABB()
BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
{
- std::cerr << "AABB distance not implemented!" << std::endl;
- return 0.0;
+ BVH_REAL result = 0;
+ for(unsigned int i = 0; i < 3; ++i)
+ {
+ BVH_REAL amin = min_[i];
+ BVH_REAL amax = max_[i];
+ BVH_REAL bmin = other.min_[i];
+ BVH_REAL bmax = other.max_[i];
+
+ if(amin > bmax)
+ {
+ BVH_REAL delta = bmax - amin;
+ result += delta * delta;
+ if(P && Q)
+ {
+ (*P)[i] = amin;
+ (*Q)[i] = bmax;
+ }
+ }
+ else if(bmin > amax)
+ {
+ BVH_REAL delta = amax - bmin;
+ result += delta * delta;
+ if(P && Q)
+ {
+ (*P)[i] = amax;
+ (*Q)[i] = bmin;
+ }
+ }
+ else
+ {
+ if(P && Q)
+ {
+ if(bmin >= amin)
+ {
+ BVH_REAL t = 0.5 * (amax + bmin);
+ (*P)[i] = t;
+ (*Q)[i] = t;
+ }
+ else
+ {
+ BVH_REAL t = 0.5 * (amin + bmax);
+ (*P)[i] = t;
+ (*Q)[i] = t;
+ }
+ }
+ }
+ }
+
+ return sqrt(result);
}
}
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index bd7b9dd3..dad4ba31 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -37,6 +37,7 @@
#include "fcl/broad_phase_collision.h"
#include "fcl/collision.h"
+#include "fcl/distance.h"
#include <algorithm>
#include <set>
#include <deque>
@@ -49,7 +50,7 @@ namespace fcl
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
{
- CollisionData* cdata = (CollisionData*)cdata_;
+ CollisionData* cdata = static_cast<CollisionData*>(cdata_);
if(cdata->done) return true;
@@ -69,9 +70,16 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
return cdata->done;
}
-BVH_REAL defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
+bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
{
- return 0.0;
+ DistanceData* cdata = static_cast<DistanceData*>(cdata_);
+
+ if(cdata->done) return true;
+
+ BVH_REAL d = distance(o1, o2);
+ if(cdata->min_distance > d) cdata->min_distance = d;
+
+ return cdata->done;
}
void NaiveCollisionManager::unregisterObject(CollisionObject* obj)
@@ -115,6 +123,16 @@ void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, Collision
}
}
+void NaiveCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ std::list<CollisionObject*>::const_iterator it;
+ for(it = objs.begin(); it != objs.end(); ++it)
+ {
+ if(callback(obj, *it, cdata))
+ return;
+ }
+}
+
void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) const
{
std::list<CollisionObject*>::const_iterator it1;
@@ -136,6 +154,52 @@ bool NaiveCollisionManager::empty() const
}
+/** \brief Functor sorting objects according to the AABB lower x bound */
+struct SortByXLow
+{
+ bool operator()(const CollisionObject* a, const CollisionObject* b) const
+ {
+ if(a->getAABB().min_[0] < b->getAABB().min_[0])
+ return true;
+ return false;
+ }
+};
+
+/** \brief Functor sorting objects according to the AABB lower y bound */
+struct SortByYLow
+{
+ bool operator()(const CollisionObject* a, const CollisionObject* b) const
+ {
+ if(a->getAABB().min_[1] < b->getAABB().min_[1])
+ return true;
+ return false;
+ }
+};
+
+/** \brief Functor sorting objects according to the AABB lower z bound */
+struct SortByZLow
+{
+ bool operator()(const CollisionObject* a, const CollisionObject* b) const
+ {
+ if(a->getAABB().min_[2] < b->getAABB().min_[2])
+ return true;
+ return false;
+ }
+};
+
+/** \brief Dummy collision object with a point AABB */
+class DummyCollisionObject : public CollisionObject
+{
+public:
+ DummyCollisionObject(const AABB& aabb_) : CollisionObject()
+ {
+ aabb = aabb_;
+ }
+
+ void computeLocalAABB() {}
+};
+
+
void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
{
setup();
@@ -246,17 +310,16 @@ void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionC
std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
std::vector<CollisionObject*>::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
unsigned int d1 = pos_end1 - pos_start1;
+
if(d1 > CUTOFF)
{
std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
-
std::vector<CollisionObject*>::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
unsigned int d2 = pos_end2 - pos_start2;
if(d2 > CUTOFF)
{
std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
-
std::vector<CollisionObject*>::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
unsigned int d3 = pos_end3 - pos_start3;
@@ -282,6 +345,114 @@ void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionC
checkColl(pos_start1, pos_end1, obj, cdata, callback);
}
+void SSaPCollisionManager::checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ while(pos_start != pos_end)
+ {
+ if(callback(*pos_start, obj, cdata))
+ return;
+
+ pos_start++;
+ }
+}
+
+void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+{
+ static const unsigned int CUTOFF = 100;
+
+ DistanceData* cdata = static_cast<DistanceData*>(cdata_);
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ Vec3f dummy_vector = obj->getAABB().max_;
+ if(cdata->min_distance < std::numeric_limits<BVH_REAL>::max())
+ dummy_vector += Vec3f(cdata->min_distance, cdata->min_distance, cdata->min_distance);
+
+ std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
+ std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
+ std::vector<CollisionObject*>::const_iterator pos_start3 = objs_z.begin();
+ std::vector<CollisionObject*>::const_iterator pos_end1 = objs_x.end();
+ std::vector<CollisionObject*>::const_iterator pos_end2 = objs_y.end();
+ std::vector<CollisionObject*>::const_iterator pos_end3 = objs_z.end();
+
+ int status = 1;
+ BVH_REAL old_min_distance;
+
+ while(1)
+ {
+ old_min_distance = cdata->min_distance;
+ AABB dummy_aabb = AABB(dummy_vector);
+ DummyCollisionObject dummyHigh(dummy_aabb);
+ // DummyCollisionObject dummyHigh(AABB(dummy_vector));
+
+ pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
+ unsigned int d1 = pos_end1 - pos_start1;
+
+ if(d1 > CUTOFF)
+ {
+ pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
+ unsigned int d2 = pos_end2 - pos_start2;
+
+ if(d2 > CUTOFF)
+ {
+ pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
+ unsigned int d3 = pos_end3 - pos_start3;
+
+ if(d3 > CUTOFF)
+ {
+ if(d3 <= d2 && d3 <= d1)
+ checkDis(pos_start3, pos_end3, obj, cdata, callback);
+ else
+ {
+ if(d2 <= d3 && d2 <= d1)
+ checkDis(pos_start2, pos_end2, obj, cdata, callback);
+ else
+ checkDis(pos_start1, pos_end1, obj, cdata, callback);
+ }
+ }
+ else
+ checkDis(pos_start3, pos_end3, obj, cdata, callback);
+ }
+ else
+ checkDis(pos_start2, pos_end2, obj, cdata, callback);
+ }
+ else
+ {
+ checkDis(pos_start1, pos_end1, obj, cdata, callback);
+ }
+
+ if(status == 1)
+ {
+ if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ {
+ if(cdata->min_distance < old_min_distance) break;
+ else // the initial min distance estimate is not correct, so turn to use conservative estimate. dummy_vector not changed this time.
+ cdata->min_distance = std::numeric_limits<BVH_REAL>::max();
+ }
+ else
+ {
+ // from infinity to a finite one, only need one additional loop
+ // to check the possible missed ones to the right of the objs array
+ if(cdata->min_distance < old_min_distance)
+ {
+ BVH_REAL d = cdata->min_distance;
+ dummy_vector = obj->getAABB().max_ + Vec3f(d, d, d);
+ status = 0;
+ }
+ else // need more loop
+ {
+ if(dummy_vector.equal(obj->getAABB().max_))
+ dummy_vector = dummy_vector + delta;
+ else
+ dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
+ }
+ if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
+ if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
+ if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
+ }
+ }
+ else if(status == 0)
+ break;
+ }
+}
void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
{
@@ -691,6 +862,19 @@ void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCa
}
}
+void SaPCollisionManager::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+{
+ DistanceData* cdata = static_cast<DistanceData*>(cdata_);
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
+ {
+ if((*it)->obj->getAABB().distance(obj->getAABB()) < cdata->min_distance)
+ {
+ if(callback(obj, (*it)->obj, cdata_))
+ return;
+ }
+ }
+}
+
void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
{
for(std::list<SaPPair>::const_iterator it = overlap_pairs.begin(); it != overlap_pairs.end(); ++it)
@@ -907,11 +1091,6 @@ void IntervalTreeCollisionManager::getObjects(std::vector<CollisionObject*>& obj
}
}
-void IntervalTreeCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, CollisionCallBack callback) const
-{
-
-}
void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
@@ -933,66 +1112,117 @@ void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, Co
int d3 = results2.size();
if(d1 >= d2 && d1 >= d3)
- {
- for(unsigned int i = 0; i < results0.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results0[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
+ checkColl(results0.begin(), results0.end(), obj, cdata, callback);
else if(d2 >= d1 && d2 >= d3)
- {
- for(unsigned int i = 0; i < results1.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results1[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
+ checkColl(results1.begin(), results1.end(), obj, cdata, callback);
else
- {
- for(unsigned int i = 0; i < results2.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results2[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
+ checkColl(results2.begin(), results2.end(), obj, cdata, callback);
}
else
+ checkColl(results2.begin(), results2.end(), obj, cdata, callback);
+ }
+ else
+ checkColl(results1.begin(), results1.end(), obj, cdata, callback);
+ }
+ else
+ checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+
+ results0.clear();
+ results1.clear();
+ results2.clear();
+}
+
+void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+{
+ static const unsigned int CUTOFF = 100;
+
+ DistanceData* cdata = static_cast<DistanceData*>(cdata_);
+ Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
+ AABB aabb = obj->getAABB();
+ if(cdata->min_distance < std::numeric_limits<BVH_REAL>::max())
+ {
+ BVH_REAL d = cdata->min_distance;
+ aabb.min_ -= Vec3f(d, d, d);
+ aabb.max_ += Vec3f(d, d, d);
+ }
+
+ int status = 1;
+ BVH_REAL old_min_distance;
+
+ while(1)
+ {
+ old_min_distance = cdata->min_distance;
+
+ std::deque<SimpleInterval*> results0, results1, results2;
+
+ results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]);
+ if(results0.size() > CUTOFF)
+ {
+ results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]);
+ if(results1.size() > CUTOFF)
{
- for(unsigned int i = 0; i < results2.size(); ++i)
+ results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]);
+ if(results2.size() > CUTOFF)
{
- SAPInterval* ivl = (SAPInterval*)results2[i];
- if(callback(ivl->obj, obj, cdata))
- break;
+ int d1 = results0.size();
+ int d2 = results1.size();
+ int d3 = results2.size();
+
+ if(d1 >= d2 && d1 >= d3)
+ checkDist(results0.begin(), results1.end(), obj, cdata, callback);
+ else if(d2 >= d1 && d2 >= d3)
+ checkDist(results1.begin(), results1.end(), obj, cdata, callback);
+ else
+ checkDist(results2.begin(), results2.end(), obj, cdata, callback);
}
+ else
+ checkDist(results2.begin(), results2.end(), obj, cdata, callback);
}
+ else
+ checkDist(results1.begin(), results1.end(), obj, cdata, callback);
}
else
+ checkDist(results0.begin(), results0.end(), obj, cdata, callback);
+
+ results0.clear();
+ results1.clear();
+ results2.clear();
+
+ if(status == 1)
{
- for(unsigned int i = 0; i < results1.size(); ++i)
+ if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
+ {
+ if(cdata->min_distance < old_min_distance) break;
+ else
+ cdata->min_distance = std::numeric_limits<BVH_REAL>::max();
+ }
+ else
{
- SAPInterval* ivl = (SAPInterval*)results1[i];
- if(callback(ivl->obj, obj, cdata))
- break;
+ if(cdata->min_distance < old_min_distance)
+ {
+ BVH_REAL d = cdata->min_distance;
+ aabb.min_ = obj->getAABB().min_ - Vec3f(d, d, d);
+ aabb.max_ = obj->getAABB().max_ + Vec3f(d, d, d);
+ status = 0;
+ }
+ else
+ {
+ if(aabb.equal(obj->getAABB()))
+ {
+ aabb.min_ -= delta;
+ aabb.max_ += delta;
+ }
+ else
+ {
+ aabb.min_ = aabb.min_ * 2 - obj->getAABB().min_;
+ aabb.max_ = aabb.max_ * 2 - obj->getAABB().max_;
+ }
+ }
}
}
+ else if(status == 0)
+ break;
}
- else
- {
- for(unsigned int i = 0; i < results0.size(); ++i)
- {
- SAPInterval* ivl = (SAPInterval*)results0[i];
- if(callback(ivl->obj, obj, cdata))
- break;
- }
- }
-
- results0.clear();
- results1.clear();
- results2.clear();
}
void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const
@@ -1055,6 +1285,35 @@ bool IntervalTreeCollisionManager::empty() const
return endpoints[0].empty();
}
+void IntervalTreeCollisionManager::checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ while(pos_start != pos_end)
+ {
+ SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
+
+ if(ivl->obj->getAABB().overlap(obj->getAABB()))
+ {
+ if(callback(ivl->obj, obj, cdata))
+ return;
+ }
+
+ pos_start++;
+ }
+}
+
+void IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ while(pos_start != pos_end)
+ {
+ SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
+ if(callback(ivl->obj, obj, cdata))
+ return;
+
+ pos_start++;
+ }
+}
+
+
}
--
GitLab