Commit 46dd3294 authored by jpan's avatar jpan
Browse files

Add the broad phase distance


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