Commit 867ccf61 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Remove broadphase.

parent 00ce8e70
......@@ -117,18 +117,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/BV/kDOP.h
include/hpp/fcl/narrowphase/narrowphase.h
include/hpp/fcl/narrowphase/gjk.h
include/hpp/fcl/broadphase/interval_tree.h
include/hpp/fcl/broadphase/broadphase_spatialhash.h
include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h
include/hpp/fcl/broadphase/broadphase_SSaP.h
include/hpp/fcl/broadphase/broadphase_interval_tree.h
include/hpp/fcl/broadphase/broadphase.h
include/hpp/fcl/broadphase/hierarchy_tree.h
include/hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h
include/hpp/fcl/broadphase/broadphase_SaP.h
include/hpp/fcl/broadphase/broadphase_bruteforce.h
include/hpp/fcl/broadphase/morton.h
include/hpp/fcl/broadphase/hash.h
include/hpp/fcl/shape/geometric_shapes_utility.h
include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
include/hpp/fcl/shape/geometric_shapes.h
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef FCL_BROAD_PHASE_H
#define FCL_BROAD_PHASE_H
#include <hpp/fcl/collision_object.h>
#include <set>
#include <vector>
namespace fcl
{
/// @brief Callback for collision between two objects. Return value is whether can stop now.
typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
/// @brief Callback for distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
/// @brief Base class for broad phase collision. It helps to accelerate the collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects.
class BroadPhaseCollisionManager
{
public:
BroadPhaseCollisionManager() : enable_tested_set_(false)
{
}
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;
/// @brief remove one object from the manager
virtual void unregisterObject(CollisionObject* obj) = 0;
/// @brief initialize the manager, related with the specific type of manager
virtual void setup() = 0;
/// @brief update the condition of manager
virtual void update() = 0;
/// @brief update the manager by explicitly given the object updated
virtual void update(CollisionObject* /*updated_obj*/)
{
update();
}
/// @brief update the manager by explicitly given the set of objects update
virtual void update(const std::vector<CollisionObject*>& /*updated_objs*/)
{
update();
}
/// @brief clear the manager
virtual void clear() = 0;
/// @brief return the objects managed by the manager
virtual void getObjects(std::vector<CollisionObject*>& objs) const = 0;
/// @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;
/// @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;
/// @brief the number of objects managed by the manager
virtual size_t size() const = 0;
protected:
/// @brief tools help to avoid repeating collision or distance callback for the pairs of objects tested before. It can be useful for some of the broadphase algorithms.
mutable std::set<std::pair<CollisionObject*, CollisionObject*> > tested_set;
mutable bool enable_tested_set_;
bool inTestedSet(CollisionObject* a, CollisionObject* b) const
{
if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end();
else return tested_set.find(std::make_pair(b, a)) != tested_set.end();
}
void insertTestedSet(CollisionObject* a, CollisionObject* b) const
{
if(a < b) tested_set.insert(std::make_pair(a, b));
else tested_set.insert(std::make_pair(b, a));
}
};
}
#include <hpp/fcl/broadphase/broadphase_bruteforce.h>
#include <hpp/fcl/broadphase/broadphase_spatialhash.h>
#include <hpp/fcl/broadphase/broadphase_SaP.h>
#include <hpp/fcl/broadphase/broadphase_SSaP.h>
#include <hpp/fcl/broadphase/broadphase_interval_tree.h>
#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h>
#include <hpp/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h>
#endif
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef FCL_BROAD_PHASE_SSAP_H
#define FCL_BROAD_PHASE_SSAP_H
#include <hpp/fcl/broadphase/broadphase.h>
namespace fcl
{
/// @brief Simple SAP collision manager
class SSaPCollisionManager : public BroadPhaseCollisionManager
{
public:
SSaPCollisionManager() : setup_(false)
{}
/// @brief remove one object from the manager
void registerObject(CollisionObject* obj);
/// @brief add one object to 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 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;
/// @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;
/// @brief the number of objects managed by the manager
inline size_t size() const { return objs_x.size(); }
protected:
/// @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, FCL_REAL& min_dist) const;
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_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 y value
std::vector<CollisionObject*> objs_y;
/// @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_;
};
}
#endif
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
#ifndef FCL_BROAD_PHASE_SAP_H
#define FCL_BROAD_PHASE_SAP_H
#include <hpp/fcl/broadphase/broadphase.h>
#include <map>
#include <list>
namespace fcl
{
/// @brief Rigorous SAP collision manager
class SaPCollisionManager : public BroadPhaseCollisionManager
{
public:
SaPCollisionManager()
{
elist[0] = NULL;
elist[1] = NULL;
elist[2] = NULL;
optimal_axis = 0;
}
~SaPCollisionManager()
{
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);
/// @brief add one object to 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 update the manager by explicitly given the object updated
void update(CollisionObject* updated_obj);
/// @brief update the manager by explicitly given the set of objects update
void update(const std::vector<CollisionObject*>& updated_objs);
/// @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 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;
/// @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;
/// @brief the number of objects managed by the manager
inline size_t size() const { return AABB_arr.size(); }
protected:
struct EndPoint;
/// @brief SAP interval for one object
struct SaPAABB
{
/// @brief object
CollisionObject* obj;
/// @brief lower bound end point of the interval
EndPoint* lo;
/// @brief higher bound end point of the interval
EndPoint* hi;
/// @brief cached AABB value
AABB cached;
};
/// @brief End point for an interval
struct EndPoint
{
/// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi
char minmax;
/// @brief back pointer to SAP interval
SaPAABB* aabb;
/// @brief the previous end point in the end point list
EndPoint* prev[3];
/// @brief the next end point in the end point list
EndPoint* next[3];
/// @brief get the value of the end point
inline const Vec3f& getVal() const
{
if(minmax) return aabb->cached.max_;
else return aabb->cached.min_;
}
/// @brief set the value of the end point
inline Vec3f& getVal()
{
if(minmax) return aabb->cached.max_;
else return aabb->cached.min_;
}
inline Vec3f::Scalar getVal(size_t i) const
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
}
inline Vec3f::Scalar& 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
struct SaPPair
{
SaPPair(CollisionObject* a, CollisionObject* b)
{
if(a < b)
{
obj1 = a;
obj2 = b;
}
else
{
obj1 = b;
obj2 = a;
}
}
CollisionObject* obj1;
CollisionObject* obj2;
bool operator == (const SaPPair& other) const
{
return ((obj1 == other.obj1) && (obj2 == other.obj2));
}
};
/// @brief Functor to help unregister one object
class isUnregistered
{
CollisionObject* obj;
public:
isUnregistered(CollisionObject* obj_) : obj(obj_)
{}
bool operator() (const SaPPair& pair) const
{
return (pair.obj1 == obj) || (pair.obj2 == obj);
}
};
/// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away)
class isNotValidPair
{
CollisionObject* obj1;
CollisionObject* obj2;
public:
isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_),
obj2(obj2_)
{}
bool operator() (const SaPPair& pair)
{
return (pair.obj1 == obj1) && (pair.obj2 == obj2);
}
};
void update_(SaPAABB* updated_aabb);
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];
/// @brief vector version of elist, for acceleration
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;