From c72fef809150907cde8bbfc64f0191ae25411653 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Mon, 2 Jul 2012 03:20:42 +0000
Subject: [PATCH] Optimize the broadphase for performance
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@111 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 2 +-
trunk/fcl/include/fcl/BVH_model.h | 1 -
trunk/fcl/include/fcl/broad_phase_collision.h | 674 ++++++++++++++--
trunk/fcl/src/BV/AABB.cpp | 2 +-
trunk/fcl/src/broad_phase_collision.cpp | 736 ++++++++++++++----
trunk/fcl/src/interval_tree.cpp | 2 +-
trunk/fcl/src/traversal_recurse.cpp | 2 +
7 files changed, 1203 insertions(+), 216 deletions(-)
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index c2717d0c..9d2f4949 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp src/narrowphase.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp src/narrowphase.cpp src/hierarchy_tree.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index a4b11700..472f6066 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -108,7 +108,6 @@ public:
bv_splitter.reset(new BVSplitter<BV>(SPLIT_METHOD_MEAN));
bv_fitter.reset(new BVFitter<BV>());
}
-
BVHModel(const BVHModel& other);
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index e52c62db..cb9f1a30 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -43,24 +43,29 @@
#include "fcl/collision_data.h"
#include "fcl/BV/AABB.h"
#include "fcl/interval_tree.h"
+#include "fcl/hierarchy_tree.h"
#include "fcl/hash.h"
#include <vector>
#include <list>
#include <iostream>
+#include <boost/unordered_map.hpp>
+
+#include <boost/bind.hpp>
namespace fcl
{
-
+/** \brief collision function for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now */
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
-bool defaultDistanceFunction(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);
/** \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);
+typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, BVH_REAL& dist);
/** \brief Base class for broad phase collision */
class BroadPhaseCollisionManager
@@ -68,6 +73,13 @@ class BroadPhaseCollisionManager
public:
virtual ~BroadPhaseCollisionManager() {}
+ /** \brief add objects to the manager */
+ virtual void registerObjects(const std::vector<CollisionObject*>& other_objs)
+ {
+ for(size_t i = 0; i < other_objs.size(); ++i)
+ registerObject(other_objs[i]);
+ }
+
/** \brief add one object to the manager */
virtual void registerObject(CollisionObject* obj) = 0;
@@ -95,6 +107,15 @@ public:
/** \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;
+ /** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
+ virtual void distance(void* cdata, DistanceCallBack callback) const = 0;
+
+ /** \brief perform collision test with objects belonging to another manager */
+ virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0;
+
+ /** \brief perform distance test with objects belonging to another manager */
+ virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0;
+
/** \brief whether the manager is empty */
virtual bool empty() const = 0;
@@ -109,10 +130,13 @@ class NaiveCollisionManager : public BroadPhaseCollisionManager
public:
NaiveCollisionManager() {}
- /** \brief remove one object from the manager */
- void registerObject(CollisionObject* obj);
+ /** \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 */
@@ -136,6 +160,15 @@ public:
/** \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 perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /** \brief perform collision test with objects belonging to another manager */
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /** \brief perform distance test with objects belonging to another manager */
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
/** \brief whether the manager is empty */
bool empty() const;
@@ -236,6 +269,15 @@ public:
/** \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 perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /** \brief perform collision test with objects belonging to another manager */
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /** \brief perform distance test with objects belonging to another manager */
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
/** \brief whether the manager is empty */
bool empty() const;
@@ -255,6 +297,13 @@ public:
protected:
+ /** \brief perform collision test between one object and all the objects belonging to the manager */
+ 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;
+
+
// all objects in the scene
std::list<CollisionObject*> objs;
@@ -339,7 +388,20 @@ void SpatialHashingCollisionManager<HashTable>::getObjects(std::vector<Collision
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
- if(size() == 0) return;
+ collide_(obj, cdata, callback);
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ distance_(obj, cdata, callback, min_dist);
+}
+
+template<typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return false;
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
@@ -351,14 +413,14 @@ void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, vo
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)) return;
+ if(callback(obj, *it, cdata)) return true;
}
}
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)) return;
+ if(callback(obj, query_result[i], cdata)) return true;
}
}
else
@@ -366,25 +428,27 @@ void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, vo
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)) return;
+ if(callback(obj, *it, cdata)) return true;
}
}
+
+ return false;
}
template<typename HashTable>
-void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
{
- if(size() == 0) return;
+ if(size() == 0) return false;
- 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())
+ if(min_dist < std::numeric_limits<BVH_REAL>::max())
{
- BVH_REAL d = cdata->min_distance;
- aabb.min_ -= Vec3f(d, d, d);
- aabb.max_ += Vec3f(d, d, d);
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb.min_ -= min_dist_delta;
+ aabb.max_ += min_dist_delta;
}
+
AABB overlap_aabb;
int status = 1;
@@ -392,7 +456,7 @@ void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, v
while(1)
{
- old_min_distance = cdata->min_distance;
+ old_min_distance = min_dist;
if(scene_limit.overlap(aabb, overlap_aabb))
{
@@ -401,14 +465,16 @@ void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, v
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(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
}
}
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;
+ if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist)
+ if(callback(obj, query_result[i], cdata, min_dist)) return true;
}
}
else
@@ -416,7 +482,8 @@ void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, v
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(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ if(callback(obj, *it, cdata, min_dist)) return true;
}
}
@@ -424,17 +491,17 @@ void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, v
{
if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
{
- if(cdata->min_distance < old_min_distance) break;
+ if(min_dist < old_min_distance) break;
else
- cdata->min_distance = std::numeric_limits<BVH_REAL>::max();
+ min_dist = std::numeric_limits<BVH_REAL>::max();
}
else
{
- if(cdata->min_distance < old_min_distance)
+ if(min_dist < 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);
+ 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;
status = 0;
}
else
@@ -455,6 +522,8 @@ void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, v
else if(status == 0)
break;
}
+
+ return false;
}
template<typename HashTable>
@@ -496,6 +565,61 @@ void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCa
}
}
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(void* cdata, DistanceCallBack callback) const
+{
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(); it != objs.end(); ++it)
+ if(distance_(*it, cdata, callback, min_dist)) return;
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ if(this->size() < other_manager->size())
+ {
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(); it != objs.end(); ++it)
+ if(other_manager->collide_(*it, cdata, callback)) return;
+ }
+ else
+ {
+ for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(); it != other_manager->objs.end(); ++it)
+ if(collide_(*it, cdata, callback)) return;
+ }
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+
+ if(this->size() < other_manager->size())
+ {
+ for(std::list<CollisionObject*>::const_iterator it = objs.begin(); it != objs.end(); ++it)
+ if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
+ }
+ else
+ {
+ for(std::list<CollisionObject*>::const_iterator it = other_manager->objs.begin(); it != other_manager->objs.end(); ++it)
+ if(distance_(*it, cdata, callback, min_dist)) return;
+ }
+}
+
template<typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::empty() const
{
@@ -526,6 +650,9 @@ public:
clear();
}
+ /** \brief add objects to the manager */
+ void registerObjects(const std::vector<CollisionObject*>& other_objs);
+
/** \brief remove one object from the manager */
void registerObject(CollisionObject* obj);
@@ -553,6 +680,15 @@ public:
/** \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 perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /** \brief perform collision test with objects belonging to another manager */
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /** \brief perform distance test with objects belonging to another manager */
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
/** \brief whether the manager is empty */
bool empty() const;
@@ -594,18 +730,31 @@ protected:
EndPoint* next[3];
/** \brief get the value of the end point */
- const Vec3f& getVal() const
+ inline const Vec3f& getVal() const
{
- if(minmax == 0) return aabb->cached.min_;
- else return aabb->cached.max_;
+ if(minmax) return aabb->cached.max_;
+ else return aabb->cached.min_;
}
/** \brief set the value of the end point */
- Vec3f& getVal()
+ inline Vec3f& getVal()
{
- if(minmax == 0) return aabb->cached.min_;
- else return aabb->cached.max_;
+ if(minmax) return aabb->cached.max_;
+ else return aabb->cached.min_;
}
+
+ inline BVH_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)
+ {
+ if(minmax) return aabb->cached.max_[i];
+ else return aabb->cached.min_[i];
+ }
+
};
/** \brief A pair of objects that are not culling away and should further check collision */
@@ -653,18 +802,38 @@ protected:
bool operator() (const SaPPair& pair)
{
- return (pair.obj1 == obj1 && pair.obj2 == obj2) || (pair.obj1 == obj2 && pair.obj2 == obj1);
+ return ((pair.obj1 == obj1) && (pair.obj2 == obj2)) || ((pair.obj1 == obj2) && (pair.obj2 == obj1));
}
};
+ void updateVelist()
+ {
+ for(int coord = 0; coord < 3; ++coord)
+ {
+ velist[coord].resize(size() * 2);
+ EndPoint* current = elist[coord];
+ size_t id = 0;
+ while(current)
+ {
+ velist[coord][id] = current;
+ current = current->next[coord];
+ id++;
+ }
+ }
+ }
+
/** \brief End point list for x, y, z coordinates */
EndPoint* elist[3];
+
+ std::vector<EndPoint*> velist[3];
/** \brief SAP interval list */
std::list<SaPAABB*> AABB_arr;
/** \brief The pair of objects that should further check for collision */
std::list<SaPPair> overlap_pairs;
+
+ size_t optimal_axis;
};
@@ -706,6 +875,15 @@ public:
/** \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 perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /** \brief perform collision test with objects belonging to another manager */
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /** \brief perform distance test with objects belonging to another manager */
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
/** \brief whether the manager is empty */
bool empty() const;
@@ -713,25 +891,63 @@ public:
inline size_t size() const { return objs_x.size(); }
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 check collision between one object and a list of objects, return value is whether stop is possible */
+ bool checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
+ CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ /** \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;
- void checkDis(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
- CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+
+ bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_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)
+ {
+ // simple sweep and prune method
+ double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
+ double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
+ double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2];
+
+ int axis = 0;
+ if(delta_y > delta_x && delta_y > delta_z)
+ axis = 1;
+ else if(delta_z > delta_y && delta_z > delta_x)
+ axis = 2;
+
+ switch(axis)
+ {
+ case 0:
+ it_beg = objs_x.begin();
+ it_end = objs_x.end();
+ break;
+ case 1:
+ it_beg = objs_y.begin();
+ it_end = objs_y.end();
+ break;
+ case 2:
+ it_beg = objs_z.begin();
+ it_end = objs_z.end();
+ break;
+ }
+
+ return axis;
+ }
- /** \brief Objects sorted according to lower x value */
- std::vector<CollisionObject*> objs_x;
+ /** \brief Objects sorted according to lower x value */
+ std::vector<CollisionObject*> objs_x;
- /** \brief Objects sorted according to lower y value */
- std::vector<CollisionObject*> objs_y;
+ /** \brief Objects sorted according to lower y value */
+ std::vector<CollisionObject*> objs_y;
- /** \brief Objects sorted according to lower z value */
- std::vector<CollisionObject*> objs_z;
+ /** \brief Objects sorted according to lower z value */
+ std::vector<CollisionObject*> objs_z;
- /** \brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly */
- bool setup_;
+ /** \brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly */
+ bool setup_;
};
/** Collision manager based on interval tree */
@@ -772,6 +988,15 @@ public:
/** \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 perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
+ void distance(void* cdata, DistanceCallBack callback) const;
+
+ /** \brief perform collision test with objects belonging to another manager */
+ void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
+
+ /** \brief perform distance test with objects belonging to another manager */
+ void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
+
/** \brief whether the manager is empty */
bool empty() const;
@@ -809,7 +1034,7 @@ protected:
struct SAPInterval : public SimpleInterval
{
CollisionObject* obj;
- SAPInterval(double low_, double high_, CollisionObject* obj_)
+ SAPInterval(double low_, double high_, CollisionObject* obj_) : SimpleInterval()
{
low = low_;
high = high_;
@@ -818,9 +1043,13 @@ protected:
};
- void checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+ 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;
- void checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback) const;
+ bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const;
/** \brief vector stores all the end points */
std::vector<EndPoint> endpoints[3];
@@ -834,6 +1063,353 @@ protected:
};
+class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+ typedef NodeBase<AABB> DynamicAABBNode;
+ typedef boost::unordered_map<CollisionObject*, DynamicAABBNode*> DynamicAABBTable;
+
+ int max_tree_nonbalanced_level;
+ int tree_incremental_balance_pass;
+ int tree_topdown_balance_threshold;
+
+ DynamicAABBTreeCollisionManager()
+ {
+ max_tree_nonbalanced_level = 4;
+ tree_incremental_balance_pass = 10;
+ tree_topdown_balance_threshold = 128;
+ }
+
+ /** \brief add one object to the manager */
+ void registerObject(CollisionObject* obj)
+ {
+ DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
+ table[obj] = node;
+ }
+
+ /** \brief remove one object from the manager */
+ void unregisterObject(CollisionObject* obj)
+ {
+ DynamicAABBNode* node = table[obj];
+ table.erase(obj);
+ dtree.remove(node);
+ }
+
+ /** \brief initialize the manager, related with the specific type of manager */
+ void setup()
+ {
+ size_t height = dtree.getMaxHeight(dtree.getRoot());
+ size_t num = dtree.size();
+ if(height - std::log((BVH_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
+ dtree.balanceIncremental(10);
+ else
+ dtree.balanceTopdown(tree_topdown_balance_threshold);
+ }
+
+ /** \brief update the condition of manager */
+ void update()
+ {
+ for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it)
+ {
+ CollisionObject* obj = it->first;
+ DynamicAABBNode* node = it->second;
+ if(!node->bv.equal(obj->getAABB()))
+ dtree.update(node, obj->getAABB());
+ }
+ }
+
+ /** \brief update a specific object */
+ void update(CollisionObject* obj)
+ {
+ DynamicAABBTable::const_iterator it = table.find(obj);
+ if(it != table.end())
+ {
+ DynamicAABBNode* node = it->second;
+ if(!node->bv.equal(obj->getAABB()))
+ dtree.update(node, obj->getAABB());
+ }
+ }
+
+ /** \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.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
+ {
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ distanceRecurse(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.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
+ {
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ selfDistanceRecurse(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
+ {
+ DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
+ collisionRecurse(dtree.getRoot(), 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
+ {
+ DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ distanceRecurse(dtree.getRoot(), 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:
+ HierarchyTree<AABB> dtree;
+ boost::unordered_map<CollisionObject*, DynamicAABBNode*> table;
+
+ bool collisionRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) const
+ {
+ 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(root1->childs[0], root2, cdata, callback))
+ return true;
+ if(collisionRecurse(root1->childs[1], root2, cdata, callback))
+ return true;
+ }
+ else
+ {
+ if(collisionRecurse(root1, root2->childs[0], cdata, callback))
+ return true;
+ if(collisionRecurse(root1, root2->childs[1], cdata, callback))
+ return true;
+ }
+ return false;
+ }
+
+ bool collisionRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) const
+ {
+ 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 = select(query->getAABB(), *(root->childs[0]), *(root->childs[1]));
+
+ if(collisionRecurse(root->childs[select_res], query, cdata, callback))
+ return true;
+
+ if(collisionRecurse(root->childs[1-select_res], query, cdata, callback))
+ return true;
+
+ return false;
+ }
+
+ bool selfCollisionRecurse(DynamicAABBNode* root, void* cdata, CollisionCallBack callback) const
+ {
+ if(root->isLeaf()) return false;
+
+ if(selfCollisionRecurse(root->childs[0], cdata, callback))
+ return true;
+
+ if(selfCollisionRecurse(root->childs[1], cdata, callback))
+ return true;
+
+ if(collisionRecurse(root->childs[0], root->childs[1], cdata, callback))
+ return true;
+
+ return false;
+ }
+
+ bool distanceRecurse(DynamicAABBNode* root1, DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+ {
+ 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())))
+ {
+ BVH_REAL d1 = root2->bv.distance(root1->childs[0]->bv);
+ BVH_REAL d2 = root2->bv.distance(root1->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], root2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], root2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ BVH_REAL d1 = root1->bv.distance(root2->childs[0]->bv);
+ BVH_REAL d2 = root1->bv.distance(root2->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[0], cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1, root2->childs[1], cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+
+ return false;
+ }
+
+ bool distanceRecurse(DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+ {
+ if(root->isLeaf())
+ {
+ CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
+ 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);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root->childs[0], query, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root->childs[1], query, cdata, callback, min_dist))
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ bool selfDistanceRecurse(DynamicAABBNode* root, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
+ {
+ if(root->isLeaf()) return false;
+
+ if(selfDistanceRecurse(root->childs[0], cdata, callback, min_dist))
+ return true;
+
+ if(selfDistanceRecurse(root->childs[1], cdata, callback, min_dist))
+ return true;
+
+ if(distanceRecurse(root->childs[0], root->childs[1], cdata, callback, min_dist))
+ return true;
+
+ return false;
+ }
+
+
+};
+
+
}
#endif
diff --git a/trunk/fcl/src/BV/AABB.cpp b/trunk/fcl/src/BV/AABB.cpp
index 03faf76f..63099f7b 100644
--- a/trunk/fcl/src/BV/AABB.cpp
+++ b/trunk/fcl/src/BV/AABB.cpp
@@ -52,7 +52,7 @@ AABB::AABB()
BVH_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
{
BVH_REAL result = 0;
- for(unsigned int i = 0; i < 3; ++i)
+ for(size_t i = 0; i < 3; ++i)
{
BVH_REAL amin = min_[i];
BVH_REAL amax = max_[i];
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 33625059..4878e243 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -70,18 +70,24 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
return cdata->done;
}
-bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
+bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, BVH_REAL& dist)
{
DistanceData* cdata = static_cast<DistanceData*>(cdata_);
- if(cdata->done) return true;
+ if(cdata->done) { dist = cdata->min_distance; return true; }
BVH_REAL d = distance(o1, o2);
if(cdata->min_distance > d) cdata->min_distance = d;
+ dist = cdata->min_distance;
return cdata->done;
}
+void NaiveCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
+}
+
void NaiveCollisionManager::unregisterObject(CollisionObject* obj)
{
objs.remove(obj);
@@ -130,10 +136,14 @@ 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();
for(it = objs.begin(); it != objs.end(); ++it)
{
- if(callback(obj, *it, cdata))
- return;
+ if(obj->getAABB().distance((*it)->getAABB()) < min_dist)
+ {
+ if(callback(obj, *it, cdata, min_dist))
+ return;
+ }
}
}
@@ -154,6 +164,75 @@ void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) con
}
}
+void NaiveCollisionManager::distance(void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0) return;
+
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ std::list<CollisionObject*>::const_iterator it1;
+ std::list<CollisionObject*>::const_iterator it2;
+ for(it1 = objs.begin(); it1 != objs.end(); ++it1)
+ {
+ it2 = it1; it2++;
+ for(; it2 != objs.end(); ++it2)
+ {
+ if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
+ {
+ if(callback(*it1, *it2, cdata, min_dist))
+ return;
+ }
+ }
+ }
+}
+
+void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ std::list<CollisionObject*>::const_iterator it1;
+ std::list<CollisionObject*>::const_iterator it2;
+ for(it1 = objs.begin(); it1 != objs.end(); ++it1)
+ {
+ for(it2 = other_manager->objs.begin(); it2 != other_manager->objs.end(); ++it2)
+ {
+ if(callback((*it1), (*it2), cdata))
+ return;
+ }
+ }
+}
+
+void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ std::list<CollisionObject*>::const_iterator it1;
+ std::list<CollisionObject*>::const_iterator it2;
+ for(it1 = objs.begin(); it1 != objs.end(); ++it1)
+ {
+ for(it2 = other_manager->objs.begin(); it2 != other_manager->objs.end(); ++it2)
+ {
+ if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist)
+ {
+ if(callback(*it1, *it2, cdata, min_dist))
+ return;
+ }
+ }
+ }
+}
+
bool NaiveCollisionManager::empty() const
{
return objs.empty();
@@ -292,28 +371,57 @@ void SSaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
std::copy(objs_x.begin(), objs_x.end(), objs.begin());
}
-void SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
+bool SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
while(pos_start < pos_end)
{
- if((*pos_start)->getAABB().overlap(obj->getAABB()))
+ if(*pos_start != obj) // no collision between the same object
{
- if(callback(*pos_start, obj, cdata))
- return;
+ if((*pos_start)->getAABB().overlap(obj->getAABB()))
+ {
+ if(callback(*pos_start, obj, cdata))
+ return true;
+ }
+ }
+ pos_start++;
+ }
+ 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
+{
+ while(pos_start < pos_end)
+ {
+ if(*pos_start != obj) // no distance between the same object
+ {
+ if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(*pos_start, obj, cdata, min_dist))
+ return true;
+ }
}
pos_start++;
}
+
+ return false;
}
+
void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
- if(size() == 0) return;
+ collide_(obj, cdata, callback);
+}
+
+bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return false;
static const unsigned int CUTOFF = 100;
DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
+ bool coll_res = false;
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());
@@ -334,47 +442,43 @@ void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionC
if(d3 > CUTOFF)
{
if(d3 <= d2 && d3 <= d1)
- checkColl(pos_start3, pos_end3, obj, cdata, callback);
+ coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
else
{
if(d2 <= d3 && d2 <= d1)
- checkColl(pos_start2, pos_end2, obj, cdata, callback);
+ coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
else
- checkColl(pos_start1, pos_end1, obj, cdata, callback);
+ coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
}
}
else
- checkColl(pos_start3, pos_end3, obj, cdata, callback);
+ coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
}
else
- checkColl(pos_start2, pos_end2, obj, cdata, callback);
+ coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
}
else
- checkColl(pos_start1, pos_end1, obj, cdata, callback);
+ coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
+
+ return coll_res;
}
-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
+{
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ distance_(obj, cdata, callback, min_dist);
}
-void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
{
- if(size() == 0) return;
+ if(size() == 0) return false;
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);
+ if(min_dist < std::numeric_limits<BVH_REAL>::max())
+ dummy_vector += Vec3f(min_dist, min_dist, min_dist);
std::vector<CollisionObject*>::const_iterator pos_start1 = objs_x.begin();
std::vector<CollisionObject*>::const_iterator pos_start2 = objs_y.begin();
@@ -388,12 +492,14 @@ void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata_, Distance
while(1)
{
- old_min_distance = cdata->min_distance;
+ old_min_distance = min_dist;
DummyCollisionObject dummyHigh((AABB(dummy_vector)));
pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
unsigned int d1 = pos_end1 - pos_start1;
+ bool dist_res = false;
+
if(d1 > CUTOFF)
{
pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
@@ -407,42 +513,43 @@ void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata_, Distance
if(d3 > CUTOFF)
{
if(d3 <= d2 && d3 <= d1)
- checkDis(pos_start3, pos_end3, obj, cdata, callback);
+ dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
else
{
if(d2 <= d3 && d2 <= d1)
- checkDis(pos_start2, pos_end2, obj, cdata, callback);
+ dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
else
- checkDis(pos_start1, pos_end1, obj, cdata, callback);
+ dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
}
}
else
- checkDis(pos_start3, pos_end3, obj, cdata, callback);
+ dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
}
else
- checkDis(pos_start2, pos_end2, obj, cdata, callback);
+ dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
}
else
{
- checkDis(pos_start1, pos_end1, obj, cdata, callback);
+ dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
}
+ if(dist_res) return true;
+
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();
+ if(min_dist < 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.
+ min_dist = 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)
+ if(min_dist < old_min_distance)
{
- BVH_REAL d = cdata->min_distance;
- dummy_vector = obj->getAABB().max_ + Vec3f(d, d, d);
+ dummy_vector = obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
status = 0;
}
else // need more loop
@@ -452,55 +559,32 @@ void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata_, Distance
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;
}
+ 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;
}
+
+ return false;
}
void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
{
- if (size() == 0)
+ if(size() == 0)
return;
-
- // simple sweep and prune method
- double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
- double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
- double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2];
-
- int axis = 0;
- if(delta_y > delta_x && delta_y > delta_z)
- axis = 1;
- else if(delta_z > delta_y && delta_z > delta_x)
- axis = 2;
-
- int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
- int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
+ size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z,
+ pos, pos_end);
+ size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
+ size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
- switch(axis)
- {
- case 0:
- pos = objs_x.begin();
- pos_end = objs_x.end();
- break;
- case 1:
- pos = objs_y.begin();
- pos_end = objs_y.end();
- break;
- case 2:
- pos = objs_z.begin();
- pos_end = objs_z.end();
- break;
- }
run_pos = pos;
- while((run_pos != pos_end) && (pos != pos_end))
+ while((run_pos < pos_end) && (pos < pos_end))
{
CollisionObject* obj = *(pos++);
@@ -519,7 +603,7 @@ void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) cons
}
}
- if(run_pos != pos_end)
+ if(run_pos < pos_end)
{
std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
@@ -543,6 +627,69 @@ void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) cons
}
}
+
+void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
+{
+ if(size() == 0)
+ return;
+
+ 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();
+ for(; it != it_end; ++it)
+ {
+ if(distance_(*it, cdata, callback, min_dist))
+ return;
+ }
+}
+
+void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+
+ std::vector<CollisionObject*>::const_iterator it;
+ if(this->size() < other_manager->size())
+ {
+ for(it = objs_x.begin(); it != objs_x.end(); ++it)
+ if(other_manager->collide_(*it, cdata, callback)) return;
+ }
+ else
+ {
+ for(it = other_manager->objs_x.begin(); it != other_manager->objs_x.end(); ++it)
+ if(collide_(*it, cdata, callback)) return;
+ }
+}
+
+void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ std::vector<CollisionObject*>::const_iterator it;
+ if(this->size() < other_manager->size())
+ {
+ for(it = objs_x.begin(); it != objs_x.end(); ++it)
+ if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
+ }
+ else
+ {
+ for(it = other_manager->objs_x.begin(); it != other_manager->objs_x.end(); ++it)
+ if(distance_(*it, cdata, callback, min_dist)) return;
+ }
+}
+
bool SSaPCollisionManager::empty() const
{
return objs_x.empty();
@@ -593,6 +740,90 @@ void SaPCollisionManager::unregisterObject(CollisionObject* obj)
overlap_pairs.remove_if(isUnregistered(obj));
}
+void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
+{
+ if(size() > 0)
+ BroadPhaseCollisionManager::registerObjects(other_objs);
+ else
+ {
+ std::vector<EndPoint*> endpoints(2 * other_objs.size());
+
+ for(size_t i = 0; i < other_objs.size(); ++i)
+ {
+ SaPAABB* sapaabb = new SaPAABB();
+ sapaabb->obj = other_objs[i];
+ sapaabb->lo = new EndPoint();
+ sapaabb->hi = new EndPoint();
+ sapaabb->cached = other_objs[i]->getAABB();
+ endpoints[2 * i] = sapaabb->lo;
+ endpoints[2 * i + 1] = sapaabb->hi;
+ sapaabb->lo->minmax = 0;
+ sapaabb->hi->minmax = 1;
+ sapaabb->lo->aabb = sapaabb;
+ sapaabb->hi->aabb = sapaabb;
+ AABB_arr.push_back(sapaabb);
+ }
+
+
+ BVH_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)));
+
+ endpoints[0]->prev[coord] = NULL;
+ endpoints[0]->next[coord] = endpoints[1];
+ for(int i = 1; i < endpoints.size() - 1; ++i)
+ {
+ endpoints[i]->prev[coord] = endpoints[i-1];
+ endpoints[i]->next[coord] = endpoints[i+1];
+ }
+ endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2];
+ endpoints[endpoints.size() - 1]->next[coord] = NULL;
+
+ elist[coord] = endpoints[0];
+
+ scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord];
+ }
+
+ int axis = 0;
+ if(scale[axis] < scale[1]) axis = 1;
+ if(scale[axis] < scale[2]) axis = 2;
+
+ EndPoint* pos = elist[axis];
+
+ while(pos != NULL)
+ {
+ EndPoint* pos_next = NULL;
+ SaPAABB* aabb = pos->aabb;
+ EndPoint* pos_it = pos->next[axis];
+
+ while(pos_it != NULL)
+ {
+ if(pos_it->aabb == aabb)
+ {
+ if(pos_next == NULL) pos_next = pos_it;
+ break;
+ }
+
+ if(pos_it->minmax == 0)
+ {
+ if(pos_next == NULL) pos_next = pos_it;
+ if(pos_it->aabb->cached.overlap(aabb->cached))
+ overlap_pairs.push_back(SaPPair(pos_it->aabb->obj, aabb->obj));
+ }
+ pos_it = pos_it->next[axis];
+ }
+
+ pos = pos_next;
+ }
+ }
+
+ updateVelist();
+}
+
void SaPCollisionManager::registerObject(CollisionObject* obj)
{
SaPAABB* curr = new SaPAABB;
@@ -606,76 +837,99 @@ void SaPCollisionManager::registerObject(CollisionObject* obj)
curr->hi->minmax = 1;
curr->hi->aabb = curr;
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
- {
- if((*it)->cached.overlap(curr->cached))
- overlap_pairs.push_back(SaPPair(obj, (*it)->obj));
- }
-
- AABB_arr.push_back(curr);
-
for(int coord = 0; coord < 3; ++coord)
{
EndPoint* current = elist[coord];
- // first insert the hi end point
+ // first insert the lo end point
if(current == NULL) // empty list
{
- elist[coord] = curr->hi;
- curr->hi->prev[coord] = curr->hi->next[coord] = NULL;
+ elist[coord] = curr->lo;
+ curr->lo->prev[coord] = curr->lo->next[coord] = NULL;
}
else // otherwise, find the correct location in the list and insert
{
- while((current->next[coord] != NULL) &&
- (current->getVal()[coord] < curr->hi->getVal()[coord]))
+ EndPoint* curr_lo = curr->lo;
+ BVH_REAL curr_lo_val = curr_lo->getVal()[coord];
+ while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != NULL))
current = current->next[coord];
- if(current->getVal()[coord] >= curr->hi->getVal()[coord])
+ if(current->getVal()[coord] >= curr_lo_val)
{
- curr->hi->prev[coord] = current->prev[coord];
- curr->hi->next[coord] = current;
+ curr_lo->prev[coord] = current->prev[coord];
+ curr_lo->next[coord] = current;
if(current->prev[coord] == NULL)
- elist[coord] = curr->hi;
+ elist[coord] = curr_lo;
else
- current->prev[coord]->next[coord] = curr->hi;
+ current->prev[coord]->next[coord] = curr_lo;
+
+ current->prev[coord] = curr_lo;
}
else
{
- curr->hi->prev[coord] = current;
- curr->hi->next[coord] = NULL;
- current->next[coord] = curr->hi;
+ curr_lo->prev[coord] = current;
+ curr_lo->next[coord] = NULL;
+ current->next[coord] = curr_lo;
}
}
- // now insert lo end point
- current = elist[coord];
+ // now insert hi end point
+ current = curr->lo;
+
+ EndPoint* curr_hi = curr->hi;
+ BVH_REAL curr_hi_val = curr_hi->getVal()[coord];
+
+ if(coord == 0)
+ {
+ while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
+ {
+ if(current != curr->lo)
+ if(current->aabb->cached.overlap(curr->cached))
+ overlap_pairs.push_back(SaPPair(current->aabb->obj, obj));
- while ((current->next[coord] != NULL) && (current->getVal()[coord] < curr->lo->getVal()[coord]))
- current = current->next[coord];
+ current = current->next[coord];
+ }
+ }
+ else
+ {
+ while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL))
+ current = current->next[coord];
+ }
- if (current->getVal()[coord] >= curr->lo->getVal()[coord])
+ if(current->getVal()[coord] >= curr_hi_val)
{
- curr->lo->prev[coord] = current->prev[coord];
- curr->lo->next[coord] = current;
+ curr_hi->prev[coord] = current->prev[coord];
+ curr_hi->next[coord] = current;
if(current->prev[coord] == NULL)
- elist[coord] = curr->lo;
+ elist[coord] = curr_hi;
else
- current->prev[coord]->next[coord] = curr->lo;
+ current->prev[coord]->next[coord] = curr_hi;
- current->prev[coord] = curr->lo;
+ current->prev[coord] = curr_hi;
}
else
{
- curr->lo->prev[coord] = current;
- curr->lo->next[coord] = NULL;
- current->next[coord] = curr->lo;
+ curr_hi->prev[coord] = current;
+ curr_hi->next[coord] = NULL;
+ current->next[coord] = curr_hi;
}
}
+
+ AABB_arr.push_back(curr);
+
+ updateVelist();
}
void SaPCollisionManager::setup()
{
-
+ BVH_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);
+ size_t axis = 0;
+ if(scale[axis] < scale[1]) axis = 1;
+ if(scale[axis] < scale[2]) axis = 2;
+ optimal_axis = axis;
}
void SaPCollisionManager::update()
@@ -767,7 +1021,7 @@ void SaPCollisionManager::update()
current->hi->getVal()[coord] = new_max[coord];
}
- else if (direction == 1)
+ else if(direction == 1)
{
//here, we first update the "hi" endpoint.
if(current->hi->next[coord] != NULL)
@@ -828,6 +1082,9 @@ void SaPCollisionManager::update()
}
}
}
+
+ // update velist
+ updateVelist();
}
@@ -849,6 +1106,10 @@ void SaPCollisionManager::clear()
elist[0] = NULL;
elist[1] = NULL;
elist[2] = NULL;
+
+ velist[0].clear();
+ velist[1].clear();
+ velist[2].clear();
}
void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
@@ -864,27 +1125,43 @@ void SaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
if(size() == 0) return;
+
+ size_t axis = optimal_axis;
+ const AABB& obj_aabb = obj->getAABB();
- for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
+ BVH_REAL min_val = obj_aabb.min_[axis];
+ BVH_REAL max_val = obj_aabb.max_[axis];
+
+ std::vector<EndPoint*>::const_iterator res_it = std::lower_bound(velist[axis].begin(), velist[axis].end(), min_val,
+ boost::bind(std::less<BVH_REAL>(),
+ boost::bind(static_cast<BVH_REAL (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ min_val));
+
+ if(res_it == velist[axis].end()) return;
+
+ EndPoint* pos = *res_it;
+
+ while(pos != NULL & pos->getVal(axis) <= max_val)
{
- if((*it)->obj->getAABB().overlap(obj->getAABB()))
+ if(pos->aabb->cached.overlap(obj->getAABB()))
{
- if(callback(obj, (*it)->obj, cdata))
+ if(callback(obj, pos->aabb->obj, cdata))
return;
}
- }
+ pos = pos->next[axis];
+ }
}
-void SaPCollisionManager::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
if(size() == 0) return;
- DistanceData* cdata = static_cast<DistanceData*>(cdata_);
+ 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((*it)->obj->getAABB().distance(obj->getAABB()) < cdata->min_distance)
+ if((*it)->obj->getAABB().distance(obj->getAABB()) < min_dist)
{
- if(callback(obj, (*it)->obj, cdata_))
+ if(callback(obj, (*it)->obj, cdata, min_dist))
return;
}
}
@@ -904,6 +1181,64 @@ void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
}
}
+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;
+ }
+ }
+ }
+}
+
+void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ for(std::list<SaPAABB*>::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it)
+ {
+ for(std::list<SaPAABB*>::const_iterator it2 = other_manager->AABB_arr.begin(); it2 != other_manager->AABB_arr.end(); ++it2)
+ {
+ if(callback((*it)->obj, (*it2)->obj, cdata)) return;
+ }
+ }
+}
+
+void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ 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)
+ {
+ for(std::list<SaPAABB*>::const_iterator it2 = other_manager->AABB_arr.begin(); it2 != other_manager->AABB_arr.end(); ++it2)
+ {
+ if((*it)->obj->getAABB().distance((*it2)->obj->getAABB()) < min_dist)
+ {
+ if(callback((*it)->obj, (*it2)->obj, cdata, min_dist)) return;
+ }
+ }
+ }
+}
+
bool SaPCollisionManager::empty() const
{
return AABB_arr.size();
@@ -1108,10 +1443,14 @@ void IntervalTreeCollisionManager::getObjects(std::vector<CollisionObject*>& obj
}
}
-
void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
- if(size() == 0) return;
+ collide_(obj, cdata, callback);
+}
+
+bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
+{
+ if(size() == 0) return false;
static const unsigned int CUTOFF = 100;
@@ -1131,40 +1470,41 @@ void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, Co
int d3 = results2.size();
if(d1 >= d2 && d1 >= d3)
- checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+ return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
else if(d2 >= d1 && d2 >= d3)
- checkColl(results1.begin(), results1.end(), obj, cdata, callback);
+ return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
else
- checkColl(results2.begin(), results2.end(), obj, cdata, callback);
+ return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
}
else
- checkColl(results2.begin(), results2.end(), obj, cdata, callback);
+ return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
}
else
- checkColl(results1.begin(), results1.end(), obj, cdata, callback);
+ return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
}
else
- checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+ return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
+}
- results0.clear();
- results1.clear();
- results2.clear();
+void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+{
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+ distance_(obj, cdata, callback, min_dist);
}
-void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata_, DistanceCallBack callback) const
+bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, BVH_REAL& min_dist) const
{
- if(size() == 0) return;
+ if(size() == 0) return false;
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())
+ if(min_dist < std::numeric_limits<BVH_REAL>::max())
{
- BVH_REAL d = cdata->min_distance;
- aabb.min_ -= Vec3f(d, d, d);
- aabb.max_ += Vec3f(d, d, d);
+ Vec3f min_dist_delta(min_dist, min_dist, min_dist);
+ aabb.min_ -= min_dist_delta;
+ aabb.max_ += min_dist_delta;
}
int status = 1;
@@ -1172,7 +1512,9 @@ void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata_,
while(1)
{
- old_min_distance = cdata->min_distance;
+ bool dist_res = false;
+
+ old_min_distance = min_dist;
std::deque<SimpleInterval*> results0, results1, results2;
@@ -1190,20 +1532,22 @@ void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata_,
int d3 = results2.size();
if(d1 >= d2 && d1 >= d3)
- checkDist(results0.begin(), results1.end(), obj, cdata, callback);
+ dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
else if(d2 >= d1 && d2 >= d3)
- checkDist(results1.begin(), results1.end(), obj, cdata, callback);
+ dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
else
- checkDist(results2.begin(), results2.end(), obj, cdata, callback);
+ dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
}
else
- checkDist(results2.begin(), results2.end(), obj, cdata, callback);
+ dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
}
else
- checkDist(results1.begin(), results1.end(), obj, cdata, callback);
+ dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
}
else
- checkDist(results0.begin(), results0.end(), obj, cdata, callback);
+ dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
+
+ if(dist_res) return true;
results0.clear();
results1.clear();
@@ -1213,17 +1557,17 @@ void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata_,
{
if(old_min_distance < std::numeric_limits<BVH_REAL>::max())
{
- if(cdata->min_distance < old_min_distance) break;
+ if(min_dist < old_min_distance) break;
else
- cdata->min_distance = std::numeric_limits<BVH_REAL>::max();
+ min_dist = std::numeric_limits<BVH_REAL>::max();
}
else
{
- if(cdata->min_distance < old_min_distance)
+ if(min_dist < 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);
+ 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;
status = 0;
}
else
@@ -1244,6 +1588,8 @@ void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata_,
else if(status == 0)
break;
}
+
+ return false;
}
void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const
@@ -1303,37 +1649,101 @@ void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callba
}
+void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const
+{
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+
+ for(size_t i = 0; i < endpoints[0].size(); ++i)
+ if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return;
+}
+
+void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
+{
+ IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+ if(this == other_manager)
+ {
+ collide(cdata, callback);
+ return;
+ }
+
+ if(this->size() < other_manager->size())
+ {
+ for(size_t i = 0; i < endpoints[0].size(); ++i)
+ if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return;
+ }
+ else
+ {
+ for(size_t i = 0; i < other_manager->endpoints[0].size(); ++i)
+ if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return;
+ }
+}
+
+void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
+{
+ IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+ if(this == other_manager)
+ {
+ distance(cdata, callback);
+ return;
+ }
+
+ BVH_REAL min_dist = std::numeric_limits<BVH_REAL>::max();
+
+ if(this->size() < other_manager->size())
+ {
+ for(size_t i = 0; i < endpoints[0].size(); ++i)
+ if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return;
+ }
+ else
+ {
+ for(size_t i = 0; i < other_manager->endpoints[0].size(); ++i)
+ if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return;
+ }
+}
+
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
+bool 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)
+ while(pos_start < pos_end)
{
- SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
-
- if(ivl->obj->getAABB().overlap(obj->getAABB()))
+ SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
+ if(ivl->obj != obj)
{
- if(callback(ivl->obj, obj, cdata))
- return;
+ if(ivl->obj->getAABB().overlap(obj->getAABB()))
+ {
+ if(callback(ivl->obj, obj, cdata))
+ return true;
+ }
}
pos_start++;
}
+
+ return false;
}
-void IntervalTreeCollisionManager::checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback) const
+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
{
- while(pos_start != pos_end)
+ while(pos_start < pos_end)
{
SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
- if(callback(ivl->obj, obj, cdata))
- return;
+ if(ivl->obj != obj)
+ {
+ if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
+ {
+ if(callback(ivl->obj, obj, cdata, min_dist))
+ return true;
+ }
+ }
pos_start++;
}
+
+ return false;
}
diff --git a/trunk/fcl/src/interval_tree.cpp b/trunk/fcl/src/interval_tree.cpp
index 397cd16b..bcbea9e2 100644
--- a/trunk/fcl/src/interval_tree.cpp
+++ b/trunk/fcl/src/interval_tree.cpp
@@ -50,7 +50,7 @@ IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) :
high(new_interval->high),
max_high(high) {}
-IntervalTreeNode::~IntervalTreeNode(){}
+IntervalTreeNode::~IntervalTreeNode() {}
IntervalTree::IntervalTree()
diff --git a/trunk/fcl/src/traversal_recurse.cpp b/trunk/fcl/src/traversal_recurse.cpp
index 590e61b1..ba876831 100644
--- a/trunk/fcl/src/traversal_recurse.cpp
+++ b/trunk/fcl/src/traversal_recurse.cpp
@@ -184,8 +184,10 @@ void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList*
int c2 = node->getFirstRightChild(b);
selfCollisionRecurse(node, c1, front_list);
+ if(node->canStop() && !front_list) return;
selfCollisionRecurse(node, c2, front_list);
+ if(node->canStop() && !front_list) return;
collisionRecurse(node, c1, c2, front_list);
}
--
GitLab