Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/hpp-fcl
  • coal-library/coal
2 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 4410 additions and 329 deletions
include/fcl/broadphase/broadphase_interval_tree.h → include/coal/broadphase/broadphase_interval_tree.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -30,35 +31,31 @@
* 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 */
/** @author Jia Pan */
#ifndef FCL_BROAD_PHASE_INTERVAL_TREE_H
#define FCL_BROAD_PHASE_INTERVAL_TREE_H
#ifndef COAL_BROAD_PHASE_INTERVAL_TREE_H
#define COAL_BROAD_PHASE_INTERVAL_TREE_H
#include "fcl/broadphase/broadphase.h"
#include "fcl/broadphase/interval_tree.h"
#include <deque>
#include <map>
namespace fcl
{
#include "coal/broadphase/broadphase_collision_manager.h"
#include "coal/broadphase/detail/interval_tree.h"
namespace coal {
/// @brief Collision manager based on interval tree
class IntervalTreeCollisionManager : public BroadPhaseCollisionManager
{
public:
IntervalTreeCollisionManager() : setup_(false)
{
for(int i = 0; i < 3; ++i)
interval_trees[i] = NULL;
}
~IntervalTreeCollisionManager()
{
clear();
}
class COAL_DLLAPI IntervalTreeCollisionManager
: public BroadPhaseCollisionManager {
public:
typedef BroadPhaseCollisionManager Base;
using Base::getObjects;
IntervalTreeCollisionManager();
~IntervalTreeCollisionManager();
/// @brief remove one object from the manager
void registerObject(CollisionObject* obj);
......@@ -70,7 +67,7 @@ public:
void setup();
/// @brief update the condition of manager
void update();
virtual void update();
/// @brief update the manager by explicitly given the object updated
void update(CollisionObject* updated_obj);
......@@ -84,72 +81,82 @@ public:
/// @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 collision test between one object and all the objects
/// belonging to the manager
void collide(CollisionObject* obj, CollisionCallBackBase* 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 distance computation between one object and all the objects
/// belonging to the manager
void distance(CollisionObject* obj, DistanceCallBackBase* 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 collision test for the objects belonging to the manager
/// (i.e., N^2 self collision)
void collide(CollisionCallBackBase* 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 distance test for the objects belonging to the manager
/// (i.e., N^2 self distance)
void distance(DistanceCallBackBase* callback) const;
/// @brief perform collision test with objects belonging to another manager
void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
void collide(BroadPhaseCollisionManager* other_manager,
CollisionCallBackBase* callback) const;
/// @brief perform distance test with objects belonging to another manager
void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
void distance(BroadPhaseCollisionManager* other_manager,
DistanceCallBackBase* 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 endpoints[0].size() / 2; }
protected:
/// @brief the number of objects managed by the manager
size_t size() const;
protected:
/// @brief SAP end point
struct EndPoint
{
/// @brief SAP end point
struct COAL_DLLAPI EndPoint {
/// @brief object related with the end point
CollisionObject* obj;
/// @brief end point value
FCL_REAL value;
CoalScalar value;
/// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi
/// @brief tag for whether it is a lower bound or higher bound of an
/// interval, 0 for lo, and 1 for hi
char minmax;
bool operator<(const EndPoint& p) const;
};
/// @brief Extention interval tree's interval to SAP interval, adding more information
struct SAPInterval : public SimpleInterval
{
/// @brief Extention interval tree's interval to SAP interval, adding more
/// information
struct COAL_DLLAPI SAPInterval : public detail::SimpleInterval {
CollisionObject* obj;
SAPInterval(double low_, double high_, CollisionObject* obj_) : SimpleInterval()
{
low = low_;
high = high_;
obj = obj_;
}
};
SAPInterval(CoalScalar low_, CoalScalar high_, CollisionObject* obj_);
};
bool checkColl(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
bool checkColl(
typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
CollisionObject* obj, CollisionCallBackBase* callback) const;
bool checkDist(std::deque<SimpleInterval*>::const_iterator pos_start, std::deque<SimpleInterval*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool checkDist(
typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
CollisionObject* obj, DistanceCallBackBase* callback,
CoalScalar& min_dist) const;
bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
CoalScalar& min_dist) const;
/// @brief vector stores all the end points
std::vector<EndPoint> endpoints[3];
/// @brief interval tree manages the intervals
IntervalTree* interval_trees[3];
detail::IntervalTree* interval_trees[3];
std::map<CollisionObject*, SAPInterval*> obj_interval_maps[3];
......@@ -157,7 +164,6 @@ protected:
bool setup_;
};
}
} // namespace coal
#endif
include/coal/broadphase/broadphase_spatialhash-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
#include "coal/broadphase/broadphase_spatialhash.h"
namespace coal {
//==============================================================================
template <typename HashTable>
SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager(
CoalScalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max,
unsigned int default_table_size)
: scene_limit(AABB(scene_min, scene_max)),
hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
hash_table->init(default_table_size);
}
//==============================================================================
template <typename HashTable>
SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() {
delete hash_table;
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::registerObject(
CollisionObject* obj) {
objs.push_back(obj);
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
if (!scene_limit.contain(obj_aabb))
objs_partially_penetrating_scene_limit.push_back(obj);
hash_table->insert(overlap_aabb, obj);
} else {
objs_outside_scene_limit.push_back(obj);
}
obj_aabb_map[obj] = obj_aabb;
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::unregisterObject(
CollisionObject* obj) {
objs.remove(obj);
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
if (!scene_limit.contain(obj_aabb))
objs_partially_penetrating_scene_limit.remove(obj);
hash_table->remove(overlap_aabb, obj);
} else {
objs_outside_scene_limit.remove(obj);
}
obj_aabb_map.erase(obj);
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::setup() {
// Do nothing
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update() {
hash_table->clear();
objs_partially_penetrating_scene_limit.clear();
objs_outside_scene_limit.clear();
for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
CollisionObject* obj = *it;
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
if (!scene_limit.contain(obj_aabb))
objs_partially_penetrating_scene_limit.push_back(obj);
hash_table->insert(overlap_aabb, obj);
} else {
objs_outside_scene_limit.push_back(obj);
}
obj_aabb_map[obj] = obj_aabb;
}
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update(
CollisionObject* updated_obj) {
const AABB& new_aabb = updated_obj->getAABB();
const AABB& old_aabb = obj_aabb_map[updated_obj];
AABB old_overlap_aabb;
const auto is_old_aabb_overlapping =
scene_limit.overlap(old_aabb, old_overlap_aabb);
if (is_old_aabb_overlapping)
hash_table->remove(old_overlap_aabb, updated_obj);
AABB new_overlap_aabb;
const auto is_new_aabb_overlapping =
scene_limit.overlap(new_aabb, new_overlap_aabb);
if (is_new_aabb_overlapping)
hash_table->insert(new_overlap_aabb, updated_obj);
ObjectStatus old_status;
if (is_old_aabb_overlapping) {
if (scene_limit.contain(old_aabb))
old_status = Inside;
else
old_status = PartiallyPenetrating;
} else {
old_status = Outside;
}
if (is_new_aabb_overlapping) {
if (scene_limit.contain(new_aabb)) {
if (old_status == PartiallyPenetrating) {
// Status change: PartiallyPenetrating --> Inside
// Required action(s):
// - remove object from "objs_partially_penetrating_scene_limit"
auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
objs_partially_penetrating_scene_limit.end(),
updated_obj);
objs_partially_penetrating_scene_limit.erase(find_it);
} else if (old_status == Outside) {
// Status change: Outside --> Inside
// Required action(s):
// - remove object from "objs_outside_scene_limit"
auto find_it = std::find(objs_outside_scene_limit.begin(),
objs_outside_scene_limit.end(), updated_obj);
objs_outside_scene_limit.erase(find_it);
}
} else {
if (old_status == Inside) {
// Status change: Inside --> PartiallyPenetrating
// Required action(s):
// - add object to "objs_partially_penetrating_scene_limit"
objs_partially_penetrating_scene_limit.push_back(updated_obj);
} else if (old_status == Outside) {
// Status change: Outside --> PartiallyPenetrating
// Required action(s):
// - remove object from "objs_outside_scene_limit"
// - add object to "objs_partially_penetrating_scene_limit"
auto find_it = std::find(objs_outside_scene_limit.begin(),
objs_outside_scene_limit.end(), updated_obj);
objs_outside_scene_limit.erase(find_it);
objs_partially_penetrating_scene_limit.push_back(updated_obj);
}
}
} else {
if (old_status == Inside) {
// Status change: Inside --> Outside
// Required action(s):
// - add object to "objs_outside_scene_limit"
objs_outside_scene_limit.push_back(updated_obj);
} else if (old_status == PartiallyPenetrating) {
// Status change: PartiallyPenetrating --> Outside
// Required action(s):
// - remove object from "objs_partially_penetrating_scene_limit"
// - add object to "objs_outside_scene_limit"
auto find_it =
std::find(objs_partially_penetrating_scene_limit.begin(),
objs_partially_penetrating_scene_limit.end(), updated_obj);
objs_partially_penetrating_scene_limit.erase(find_it);
objs_outside_scene_limit.push_back(updated_obj);
}
}
obj_aabb_map[updated_obj] = new_aabb;
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::update(
const std::vector<CollisionObject*>& updated_objs) {
for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::clear() {
objs.clear();
hash_table->clear();
objs_outside_scene_limit.clear();
obj_aabb_map.clear();
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::getObjects(
std::vector<CollisionObject*>& objs_) const {
objs_.resize(objs.size());
std::copy(objs.begin(), objs.end(), objs_.begin());
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(
CollisionObject* obj, CollisionCallBackBase* callback) const {
if (size() == 0) return;
collide_(obj, callback);
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(
CollisionObject* obj, DistanceCallBackBase* callback) const {
if (size() == 0) return;
CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
distance_(obj, callback, min_dist);
}
//==============================================================================
template <typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::collide_(
CollisionObject* obj, CollisionCallBackBase* callback) const {
const auto& obj_aabb = obj->getAABB();
AABB overlap_aabb;
if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
const auto query_result = hash_table->query(overlap_aabb);
for (const auto& obj2 : query_result) {
if (obj == obj2) continue;
if ((*callback)(obj, obj2)) return true;
}
if (!scene_limit.contain(obj_aabb)) {
for (const auto& obj2 : objs_outside_scene_limit) {
if (obj == obj2) continue;
if ((*callback)(obj, obj2)) return true;
}
}
} else {
for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
if (obj == obj2) continue;
if ((*callback)(obj, obj2)) return true;
}
for (const auto& obj2 : objs_outside_scene_limit) {
if (obj == obj2) continue;
if ((*callback)(obj, obj2)) return true;
}
}
return false;
}
//==============================================================================
template <typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::distance_(
CollisionObject* obj, DistanceCallBackBase* callback,
CoalScalar& min_dist) const {
auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
auto aabb = obj->getAABB();
if (min_dist < (std::numeric_limits<CoalScalar>::max)()) {
Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb.expand(min_dist_delta);
}
AABB overlap_aabb;
auto status = 1;
CoalScalar old_min_distance;
while (1) {
old_min_distance = min_dist;
if (scene_limit.overlap(aabb, overlap_aabb)) {
if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
callback, min_dist)) {
return true;
}
if (!scene_limit.contain(aabb)) {
if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
min_dist)) {
return true;
}
}
} else {
if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
callback, min_dist)) {
return true;
}
if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
min_dist)) {
return true;
}
}
if (status == 1) {
if (old_min_distance < (std::numeric_limits<CoalScalar>::max)()) {
break;
} else {
if (min_dist < old_min_distance) {
Vec3s min_dist_delta(min_dist, min_dist, min_dist);
aabb = AABB(obj->getAABB(), min_dist_delta);
status = 0;
} else {
if (aabb == obj->getAABB())
aabb.expand(delta);
else
aabb.expand(obj->getAABB(), 2.0);
}
}
} else if (status == 0) {
break;
}
}
return false;
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(
CollisionCallBackBase* callback) const {
if (size() == 0) return;
for (const auto& obj1 : objs) {
const auto& obj_aabb = obj1->getAABB();
AABB overlap_aabb;
if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
auto query_result = hash_table->query(overlap_aabb);
for (const auto& obj2 : query_result) {
if (obj1 < obj2) {
if ((*callback)(obj1, obj2)) return;
}
}
if (!scene_limit.contain(obj_aabb)) {
for (const auto& obj2 : objs_outside_scene_limit) {
if (obj1 < obj2) {
if ((*callback)(obj1, obj2)) return;
}
}
}
} else {
for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
if (obj1 < obj2) {
if ((*callback)(obj1, obj2)) return;
}
}
for (const auto& obj2 : objs_outside_scene_limit) {
if (obj1 < obj2) {
if ((*callback)(obj1, obj2)) return;
}
}
}
}
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(
DistanceCallBackBase* callback) const {
if (size() == 0) return;
this->enable_tested_set_ = true;
this->tested_set.clear();
CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
for (const auto& obj : objs) {
if (distance_(obj, callback, min_dist)) break;
}
this->enable_tested_set_ = false;
this->tested_set.clear();
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(
BroadPhaseCollisionManager* other_manager_,
CollisionCallBackBase* callback) const {
auto* other_manager =
static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
if ((size() == 0) || (other_manager->size() == 0)) return;
if (this == other_manager) {
collide(callback);
return;
}
if (this->size() < other_manager->size()) {
for (const auto& obj : objs) {
if (other_manager->collide_(obj, callback)) return;
}
} else {
for (const auto& obj : other_manager->objs) {
if (collide_(obj, callback)) return;
}
}
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(
BroadPhaseCollisionManager* other_manager_,
DistanceCallBackBase* callback) const {
auto* other_manager =
static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
if ((size() == 0) || (other_manager->size() == 0)) return;
if (this == other_manager) {
distance(callback);
return;
}
CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
if (this->size() < other_manager->size()) {
for (const auto& obj : objs)
if (other_manager->distance_(obj, callback, min_dist)) return;
} else {
for (const auto& obj : other_manager->objs)
if (distance_(obj, callback, min_dist)) return;
}
}
//==============================================================================
template <typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::empty() const {
return objs.empty();
}
//==============================================================================
template <typename HashTable>
size_t SpatialHashingCollisionManager<HashTable>::size() const {
return objs.size();
}
//==============================================================================
template <typename HashTable>
void SpatialHashingCollisionManager<HashTable>::computeBound(
std::vector<CollisionObject*>& objs, Vec3s& l, Vec3s& u) {
AABB bound;
for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
l = bound.min_;
u = bound.max_;
}
//==============================================================================
template <typename HashTable>
template <typename Container>
bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects(
CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
CoalScalar& min_dist) const {
for (auto& obj2 : objs) {
if (obj == obj2) continue;
if (!this->enable_tested_set_) {
if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
if ((*callback)(obj, obj2, min_dist)) return true;
}
} else {
if (!this->inTestedSet(obj, obj2)) {
if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
if ((*callback)(obj, obj2, min_dist)) return true;
}
this->insertTestedSet(obj, obj2);
}
}
}
return false;
}
} // namespace coal
#endif
include/fcl/broadphase/broadphase_spatialhash.h → include/coal/broadphase/broadphase_spatialhash.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -30,79 +31,36 @@
* 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 */
/** @author Jia Pan */
#ifndef FCL_BROAD_PHASE_SPATIAL_HASH_H
#define FCL_BROAD_PHASE_SPATIAL_HASH_H
#ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_H
#define COAL_BROADPHASE_BROADPAHSESPATIALHASH_H
#include "fcl/broadphase/broadphase.h"
#include "fcl/broadphase/hash.h"
#include "fcl/BV/AABB.h"
#include <list>
#include <map>
#include "coal/BV/AABB.h"
#include "coal/broadphase/broadphase_collision_manager.h"
#include "coal/broadphase/detail/simple_hash_table.h"
#include "coal/broadphase/detail/sparse_hash_table.h"
#include "coal/broadphase/detail/spatial_hash.h"
namespace fcl
{
/// @brief Spatial hash function: hash an AABB to a set of integer values
struct SpatialHash
{
SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) : cell_size(cell_size_),
scene_limit(scene_limit_)
{
width[0] = std::ceil(scene_limit.width() / cell_size);
width[1] = std::ceil(scene_limit.height() / cell_size);
width[2] = std::ceil(scene_limit.depth() / cell_size);
}
std::vector<unsigned int> operator() (const AABB& aabb) const
{
int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size);
std::vector<unsigned int> keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z));
int id = 0;
for(int x = min_x; x < max_x; ++x)
{
for(int y = min_y; y < max_y; ++y)
{
for(int z = min_z; z < max_z; ++z)
{
keys[id++] = x + y * width[0] + z * width[0] * width[1];
}
}
}
return keys;
}
private:
FCL_REAL cell_size;
AABB scene_limit;
unsigned int width[3];
};
namespace coal {
/// @brief spatial hashing collision mananger
template<typename HashTable = SimpleHashTable<AABB, CollisionObject*, SpatialHash> >
class SpatialHashingCollisionManager : public BroadPhaseCollisionManager
{
public:
SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000) : scene_limit(AABB(scene_min, scene_max)),
hash_table(new HashTable(SpatialHash(scene_limit, cell_size)))
{
hash_table->init(default_table_size);
}
~SpatialHashingCollisionManager()
{
delete hash_table;
}
template <typename HashTable = detail::SimpleHashTable<AABB, CollisionObject*,
detail::SpatialHash> >
class SpatialHashingCollisionManager : public BroadPhaseCollisionManager {
public:
typedef BroadPhaseCollisionManager Base;
using Base::getObjects;
SpatialHashingCollisionManager(CoalScalar cell_size, const Vec3s& scene_min,
const Vec3s& scene_max,
unsigned int default_table_size = 1000);
~SpatialHashingCollisionManager();
/// @brief add one object to the manager
void registerObject(CollisionObject* obj);
......@@ -114,7 +72,7 @@ public:
void setup();
/// @brief update the condition of manager
void update();
virtual void update();
/// @brief update the manager by explicitly given the object updated
void update(CollisionObject* updated_obj);
......@@ -128,23 +86,29 @@ public:
/// @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 collision test between one object and all the objects
/// belonging to the manager
void collide(CollisionObject* obj, CollisionCallBackBase* 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 distance computation between one object and all the objects
/// belonging ot the manager
void distance(CollisionObject* obj, DistanceCallBackBase* 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 collision test for the objects belonging to the manager
/// (i.e, N^2 self collision)
void collide(CollisionCallBackBase* 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 distance test for the objects belonging to the manager
/// (i.e., N^2 self distance)
void distance(DistanceCallBackBase* callback) const;
/// @brief perform collision test with objects belonging to another manager
void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const;
void collide(BroadPhaseCollisionManager* other_manager,
CollisionCallBackBase* callback) const;
/// @brief perform distance test with objects belonging to another manager
void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const;
void distance(BroadPhaseCollisionManager* other_manager,
DistanceCallBackBase* callback) const;
/// @brief whether the manager is empty
bool empty() const;
......@@ -153,46 +117,51 @@ public:
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_;
}
protected:
static void computeBound(std::vector<CollisionObject*>& objs, Vec3s& l,
Vec3s& u);
/// @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, FCL_REAL& min_dist) const;
protected:
/// @brief perform collision test between one object and all the objects
/// belonging to the manager
bool collide_(CollisionObject* obj, CollisionCallBackBase* callback) const;
/// @brief perform distance computation between one object and all the objects
/// belonging ot the manager
bool distance_(CollisionObject* obj, DistanceCallBackBase* callback,
CoalScalar& min_dist) const;
/// @brief all objects in the scene
std::list<CollisionObject*> objs;
/// @brief objects partially penetrating (not totally inside nor outside) the
/// scene limit are in another list
std::list<CollisionObject*> objs_partially_penetrating_scene_limit;
/// @brief objects outside the scene limit are in another list
std::list<CollisionObject*> objs_outside_scene_limit;
/// @brief the size of the scene
AABB scene_limit;
/// @brief store the map between objects and their aabbs. will make update more convenient
std::map<CollisionObject*, AABB> obj_aabb_map;
/// @brief store the map between objects and their aabbs. will make update
/// more convenient
std::map<CollisionObject*, AABB> obj_aabb_map;
/// @brief objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table
/// @brief objects in the scene limit (given by scene_min and scene_max) are
/// in the spatial hash table
HashTable* hash_table;
};
private:
enum ObjectStatus { Inside, PartiallyPenetrating, Outside };
template <typename Container>
bool distanceObjectToObjects(CollisionObject* obj, const Container& objs,
DistanceCallBackBase* callback,
CoalScalar& min_dist) const;
};
}
#include "fcl/broadphase/broadphase_spatialhash.hxx"
} // namespace coal
#include "coal/broadphase/broadphase_spatialhash-inl.h"
#endif
include/coal/broadphase/default_broadphase_callbacks.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Toyota Research Institute
* Copyright (c) 2022-2023, INRIA
* 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 the copyright holder 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 Sean Curtis (sean@tri.global) */
/** @author Justin Carpentier (justin.carpentier@inria.fr) */
#ifndef COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
#define COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
#include "coal/broadphase/broadphase_callbacks.h"
#include "coal/collision.h"
#include "coal/distance.h"
// #include "coal/narrowphase/continuous_collision.h"
// #include "coal/narrowphase/continuous_collision_request.h"
// #include "coal/narrowphase/continuous_collision_result.h"
// #include "coal/narrowphase/distance_request.h"
// #include "coal/narrowphase/distance_result.h"
namespace coal {
/// @brief Collision data stores the collision request and the result given by
/// collision algorithm.
struct CollisionData {
CollisionData() { done = false; }
/// @brief Collision request
CollisionRequest request;
/// @brief Collision result
CollisionResult result;
/// @brief Whether the collision iteration can stop
bool done;
/// @brief Clears the CollisionData
void clear() {
result.clear();
done = false;
}
};
/// @brief Distance data stores the distance request and the result given by
/// distance algorithm.
struct DistanceData {
DistanceData() { done = false; }
/// @brief Distance request
DistanceRequest request;
/// @brief Distance result
DistanceResult result;
/// @brief Whether the distance iteration can stop
bool done;
/// @brief Clears the DistanceData
void clear() {
result.clear();
done = false;
}
};
/// @brief Provides a simple callback for the collision query in the
/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
/// points to an instance of CollisionData. It simply invokes the
/// `collide()` method on the culled pair of geometries and stores the results
/// in the data's CollisionResult instance.
///
/// This callback will cause the broadphase evaluation to stop if:
/// - the collision requests _disables_ cost _and_
/// - the collide() reports a collision for the culled pair, _and_
/// - we've reported the number of contacts requested in the CollisionRequest.
///
/// For a given instance of CollisionData, if broadphase evaluation has
/// already terminated (i.e., defaultCollisionFunction() returned `true`),
/// subsequent invocations with the same instance of CollisionData will
/// return immediately, requesting termination of broadphase evaluation (i.e.,
/// return `true`).
///
/// @param o1 The first object in the culled pair.
/// @param o2 The second object in the culled pair.
/// @param data A non-null pointer to a CollisionData instance.
/// @return `true` if the broadphase evaluation should stop.
/// @tparam S The scalar type with which the computation will be performed.
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2,
void* data);
/// @brief Collision data for use with the DefaultContinuousCollisionFunction.
/// It stores the collision request and the result given by the collision
/// algorithm (and stores the conclusion of whether further evaluation of the
/// broadphase collision manager has been deemed unnecessary).
// struct DefaultContinuousCollisionData {
// ContinuousCollisionRequest request;
// ContinuousCollisionResult result;
//
// /// If `true`, requests that the broadphase evaluation stop.
// bool done{false};
// };
/// @brief Provides a simple callback for the continuous collision query in the
/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
/// points to an instance of DefaultContinuousCollisionData. It simply invokes
/// the `collide()` method on the culled pair of geometries and stores the
/// results in the data's ContinuousCollisionResult instance.
///
/// This callback will never cause the broadphase evaluation to terminate early.
/// However, if the `done` member of the DefaultContinuousCollisionData is set
/// to true, this method will simply return without doing any computation.
///
/// For a given instance of DefaultContinuousCollisionData, if broadphase
/// evaluation has already terminated (i.e.,
/// DefaultContinuousCollisionFunction() returned `true`), subsequent
/// invocations with the same instance of CollisionData will return
/// immediately, requesting termination of broadphase evaluation (i.e., return
/// `true`).
///
/// @param o1 The first object in the culled pair.
/// @param o2 The second object in the culled pair.
/// @param data A non-null pointer to a CollisionData instance.
/// @return True if the broadphase evaluation should stop.
/// @tparam S The scalar type with which the computation will be performed.
// bool DefaultContinuousCollisionFunction(ContinuousCollisionObject* o1,
// ContinuousCollisionObject* o2,
// void* data) {
// assert(data != nullptr);
// auto* cdata = static_cast<DefaultContinuousCollisionData*>(data);
//
// if (cdata->done) return true;
//
// const ContinuousCollisionRequest& request = cdata->request;
// ContinuousCollisionResult& result = cdata->result;
// collide(o1, o2, request, result);
//
// return cdata->done;
// }
/// @brief Provides a simple callback for the distance query in the
/// BroadPhaseCollisionManager. It assumes the `data` parameter is non-null and
/// points to an instance of DistanceData. It simply invokes the
/// `distance()` method on the culled pair of geometries and stores the results
/// in the data's DistanceResult instance.
///
/// This callback will cause the broadphase evaluation to stop if:
/// - The distance is less than or equal to zero (i.e., the pair is in
/// contact).
///
/// For a given instance of DistanceData, if broadphase evaluation has
/// already terminated (i.e., defaultDistanceFunction() returned `true`),
/// subsequent invocations with the same instance of DistanceData will
/// simply report the previously reported minimum distance and request
/// termination of broadphase evaluation (i.e., return `true`).
///
/// @param o1 The first object in the culled pair.
/// @param o2 The second object in the culled pair.
/// @param data A non-null pointer to a DistanceData instance.
/// @param dist The distance computed by distance().
/// @return `true` if the broadphase evaluation should stop.
/// @tparam S The scalar type with which the computation will be performed.
bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2,
void* data, CoalScalar& dist);
/// @brief Default collision callback to check collision between collision
/// objects.
struct COAL_DLLAPI CollisionCallBackDefault : CollisionCallBackBase {
/// @brief Initialize the callback.
/// Clears the collision result and sets the done boolean to false.
void init() { data.clear(); }
bool collide(CollisionObject* o1, CollisionObject* o2);
CollisionData data;
virtual ~CollisionCallBackDefault() {};
};
/// @brief Default distance callback to check collision between collision
/// objects.
struct COAL_DLLAPI DistanceCallBackDefault : DistanceCallBackBase {
/// @brief Initialize the callback.
/// Clears the distance result and sets the done boolean to false.
void init() { data.clear(); }
bool distance(CollisionObject* o1, CollisionObject* o2, CoalScalar& dist);
DistanceData data;
virtual ~DistanceCallBackDefault() {};
};
/// @brief Collision callback to collect collision pairs potentially in contacts
struct COAL_DLLAPI CollisionCallBackCollect : CollisionCallBackBase {
typedef std::pair<CollisionObject*, CollisionObject*> CollisionPair;
/// @brief Default constructor.
CollisionCallBackCollect(const size_t max_size);
bool collide(CollisionObject* o1, CollisionObject* o2);
/// @brief Returns the number of registered collision pairs
size_t numCollisionPairs() const;
/// @brief Returns a const reference to the active collision_pairs to check
const std::vector<CollisionPair>& getCollisionPairs() const;
/// @brief Reset the callback
void init();
/// @brief Check whether a collision pair exists
bool exist(const CollisionPair& pair) const;
/// @brief Check whether a collision pair exists
bool exist(CollisionObject* o1, CollisionObject* o2) const;
virtual ~CollisionCallBackCollect() {};
protected:
std::vector<CollisionPair> collision_pairs;
size_t max_size;
};
} // namespace coal
#endif // COAL_BROADPHASE_DEFAULT_BROADPHASE_CALLBACKS_H
include/coal/broadphase/detail/hierarchy_tree-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_HIERARCHY_TREE_INL_H
#define COAL_HIERARCHY_TREE_INL_H
#include "coal/broadphase/detail/hierarchy_tree.h"
namespace coal {
namespace detail {
//==============================================================================
template <typename BV>
HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
root_node = nullptr;
n_leaves = 0;
free_node = nullptr;
max_lookahead_level = -1;
opath = 0;
bu_threshold = bu_threshold_;
topdown_level = topdown_level_;
}
//==============================================================================
template <typename BV>
HierarchyTree<BV>::~HierarchyTree() {
clear();
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init(std::vector<Node*>& leaves, int level) {
switch (level) {
case 0:
init_0(leaves);
break;
case 1:
init_1(leaves);
break;
case 2:
init_2(leaves);
break;
case 3:
init_3(leaves);
break;
default:
init_0(leaves);
}
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::insert(const BV& bv,
void* data) {
Node* leaf = createNode(nullptr, bv, data);
insertLeaf(root_node, leaf);
++n_leaves;
return leaf;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::remove(Node* leaf) {
removeLeaf(leaf);
deleteNode(leaf);
--n_leaves;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::clear() {
if (root_node) recurseDeleteNode(root_node);
n_leaves = 0;
delete free_node;
free_node = nullptr;
max_lookahead_level = -1;
opath = 0;
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::empty() const {
return (nullptr == root_node);
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::update(Node* leaf, int lookahead_level) {
// TODO(DamrongGuoy): Since we update a leaf node by removing and
// inserting the same leaf node, it is likely to change the structure of
// the tree even if no object's pose has changed. In the future,
// find a way to preserve the structure of the tree to solve this issue:
// https://github.com/flexible-collision-library/fcl/issues/368
// First we remove the leaf node pointed by `leaf` variable from the tree.
// The `sub_root` variable is the root of the subtree containing nodes
// whose BVs were adjusted due to node removal.
typename HierarchyTree<BV>::Node* sub_root = removeLeaf(leaf);
if (sub_root) {
if (lookahead_level > 0) {
// For positive `lookahead_level`, we move the `sub_root` variable up.
for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i)
sub_root = sub_root->parent;
} else
// By default, lookahead_level = -1, and we reset the `sub_root` variable
// to the root of the entire tree.
sub_root = root_node;
}
// Then we insert the node pointed by `leaf` variable back into the
// subtree rooted at `sub_root` variable.
insertLeaf(sub_root, leaf);
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv) {
if (leaf->bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
//==============================================================================
template <typename S, typename BV>
struct UpdateImpl {
static bool run(const HierarchyTree<BV>& tree,
typename HierarchyTree<BV>::Node* leaf, const BV& bv,
const Vec3s& /*vel*/, CoalScalar /*margin*/) {
if (leaf->bv.contain(bv)) return false;
tree.update_(leaf, bv);
return true;
}
static bool run(const HierarchyTree<BV>& tree,
typename HierarchyTree<BV>::Node* leaf, const BV& bv,
const Vec3s& /*vel*/) {
if (leaf->bv.contain(bv)) return false;
tree.update_(leaf, bv);
return true;
}
};
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel,
CoalScalar margin) {
return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel, margin);
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel) {
return UpdateImpl<CoalScalar, BV>::run(*this, leaf, bv, vel);
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::getMaxHeight() const {
if (!root_node) return 0;
return getMaxHeight(root_node);
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::getMaxDepth() const {
if (!root_node) return 0;
size_t max_depth;
getMaxDepth(root_node, 0, max_depth);
return max_depth;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::balanceBottomup() {
if (root_node) {
std::vector<Node*> leaves;
leaves.reserve(n_leaves);
fetchLeaves(root_node, leaves);
bottomup(leaves.begin(), leaves.end());
root_node = leaves[0];
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::balanceTopdown() {
if (root_node) {
std::vector<Node*> leaves;
leaves.reserve(n_leaves);
fetchLeaves(root_node, leaves);
root_node = topdown(leaves.begin(), leaves.end());
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::balanceIncremental(int iterations) {
if (iterations < 0) iterations = (int)n_leaves;
if (root_node && (iterations > 0)) {
for (int i = 0; i < iterations; ++i) {
Node* node = root_node;
unsigned int bit = 0;
while (!node->isLeaf()) {
node = sort(node, root_node)->children[(opath >> bit) & 1];
bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
}
update(node);
++opath;
}
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::refit() {
if (root_node) recurseRefit(root_node);
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::extractLeaves(const Node* root,
std::vector<Node*>& leaves) const {
if (!root->isLeaf()) {
extractLeaves(root->children[0], leaves);
extractLeaves(root->children[1], leaves);
} else
leaves.push_back(root);
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::size() const {
return n_leaves;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getRoot() const {
return root_node;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node*& HierarchyTree<BV>::getRoot() {
return root_node;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::print(Node* root, int depth) {
for (int i = 0; i < depth; ++i) std::cout << " ";
std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", "
<< root->bv.min_[2] << "; " << root->bv.max_[0] << ", "
<< root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
if (root->isLeaf()) {
} else {
print(root->children[0], depth + 1);
print(root->children[1], depth + 1);
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg,
const NodeVecIterator lend) {
NodeVecIterator lcur_end = lend;
while (lbeg < lcur_end - 1) {
NodeVecIterator min_it1 = lbeg;
NodeVecIterator min_it2 = lbeg + 1;
CoalScalar min_size = (std::numeric_limits<CoalScalar>::max)();
for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) {
for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) {
CoalScalar cur_size = ((*it1)->bv + (*it2)->bv).size();
if (cur_size < min_size) {
min_size = cur_size;
min_it1 = it1;
min_it2 = it2;
}
}
}
Node* n[2] = {*min_it1, *min_it2};
Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr);
p->children[0] = n[0];
p->children[1] = n[1];
n[0]->parent = p;
n[1]->parent = p;
*min_it1 = p;
Node* tmp = *min_it2;
lcur_end--;
*min_it2 = *lcur_end;
*lcur_end = tmp;
}
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown(
const NodeVecIterator lbeg, const NodeVecIterator lend) {
switch (topdown_level) {
case 0:
return topdown_0(lbeg, lend);
break;
case 1:
return topdown_1(lbeg, lend);
break;
default:
return topdown_0(lbeg, lend);
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::getMaxHeight(Node* node) const {
if (!node->isLeaf()) {
size_t height1 = getMaxHeight(node->children[0]);
size_t height2 = getMaxHeight(node->children[1]);
return std::max(height1, height2) + 1;
} else
return 0;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::getMaxDepth(Node* node, size_t depth,
size_t& max_depth) const {
if (!node->isLeaf()) {
getMaxDepth(node->children[0], depth + 1, max_depth);
getMaxDepth(node->children[1], depth + 1, max_depth);
} else
max_depth = std::max(max_depth, depth);
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_0(
const NodeVecIterator lbeg, const NodeVecIterator lend) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (num_leaves > bu_threshold) {
BV vol = (*lbeg)->bv;
for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv;
int best_axis = 0;
CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()};
if (extent[1] > extent[0]) best_axis = 1;
if (extent[2] > extent[best_axis]) best_axis = 2;
// compute median
NodeVecIterator lcenter = lbeg + num_leaves / 2;
std::nth_element(lbeg, lcenter, lend,
std::bind(&nodeBaseLess<BV>, std::placeholders::_1,
std::placeholders::_2, std::ref(best_axis)));
Node* node = createNode(nullptr, vol, nullptr);
node->children[0] = topdown_0(lbeg, lcenter);
node->children[1] = topdown_0(lcenter, lend);
node->children[0]->parent = node;
node->children[1]->parent = node;
return node;
} else {
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1(
const NodeVecIterator lbeg, const NodeVecIterator lend) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (num_leaves > bu_threshold) {
Vec3s split_p = (*lbeg)->bv.center();
BV vol = (*lbeg)->bv;
NodeVecIterator it;
for (it = lbeg + 1; it < lend; ++it) {
split_p += (*it)->bv.center();
vol += (*it)->bv;
}
split_p /= static_cast<CoalScalar>(num_leaves);
int best_axis = -1;
long bestmidp = num_leaves;
int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
for (it = lbeg; it < lend; ++it) {
Vec3s x = (*it)->bv.center() - split_p;
for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
}
for (int i = 0; i < 3; ++i) {
if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
long midp = std::abs(splitcount[i][0] - splitcount[i][1]);
if (midp < bestmidp) {
best_axis = i;
bestmidp = midp;
}
}
}
if (best_axis < 0) best_axis = 0;
CoalScalar split_value = split_p[best_axis];
NodeVecIterator lcenter = lbeg;
for (it = lbeg; it < lend; ++it) {
if ((*it)->bv.center()[best_axis] < split_value) {
Node* temp = *it;
*it = *lcenter;
*lcenter = temp;
++lcenter;
}
}
Node* node = createNode(nullptr, vol, nullptr);
node->children[0] = topdown_1(lbeg, lcenter);
node->children[1] = topdown_1(lcenter, lend);
node->children[0]->parent = node;
node->children[1]->parent = node;
return node;
} else {
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_0(std::vector<Node*>& leaves) {
clear();
root_node = topdown(leaves.begin(), leaves.end());
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_1(std::vector<Node*>& leaves) {
clear();
BV bound_bv;
if (leaves.size() > 0) bound_bv = leaves[0]->bv;
for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
morton_functor<CoalScalar, uint32_t> coder(bound_bv);
for (size_t i = 0; i < leaves.size(); ++i)
leaves[i]->code = coder(leaves[i]->bv.center());
std::sort(leaves.begin(), leaves.end(), SortByMorton());
root_node = mortonRecurse_0(leaves.begin(), leaves.end(),
(1 << (coder.bits() - 1)), coder.bits() - 1);
refit();
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_2(std::vector<Node*>& leaves) {
clear();
BV bound_bv;
if (leaves.size() > 0) bound_bv = leaves[0]->bv;
for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
morton_functor<CoalScalar, uint32_t> coder(bound_bv);
for (size_t i = 0; i < leaves.size(); ++i)
leaves[i]->code = coder(leaves[i]->bv.center());
std::sort(leaves.begin(), leaves.end(), SortByMorton());
root_node = mortonRecurse_1(leaves.begin(), leaves.end(),
(1 << (coder.bits() - 1)), coder.bits() - 1);
refit();
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_3(std::vector<Node*>& leaves) {
clear();
BV bound_bv;
if (leaves.size() > 0) bound_bv = leaves[0]->bv;
for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
morton_functor<CoalScalar, uint32_t> coder(bound_bv);
for (size_t i = 0; i < leaves.size(); ++i)
leaves[i]->code = coder(leaves[i]->bv.center());
std::sort(leaves.begin(), leaves.end(), SortByMorton());
root_node = mortonRecurse_2(leaves.begin(), leaves.end());
refit();
n_leaves = leaves.size();
max_lookahead_level = -1;
opath = 0;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_0(
const NodeVecIterator lbeg, const NodeVecIterator lend,
const uint32_t& split, int bits) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (bits > 0) {
Node dummy;
dummy.code = split;
NodeVecIterator lcenter =
std::lower_bound(lbeg, lend, &dummy, SortByMorton());
if (lcenter == lbeg) {
uint32_t split2 = split | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split2, bits - 1);
} else if (lcenter == lend) {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split1, bits - 1);
} else {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
uint32_t split2 = split | (1 << (bits - 1));
Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
Node* node = createNode(nullptr, nullptr);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
}
} else {
Node* node = topdown(lbeg, lend);
return node;
}
} else
return *lbeg;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_1(
const NodeVecIterator lbeg, const NodeVecIterator lend,
const uint32_t& split, int bits) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (bits > 0) {
Node dummy;
dummy.code = split;
NodeVecIterator lcenter =
std::lower_bound(lbeg, lend, &dummy, SortByMorton());
if (lcenter == lbeg) {
uint32_t split2 = split | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split2, bits - 1);
} else if (lcenter == lend) {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split1, bits - 1);
} else {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
uint32_t split2 = split | (1 << (bits - 1));
Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
Node* node = createNode(nullptr, nullptr);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
}
} else {
Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
Node* node = createNode(nullptr, nullptr);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
}
} else
return *lbeg;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_2(
const NodeVecIterator lbeg, const NodeVecIterator lend) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
Node* node = createNode(nullptr, nullptr);
node->children[0] = child1;
node->children[1] = child2;
child1->parent = node;
child2->parent = node;
return node;
} else
return *lbeg;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::update_(Node* leaf, const BV& bv) {
Node* root = removeLeaf(leaf);
if (root) {
if (max_lookahead_level >= 0) {
for (int i = 0; (i < max_lookahead_level) && root->parent; ++i)
root = root->parent;
} else
root = root_node;
}
leaf->bv = bv;
insertLeaf(root, leaf);
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::sort(Node* n, Node*& r) {
Node* p = n->parent;
if (p > n) {
size_t i = indexOf(n);
size_t j = 1 - i;
Node* s = p->children[j];
Node* q = p->parent;
if (q)
q->children[indexOf(p)] = n;
else
r = n;
s->parent = n;
p->parent = n;
n->parent = q;
p->children[0] = n->children[0];
p->children[1] = n->children[1];
n->children[0]->parent = p;
n->children[1]->parent = p;
n->children[i] = p;
n->children[j] = s;
std::swap(p->bv, n->bv);
return p;
}
return n;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::insertLeaf(Node* const sub_root, Node* const leaf)
// Attempts to insert `leaf` into a subtree rooted at `sub_root`.
// 1. If the whole tree is empty, then `leaf` simply becomes the tree.
// 2. Otherwise, a leaf node called `sibling` of the subtree rooted at
// `sub_root` is selected (the selection criteria is a black box for this
// algorithm), and the `sibling` leaf is replaced by an internal node with
// two children, `sibling` and `leaf`. The bounding volumes are updated as
// necessary.
{
if (!root_node) {
// If the entire tree is empty, the node pointed by `leaf` variable will
// become the root of the tree.
root_node = leaf;
leaf->parent = nullptr;
return;
}
// Traverse the tree from the given `sub_root` down to an existing leaf
// node that we call `sibling`. The `sibling` node will eventually become
// the sibling of the given `leaf` node.
Node* sibling = sub_root;
while (!sibling->isLeaf()) {
sibling = sibling->children[select(*leaf, *(sibling->children[0]),
*(sibling->children[1]))];
}
Node* prev = sibling->parent;
// Create a new `node` that later will become the new parent of both the
// `sibling` and the given `leaf`.
Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr);
if (prev)
// If the parent `prev` of the `sibling` is an interior node, we will
// replace the `sibling` with the subtree {node {`sibling`, leaf}} like
// this:
// Before After
//
// ╱ ╱
// prev prev
// ╱ ╲ ─> ╱ ╲
// sibling ... node ...
// ╱ ╲
// sibling leaf
{
prev->children[indexOf(sibling)] = node;
node->children[0] = sibling;
sibling->parent = node;
node->children[1] = leaf;
leaf->parent = node;
// Now that we've inserted `leaf` some of the existing bounding
// volumes might not fully enclose their children. Walk up the tree
// looking for parents that don't already enclose their children
// and create a new tight-fitting bounding volume for those.
do {
if (!prev->bv.contain(node->bv))
prev->bv = prev->children[0]->bv + prev->children[1]->bv;
else
break;
node = prev;
} while (nullptr != (prev = node->parent));
} else
// If the `sibling` has no parent, i.e., the tree is a singleton,
// we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like
// this:
//
// node
// ╱ ╲
// sibling leaf
{
node->children[0] = sibling;
sibling->parent = node;
node->children[1] = leaf;
leaf->parent = node;
root_node = node;
}
// Note that the above algorithm always adds the new `leaf` node as the right
// child, i.e., children[1]. Calling removeLeaf(l) followed by calling
// this function insertLeaf(l) where l is a left child will result in
// switching l and its sibling even if no object's pose has changed.
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::removeLeaf(
Node* const leaf) {
// Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling,
// and its parent with just its sibling. It then "tightens" the ancestor
// bounding volumes. Returns a pointer to the parent of the highest node whose
// BV changed due to the removal.
if (leaf == root_node) {
// If the `leaf` node is the only node in the tree, the tree becomes empty.
root_node = nullptr;
return nullptr;
}
Node* parent = leaf->parent;
Node* prev = parent->parent;
Node* sibling = parent->children[1 - indexOf(leaf)];
if (prev) {
// If the parent node is not the root node, the sibling node will
// replace the parent node like this:
//
// Before After
// ... ...
// ╱ ╱
// prev prev
// ╱ ╲ ╱ ╲
// parent ... ─> sibling ...
// ╱ ╲ ╱╲
// leaf sibling ...
// ╱╲
// ...
//
// Step 1: replace the subtree {parent {leaf, sibling {...}}} with
// {sibling {...}}.
prev->children[indexOf(parent)] = sibling;
sibling->parent = prev;
deleteNode(parent);
// Step 2: tighten up the BVs of the ancestor nodes.
while (prev) {
BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
if (!(new_bv == prev->bv)) {
prev->bv = new_bv;
prev = prev->parent;
} else
break;
}
return prev ? prev : root_node;
} else {
// If the parent node is the root node, the sibling node will become the
// root of the whole tree like this:
//
// Before After
//
// parent
// ╱ ╲
// leaf sibling ─> sibling
// ╱╲ ╱╲
// ... ...
root_node = sibling;
sibling->parent = nullptr;
deleteNode(parent);
return root_node;
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::fetchLeaves(Node* root, std::vector<Node*>& leaves,
int depth) {
if ((!root->isLeaf()) && depth) {
fetchLeaves(root->children[0], leaves, depth - 1);
fetchLeaves(root->children[1], leaves, depth - 1);
deleteNode(root);
} else {
leaves.push_back(root);
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::indexOf(Node* node) {
// node cannot be nullptr
return (node->parent->children[1] == node);
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
const BV& bv,
void* data) {
Node* node = createNode(parent, data);
node->bv = bv;
return node;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
const BV& bv1,
const BV& bv2,
void* data) {
Node* node = createNode(parent, data);
node->bv = bv1 + bv2;
return node;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
void* data) {
Node* node = nullptr;
if (free_node) {
node = free_node;
free_node = nullptr;
} else
node = new Node();
node->parent = parent;
node->data = data;
node->children[1] = 0;
return node;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::deleteNode(Node* node) {
if (free_node != node) {
delete free_node;
free_node = node;
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::recurseDeleteNode(Node* node) {
if (!node->isLeaf()) {
recurseDeleteNode(node->children[0]);
recurseDeleteNode(node->children[1]);
}
if (node == root_node) root_node = nullptr;
deleteNode(node);
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::recurseRefit(Node* node) {
if (!node->isLeaf()) {
recurseRefit(node->children[0]);
recurseRefit(node->children[1]);
node->bv = node->children[0]->bv + node->children[1]->bv;
} else
return;
}
//==============================================================================
template <typename BV>
bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d) {
if (a->bv.center()[d] < b->bv.center()[d]) return true;
return false;
}
//==============================================================================
template <typename S, typename BV>
struct SelectImpl {
static std::size_t run(const NodeBase<BV>& /*query*/,
const NodeBase<BV>& /*node1*/,
const NodeBase<BV>& /*node2*/) {
return 0;
}
static std::size_t run(const BV& /*query*/, const NodeBase<BV>& /*node1*/,
const NodeBase<BV>& /*node2*/) {
return 0;
}
};
//==============================================================================
template <typename BV>
size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
const NodeBase<BV>& node2) {
return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
}
//==============================================================================
template <typename BV>
size_t select(const BV& query, const NodeBase<BV>& node1,
const NodeBase<BV>& node2) {
return SelectImpl<CoalScalar, BV>::run(query, node1, node2);
}
//==============================================================================
template <typename S>
struct SelectImpl<S, AABB> {
static std::size_t run(const NodeBase<AABB>& node,
const NodeBase<AABB>& node1,
const NodeBase<AABB>& node2) {
const AABB& bv = node.bv;
const AABB& bv1 = node1.bv;
const AABB& bv2 = node2.bv;
Vec3s v = bv.min_ + bv.max_;
Vec3s v1 = v - (bv1.min_ + bv1.max_);
Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
}
static std::size_t run(const AABB& query, const NodeBase<AABB>& node1,
const NodeBase<AABB>& node2) {
const AABB& bv = query;
const AABB& bv1 = node1.bv;
const AABB& bv2 = node2.bv;
Vec3s v = bv.min_ + bv.max_;
Vec3s v1 = v - (bv1.min_ + bv1.max_);
Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
}
};
} // namespace detail
} // namespace coal
#endif
include/coal/broadphase/detail/hierarchy_tree.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_HIERARCHY_TREE_H
#define COAL_HIERARCHY_TREE_H
#include <vector>
#include <map>
#include <functional>
#include <iostream>
#include "coal/warning.hh"
#include "coal/BV/AABB.h"
#include "coal/broadphase/detail/morton.h"
#include "coal/broadphase/detail/node_base.h"
namespace coal {
namespace detail {
/// @brief Class for hierarchy tree structure
template <typename BV>
class HierarchyTree {
public:
typedef NodeBase<BV> Node;
/// @brief Create hierarchy tree with suitable setting.
/// bu_threshold decides the height of tree node to start bottom-up
/// construction / optimization; topdown_level decides different methods to
/// construct tree in topdown manner. lower level method constructs tree with
/// better quality but is slower.
HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
~HierarchyTree();
/// @brief Initialize the tree by a set of leaves using algorithm with a given
/// level.
void init(std::vector<Node*>& leaves, int level = 0);
/// @brief Insest a node
Node* insert(const BV& bv, void* data);
/// @brief Remove a leaf node
void remove(Node* leaf);
/// @brief Clear the tree
void clear();
/// @brief Whether the tree is empty
bool empty() const;
/// @brief Updates a `leaf` node. A use case is when the bounding volume
/// of an object changes. Ensure every parent node has its bounding volume
/// expand or shrink to fit the bounding volumes of its children.
/// @note Strangely the structure of the tree may change even if the
/// bounding volume of the `leaf` node does not change. It is just
/// another valid tree of the same set of objects. This is because
/// update() works by first removing `leaf` and then inserting `leaf`
/// back. The structural change could be as simple as switching the
/// order of two leaves if the sibling of the `leaf` is also a leaf.
/// Or it could be more complicated if the sibling is an internal
/// node.
void update(Node* leaf, int lookahead_level = -1);
/// @brief update the tree when the bounding volume of a given leaf has
/// changed
bool update(Node* leaf, const BV& bv);
/// @brief update one leaf's bounding volume, with prediction
bool update(Node* leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
/// @brief update one leaf's bounding volume, with prediction
bool update(Node* leaf, const BV& bv, const Vec3s& vel);
/// @brief get the max height of the tree
size_t getMaxHeight() const;
/// @brief get the max depth of the tree
size_t getMaxDepth() const;
/// @brief balance the tree from bottom
void balanceBottomup();
/// @brief balance the tree from top
void balanceTopdown();
/// @brief balance the tree in an incremental way
void balanceIncremental(int iterations);
/// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
/// update the entire tree in a bottom-up manner
void refit();
/// @brief extract all the leaves of the tree
void extractLeaves(const Node* root, std::vector<Node*>& leaves) const;
/// @brief number of leaves in the tree
size_t size() const;
/// @brief get the root of the tree
Node* getRoot() const;
Node*& getRoot();
/// @brief print the tree in a recursive way
void print(Node* root, int depth);
private:
typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
typedef
typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
struct SortByMorton {
bool operator()(const Node* a, const Node* b) const {
return a->code < b->code;
}
};
/// @brief construct a tree for a set of leaves from bottom -- very heavy way
void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief construct a tree for a set of leaves from top
Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief compute the maximum height of a subtree rooted from a given node
size_t getMaxHeight(Node* node) const;
/// @brief compute the maximum depth of a subtree rooted from a given node
void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const;
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
/// topdown manner. During construction, first compute the best split axis as
/// the axis along with the longest AABB edge. Then compute the median of all
/// nodes' center projection onto the axis and using it as the split
/// threshold.
Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
/// topdown manner. During construction, first compute the best split
/// thresholds for different axes as the average of all nodes' center. Then
/// choose the split axis as the axis whose threshold can divide the nodes
/// into two parts with almost similar size. This construction is more
/// expensive then topdown_0, but also can provide tree with better quality.
Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief init tree from leaves in the topdown manner (topdown_0 or
/// topdown_1)
void init_0(std::vector<Node*>& leaves);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
/// for nodes which is of depth more than the maximum bits of the morton code,
/// we use bottomup method to construct the subtree, which is slow but can
/// construct tree with high quality.
void init_1(std::vector<Node*>& leaves);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
/// for nodes which is of depth more than the maximum bits of the morton code,
/// we split the leaves into two parts with the same size simply using the
/// node index.
void init_2(std::vector<Node*>& leaves);
/// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
/// for all nodes, we simply divide the leaves into parts with the same size
/// simply using the node index.
void init_3(std::vector<Node*>& leaves);
Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend,
const uint32_t& split, int bits);
Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend,
const uint32_t& split, int bits);
Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
/// @brief update one leaf node's bounding volume
void update_(Node* leaf, const BV& bv);
/// @brief sort node n and its parent according to their memory position
Node* sort(Node* n, Node*& r);
/// @brief Insert a leaf node and also update its ancestors. Maintain the
/// tree as a full binary tree (every interior node has exactly two children).
/// Furthermore, adjust the BV of interior nodes so that each parent's BV
/// tightly fits its children's BVs.
/// @param sub_root The root of the subtree into which we will insert the
// leaf node.
void insertLeaf(Node* const sub_root, Node* const leaf);
/// @brief Remove a leaf. Maintain the tree as a full binary tree (every
/// interior node has exactly two children). Furthermore, adjust the BV of
/// interior nodes so that each parent's BV tightly fits its children's BVs.
/// @note The leaf node itself is not deleted yet, but all the unnecessary
/// internal nodes are deleted.
/// @returns the root of the subtree containing the nodes whose BVs were
// adjusted.
Node* removeLeaf(Node* const leaf);
/// @brief Delete all internal nodes and return all leaves nodes with given
/// depth from root
void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
static size_t indexOf(Node* node);
/// @brief create one node (leaf or internal)
Node* createNode(Node* parent, const BV& bv, void* data);
Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data);
Node* createNode(Node* parent, void* data);
void deleteNode(Node* node);
void recurseDeleteNode(Node* node);
void recurseRefit(Node* node);
protected:
Node* root_node;
size_t n_leaves;
unsigned int opath;
/// This is a one Node cache, the reason is that we need to remove a node and
/// then add it again frequently.
Node* free_node;
int max_lookahead_level;
public:
/// @brief decide which topdown algorithm to use
int topdown_level;
/// @brief decide the depth to use expensive bottom-up algorithm
int bu_threshold;
};
/// @brief Compare two nodes accoording to the d-th dimension of node center
template <typename BV>
bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
/// @brief select from node1 and node2 which is close to a given query. 0 for
/// node1 and 1 for node2
template <typename BV>
size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
const NodeBase<BV>& node2);
/// @brief select from node1 and node2 which is close to a given query bounding
/// volume. 0 for node1 and 1 for node2
template <typename BV>
size_t select(const BV& query, const NodeBase<BV>& node1,
const NodeBase<BV>& node2);
} // namespace detail
} // namespace coal
#include "coal/broadphase/detail/hierarchy_tree-inl.h"
#endif
include/coal/broadphase/detail/hierarchy_tree_array-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_HIERARCHY_TREE_ARRAY_INL_H
#define COAL_HIERARCHY_TREE_ARRAY_INL_H
#include "coal/broadphase/detail/hierarchy_tree_array.h"
#include <algorithm>
#include <iostream>
namespace coal {
namespace detail {
namespace implementation_array {
//==============================================================================
template <typename BV>
HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
root_node = NULL_NODE;
n_nodes = 0;
n_nodes_alloc = 16;
nodes = new Node[n_nodes_alloc];
for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
n_leaves = 0;
freelist = 0;
opath = 0;
max_lookahead_level = -1;
bu_threshold = bu_threshold_;
topdown_level = topdown_level_;
}
//==============================================================================
template <typename BV>
HierarchyTree<BV>::~HierarchyTree() {
delete[] nodes;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init(Node* leaves, int n_leaves_, int level) {
switch (level) {
case 0:
init_0(leaves, n_leaves_);
break;
case 1:
init_1(leaves, n_leaves_);
break;
case 2:
init_2(leaves, n_leaves_);
break;
case 3:
init_3(leaves, n_leaves_);
break;
default:
init_0(leaves, n_leaves_);
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_0(Node* leaves, int n_leaves_) {
clear();
n_leaves = (size_t)n_leaves_;
root_node = NULL_NODE;
nodes = new Node[n_leaves * 2];
std::copy(leaves, leaves + n_leaves, nodes);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
size_t* ids = new size_t[n_leaves];
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
root_node = topdown(ids, ids + n_leaves);
delete[] ids;
opath = 0;
max_lookahead_level = -1;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_1(Node* leaves, int n_leaves_) {
clear();
n_leaves = (size_t)n_leaves_;
root_node = NULL_NODE;
nodes = new Node[n_leaves * 2];
std::copy(leaves, leaves + n_leaves, nodes);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
BV bound_bv;
if (n_leaves > 0) bound_bv = nodes[0].bv;
for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
morton_functor<CoalScalar, uint32_t> coder(bound_bv);
for (size_t i = 0; i < n_leaves; ++i)
nodes[i].code = coder(nodes[i].bv.center());
size_t* ids = new size_t[n_leaves];
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
const SortByMorton comp{nodes};
std::sort(ids, ids + n_leaves, comp);
root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
coder.bits() - 1);
delete[] ids;
refit();
opath = 0;
max_lookahead_level = -1;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_2(Node* leaves, int n_leaves_) {
clear();
n_leaves = (size_t)n_leaves_;
root_node = NULL_NODE;
nodes = new Node[n_leaves * 2];
std::copy(leaves, leaves + n_leaves, nodes);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
BV bound_bv;
if (n_leaves > 0) bound_bv = nodes[0].bv;
for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
morton_functor<CoalScalar, uint32_t> coder(bound_bv);
for (size_t i = 0; i < n_leaves; ++i)
nodes[i].code = coder(nodes[i].bv.center());
size_t* ids = new size_t[n_leaves];
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
const SortByMorton comp{nodes};
std::sort(ids, ids + n_leaves, comp);
root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
coder.bits() - 1);
delete[] ids;
refit();
opath = 0;
max_lookahead_level = -1;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::init_3(Node* leaves, int n_leaves_) {
clear();
n_leaves = (size_t)n_leaves_;
root_node = NULL_NODE;
nodes = new Node[n_leaves * 2];
std::copy(leaves, leaves + n_leaves, nodes);
freelist = n_leaves;
n_nodes = n_leaves;
n_nodes_alloc = 2 * n_leaves;
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
BV bound_bv;
if (n_leaves > 0) bound_bv = nodes[0].bv;
for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
morton_functor<CoalScalar, uint32_t> coder(bound_bv);
for (size_t i = 0; i < n_leaves; ++i)
nodes[i].code = coder(nodes[i].bv.center());
size_t* ids = new size_t[n_leaves];
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
const SortByMorton comp{nodes};
std::sort(ids, ids + n_leaves, comp);
root_node = mortonRecurse_2(ids, ids + n_leaves);
delete[] ids;
refit();
opath = 0;
max_lookahead_level = -1;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::insert(const BV& bv, void* data) {
size_t node = createNode(NULL_NODE, bv, data);
insertLeaf(root_node, node);
++n_leaves;
return node;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::remove(size_t leaf) {
removeLeaf(leaf);
deleteNode(leaf);
--n_leaves;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::clear() {
delete[] nodes;
root_node = NULL_NODE;
n_nodes = 0;
n_nodes_alloc = 16;
nodes = new Node[n_nodes_alloc];
for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
n_leaves = 0;
freelist = 0;
opath = 0;
max_lookahead_level = -1;
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::empty() const {
return (n_nodes == 0);
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::update(size_t leaf, int lookahead_level) {
size_t root = removeLeaf(leaf);
if (root != NULL_NODE) {
if (lookahead_level > 0) {
for (int i = 0;
(i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
root = nodes[root].parent;
} else
root = root_node;
}
insertLeaf(root, leaf);
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv) {
if (nodes[leaf].bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel,
CoalScalar margin) {
COAL_UNUSED_VARIABLE(bv);
COAL_UNUSED_VARIABLE(vel);
COAL_UNUSED_VARIABLE(margin);
if (nodes[leaf].bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
//==============================================================================
template <typename BV>
bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel) {
COAL_UNUSED_VARIABLE(vel);
if (nodes[leaf].bv.contain(bv)) return false;
update_(leaf, bv);
return true;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::getMaxHeight() const {
if (root_node == NULL_NODE) return 0;
return getMaxHeight(root_node);
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::getMaxDepth() const {
if (root_node == NULL_NODE) return 0;
size_t max_depth;
getMaxDepth(root_node, 0, max_depth);
return max_depth;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::balanceBottomup() {
if (root_node != NULL_NODE) {
Node* leaves = new Node[n_leaves];
Node* leaves_ = leaves;
extractLeaves(root_node, leaves_);
root_node = NULL_NODE;
std::copy(leaves, leaves + n_leaves, nodes);
freelist = n_leaves;
n_nodes = n_leaves;
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
size_t* ids = new size_t[n_leaves];
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
bottomup(ids, ids + n_leaves);
root_node = *ids;
delete[] ids;
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::balanceTopdown() {
if (root_node != NULL_NODE) {
Node* leaves = new Node[n_leaves];
Node* leaves_ = leaves;
extractLeaves(root_node, leaves_);
root_node = NULL_NODE;
std::copy(leaves, leaves + n_leaves, nodes);
freelist = n_leaves;
n_nodes = n_leaves;
for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
size_t* ids = new size_t[n_leaves];
for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
root_node = topdown(ids, ids + n_leaves);
delete[] ids;
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::balanceIncremental(int iterations) {
if (iterations < 0) iterations = (int)n_leaves;
if ((root_node != NULL_NODE) && (iterations > 0)) {
for (int i = 0; i < iterations; ++i) {
size_t node = root_node;
unsigned int bit = 0;
while (!nodes[node].isLeaf()) {
node = nodes[node].children[(opath >> bit) & 1];
bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
}
update(node);
++opath;
}
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::refit() {
if (root_node != NULL_NODE) recurseRefit(root_node);
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::extractLeaves(size_t root, Node*& leaves) const {
if (!nodes[root].isLeaf()) {
extractLeaves(nodes[root].children[0], leaves);
extractLeaves(nodes[root].children[1], leaves);
} else {
*leaves = nodes[root];
leaves++;
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::size() const {
return n_leaves;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::getRoot() const {
return root_node;
}
//==============================================================================
template <typename BV>
typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getNodes() const {
return nodes;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::print(size_t root, int depth) {
for (int i = 0; i < depth; ++i) std::cout << " ";
Node* n = nodes + root;
std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", "
<< n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1]
<< ", " << n->bv.max_[2] << ")" << std::endl;
if (n->isLeaf()) {
} else {
print(n->children[0], depth + 1);
print(n->children[1], depth + 1);
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::getMaxHeight(size_t node) const {
if (!nodes[node].isLeaf()) {
size_t height1 = getMaxHeight(nodes[node].children[0]);
size_t height2 = getMaxHeight(nodes[node].children[1]);
return std::max(height1, height2) + 1;
} else
return 0;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth,
size_t& max_depth) const {
if (!nodes[node].isLeaf()) {
getMaxDepth(nodes[node].children[0], depth + 1, max_depth);
getmaxDepth(nodes[node].children[1], depth + 1, max_depth);
} else
max_depth = std::max(max_depth, depth);
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend) {
size_t* lcur_end = lend;
while (lbeg < lcur_end - 1) {
size_t *min_it1 = nullptr, *min_it2 = nullptr;
CoalScalar min_size = (std::numeric_limits<CoalScalar>::max)();
for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) {
for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) {
CoalScalar cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
if (cur_size < min_size) {
min_size = cur_size;
min_it1 = it1;
min_it2 = it2;
}
}
}
size_t p =
createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr);
nodes[p].children[0] = *min_it1;
nodes[p].children[1] = *min_it2;
nodes[*min_it1].parent = p;
nodes[*min_it2].parent = p;
*min_it1 = p;
size_t tmp = *min_it2;
lcur_end--;
*min_it2 = *lcur_end;
*lcur_end = tmp;
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend) {
switch (topdown_level) {
case 0:
return topdown_0(lbeg, lend);
break;
case 1:
return topdown_1(lbeg, lend);
break;
default:
return topdown_0(lbeg, lend);
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (num_leaves > bu_threshold) {
BV vol = nodes[*lbeg].bv;
for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv;
size_t best_axis = 0;
CoalScalar extent[3] = {vol.width(), vol.height(), vol.depth()};
if (extent[1] > extent[0]) best_axis = 1;
if (extent[2] > extent[best_axis]) best_axis = 2;
nodeBaseLess<BV> comp(nodes, best_axis);
size_t* lcenter = lbeg + num_leaves / 2;
std::nth_element(lbeg, lcenter, lend, comp);
size_t node = createNode(NULL_NODE, vol, nullptr);
nodes[node].children[0] = topdown_0(lbeg, lcenter);
nodes[node].children[1] = topdown_0(lcenter, lend);
nodes[nodes[node].children[0]].parent = node;
nodes[nodes[node].children[1]].parent = node;
return node;
} else {
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (num_leaves > bu_threshold) {
Vec3s split_p = nodes[*lbeg].bv.center();
BV vol = nodes[*lbeg].bv;
for (size_t* i = lbeg + 1; i < lend; ++i) {
split_p += nodes[*i].bv.center();
vol += nodes[*i].bv;
}
split_p /= static_cast<CoalScalar>(num_leaves);
int best_axis = -1;
int bestmidp = (int)num_leaves;
int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
for (size_t* i = lbeg; i < lend; ++i) {
Vec3s x = nodes[*i].bv.center() - split_p;
for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
}
for (size_t i = 0; i < 3; ++i) {
if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
if (midp < bestmidp) {
best_axis = (int)i;
bestmidp = midp;
}
}
}
if (best_axis < 0) best_axis = 0;
CoalScalar split_value = split_p[best_axis];
size_t* lcenter = lbeg;
for (size_t* i = lbeg; i < lend; ++i) {
if (nodes[*i].bv.center()[best_axis] < split_value) {
size_t temp = *i;
*i = *lcenter;
*lcenter = temp;
++lcenter;
}
}
size_t node = createNode(NULL_NODE, vol, nullptr);
nodes[node].children[0] = topdown_1(lbeg, lcenter);
nodes[node].children[1] = topdown_1(lcenter, lend);
nodes[nodes[node].children[0]].parent = node;
nodes[nodes[node].children[1]].parent = node;
return node;
} else {
bottomup(lbeg, lend);
return *lbeg;
}
}
return *lbeg;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend,
const uint32_t& split, int bits) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (bits > 0) {
const SortByMorton comp{nodes, split};
size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
if (lcenter == lbeg) {
uint32_t split2 = split | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split2, bits - 1);
} else if (lcenter == lend) {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_0(lbeg, lend, split1, bits - 1);
} else {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
uint32_t split2 = split | (1 << (bits - 1));
size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
size_t node = createNode(NULL_NODE, nullptr);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
}
} else {
size_t node = topdown(lbeg, lend);
return node;
}
} else
return *lbeg;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend,
const uint32_t& split, int bits) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
if (bits > 0) {
const SortByMorton comp{nodes, split};
size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
if (lcenter == lbeg) {
uint32_t split2 = split | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split2, bits - 1);
} else if (lcenter == lend) {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
return mortonRecurse_1(lbeg, lend, split1, bits - 1);
} else {
uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
uint32_t split2 = split | (1 << (bits - 1));
size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
size_t node = createNode(NULL_NODE, nullptr);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
}
} else {
size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
size_t node = createNode(NULL_NODE, nullptr);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
}
} else
return *lbeg;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend) {
long num_leaves = lend - lbeg;
if (num_leaves > 1) {
size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
size_t node = createNode(NULL_NODE, nullptr);
nodes[node].children[0] = child1;
nodes[node].children[1] = child2;
nodes[child1].parent = node;
nodes[child2].parent = node;
return node;
} else
return *lbeg;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf) {
if (root_node == NULL_NODE) {
root_node = leaf;
nodes[leaf].parent = NULL_NODE;
} else {
if (!nodes[root].isLeaf()) {
do {
root = nodes[root].children[select(leaf, nodes[root].children[0],
nodes[root].children[1], nodes)];
} while (!nodes[root].isLeaf());
}
size_t prev = nodes[root].parent;
size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr);
if (prev != NULL_NODE) {
nodes[prev].children[indexOf(root)] = node;
nodes[node].children[0] = root;
nodes[root].parent = node;
nodes[node].children[1] = leaf;
nodes[leaf].parent = node;
do {
if (!nodes[prev].bv.contain(nodes[node].bv))
nodes[prev].bv = nodes[nodes[prev].children[0]].bv +
nodes[nodes[prev].children[1]].bv;
else
break;
node = prev;
} while (NULL_NODE != (prev = nodes[node].parent));
} else {
nodes[node].children[0] = root;
nodes[root].parent = node;
nodes[node].children[1] = leaf;
nodes[leaf].parent = node;
root_node = node;
}
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::removeLeaf(size_t leaf) {
if (leaf == root_node) {
root_node = NULL_NODE;
return NULL_NODE;
} else {
size_t parent = nodes[leaf].parent;
size_t prev = nodes[parent].parent;
size_t sibling = nodes[parent].children[1 - indexOf(leaf)];
if (prev != NULL_NODE) {
nodes[prev].children[indexOf(parent)] = sibling;
nodes[sibling].parent = prev;
deleteNode(parent);
while (prev != NULL_NODE) {
BV new_bv = nodes[nodes[prev].children[0]].bv +
nodes[nodes[prev].children[1]].bv;
if (!(new_bv == nodes[prev].bv)) {
nodes[prev].bv = new_bv;
prev = nodes[prev].parent;
} else
break;
}
return (prev != NULL_NODE) ? prev : root_node;
} else {
root_node = sibling;
nodes[sibling].parent = NULL_NODE;
deleteNode(parent);
return root_node;
}
}
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::update_(size_t leaf, const BV& bv) {
size_t root = removeLeaf(leaf);
if (root != NULL_NODE) {
if (max_lookahead_level >= 0) {
for (int i = 0;
(i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
root = nodes[root].parent;
}
nodes[leaf].bv = bv;
insertLeaf(root, leaf);
}
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::indexOf(size_t node) {
return (nodes[nodes[node].parent].children[1] == node);
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::allocateNode() {
if (freelist == NULL_NODE) {
Node* old_nodes = nodes;
n_nodes_alloc *= 2;
nodes = new Node[n_nodes_alloc];
std::copy(old_nodes, old_nodes + n_nodes, nodes);
delete[] old_nodes;
for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
nodes[n_nodes_alloc - 1].next = NULL_NODE;
freelist = n_nodes;
}
size_t node_id = freelist;
freelist = nodes[node_id].next;
nodes[node_id].parent = NULL_NODE;
nodes[node_id].children[0] = NULL_NODE;
nodes[node_id].children[1] = NULL_NODE;
++n_nodes;
return node_id;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv1,
const BV& bv2, void* data) {
size_t node = allocateNode();
nodes[node].parent = parent;
nodes[node].data = data;
nodes[node].bv = bv1 + bv2;
return node;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv, void* data) {
size_t node = allocateNode();
nodes[node].parent = parent;
nodes[node].data = data;
nodes[node].bv = bv;
return node;
}
//==============================================================================
template <typename BV>
size_t HierarchyTree<BV>::createNode(size_t parent, void* data) {
size_t node = allocateNode();
nodes[node].parent = parent;
nodes[node].data = data;
return node;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::deleteNode(size_t node) {
nodes[node].next = freelist;
freelist = node;
--n_nodes;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::recurseRefit(size_t node) {
if (!nodes[node].isLeaf()) {
recurseRefit(nodes[node].children[0]);
recurseRefit(nodes[node].children[1]);
nodes[node].bv =
nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
} else
return;
}
//==============================================================================
template <typename BV>
void HierarchyTree<BV>::fetchLeaves(size_t root, Node*& leaves, int depth) {
if ((!nodes[root].isLeaf()) && depth) {
fetchLeaves(nodes[root].children[0], leaves, depth - 1);
fetchLeaves(nodes[root].children[1], leaves, depth - 1);
deleteNode(root);
} else {
*leaves = nodes[root];
leaves++;
}
}
//==============================================================================
template <typename BV>
nodeBaseLess<BV>::nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
: nodes(nodes_), d(d_) {
// Do nothing
}
//==============================================================================
template <typename BV>
bool nodeBaseLess<BV>::operator()(size_t i, size_t j) const {
if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true;
return false;
}
//==============================================================================
template <typename S, typename BV>
struct SelectImpl {
static bool run(size_t query, size_t node1, size_t node2,
NodeBase<BV>* nodes) {
COAL_UNUSED_VARIABLE(query);
COAL_UNUSED_VARIABLE(node1);
COAL_UNUSED_VARIABLE(node2);
COAL_UNUSED_VARIABLE(nodes);
return 0;
}
static bool run(const BV& query, size_t node1, size_t node2,
NodeBase<BV>* nodes) {
COAL_UNUSED_VARIABLE(query);
COAL_UNUSED_VARIABLE(node1);
COAL_UNUSED_VARIABLE(node2);
COAL_UNUSED_VARIABLE(nodes);
return 0;
}
};
//==============================================================================
template <typename BV>
size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) {
return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
}
//==============================================================================
template <typename BV>
size_t select(const BV& query, size_t node1, size_t node2,
NodeBase<BV>* nodes) {
return SelectImpl<CoalScalar, BV>::run(query, node1, node2, nodes);
}
//==============================================================================
template <typename S>
struct SelectImpl<S, AABB> {
static bool run(size_t query, size_t node1, size_t node2,
NodeBase<AABB>* nodes) {
const AABB& bv = nodes[query].bv;
const AABB& bv1 = nodes[node1].bv;
const AABB& bv2 = nodes[node2].bv;
Vec3s v = bv.min_ + bv.max_;
Vec3s v1 = v - (bv1.min_ + bv1.max_);
Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
}
static bool run(const AABB& query, size_t node1, size_t node2,
NodeBase<AABB>* nodes) {
const AABB& bv = query;
const AABB& bv1 = nodes[node1].bv;
const AABB& bv2 = nodes[node2].bv;
Vec3s v = bv.min_ + bv.max_;
Vec3s v1 = v - (bv1.min_ + bv1.max_);
Vec3s v2 = v - (bv2.min_ + bv2.max_);
CoalScalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
CoalScalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
return (d1 < d2) ? 0 : 1;
}
};
} // namespace implementation_array
} // namespace detail
} // namespace coal
#endif
include/coal/broadphase/detail/hierarchy_tree_array.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_HIERARCHY_TREE_ARRAY_H
#define COAL_HIERARCHY_TREE_ARRAY_H
#include <vector>
#include <map>
#include <functional>
#include "coal/fwd.hh"
#include "coal/BV/AABB.h"
#include "coal/broadphase/detail/morton.h"
#include "coal/broadphase/detail/node_base_array.h"
namespace coal {
namespace detail {
namespace implementation_array {
/// @brief Class for hierarchy tree structure
template <typename BV>
class HierarchyTree {
public:
typedef NodeBase<BV> Node;
private:
struct SortByMorton {
SortByMorton(Node* nodes_in) : nodes(nodes_in) {}
SortByMorton(Node* nodes_in, uint32_t split_in)
: nodes(nodes_in), split(split_in) {}
bool operator()(size_t a, size_t b) const {
if ((a != NULL_NODE) && (b != NULL_NODE))
return nodes[a].code < nodes[b].code;
else if (a == NULL_NODE)
return split < nodes[b].code;
else if (b == NULL_NODE)
return nodes[a].code < split;
return false;
}
Node* nodes{};
uint32_t split{};
};
public:
/// @brief Create hierarchy tree with suitable setting.
/// bu_threshold decides the height of tree node to start bottom-up
/// construction / optimization; topdown_level decides different methods to
/// construct tree in topdown manner. lower level method constructs tree with
/// better quality but is slower.
HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
~HierarchyTree();
/// @brief Initialize the tree by a set of leaves using algorithm with a given
/// level.
void init(Node* leaves, int n_leaves_, int level = 0);
/// @brief Initialize the tree by a set of leaves using algorithm with a given
/// level.
size_t insert(const BV& bv, void* data);
/// @brief Remove a leaf node
void remove(size_t leaf);
/// @brief Clear the tree
void clear();
/// @brief Whether the tree is empty
bool empty() const;
/// @brief update one leaf node
void update(size_t leaf, int lookahead_level = -1);
/// @brief update the tree when the bounding volume of a given leaf has
/// changed
bool update(size_t leaf, const BV& bv);
/// @brief update one leaf's bounding volume, with prediction
bool update(size_t leaf, const BV& bv, const Vec3s& vel, CoalScalar margin);
/// @brief update one leaf's bounding volume, with prediction
bool update(size_t leaf, const BV& bv, const Vec3s& vel);
/// @brief get the max height of the tree
size_t getMaxHeight() const;
/// @brief get the max depth of the tree
size_t getMaxDepth() const;
/// @brief balance the tree from bottom
void balanceBottomup();
/// @brief balance the tree from top
void balanceTopdown();
/// @brief balance the tree in an incremental way
void balanceIncremental(int iterations);
/// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
/// update the entire tree in a bottom-up manner
void refit();
/// @brief extract all the leaves of the tree
void extractLeaves(size_t root, Node*& leaves) const;
/// @brief number of leaves in the tree
size_t size() const;
/// @brief get the root of the tree
size_t getRoot() const;
/// @brief get the pointer to the nodes array
Node* getNodes() const;
/// @brief print the tree in a recursive way
void print(size_t root, int depth);
private:
/// @brief construct a tree for a set of leaves from bottom -- very heavy way
void bottomup(size_t* lbeg, size_t* lend);
/// @brief construct a tree for a set of leaves from top
size_t topdown(size_t* lbeg, size_t* lend);
/// @brief compute the maximum height of a subtree rooted from a given node
size_t getMaxHeight(size_t node) const;
/// @brief compute the maximum depth of a subtree rooted from a given node
void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
/// topdown manner. During construction, first compute the best split axis as
/// the axis along with the longest AABB edge. Then compute the median of all
/// nodes' center projection onto the axis and using it as the split
/// threshold.
size_t topdown_0(size_t* lbeg, size_t* lend);
/// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
/// topdown manner. During construction, first compute the best split
/// thresholds for different axes as the average of all nodes' center. Then
/// choose the split axis as the axis whose threshold can divide the nodes
/// into two parts with almost similar size. This construction is more
/// expensive then topdown_0, but also can provide tree with better quality.
size_t topdown_1(size_t* lbeg, size_t* lend);
/// @brief init tree from leaves in the topdown manner (topdown_0 or
/// topdown_1)
void init_0(Node* leaves, int n_leaves_);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
/// for nodes which is of depth more than the maximum bits of the morton code,
/// we use bottomup method to construct the subtree, which is slow but can
/// construct tree with high quality.
void init_1(Node* leaves, int n_leaves_);
/// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
/// for nodes which is of depth more than the maximum bits of the morton code,
/// we split the leaves into two parts with the same size simply using the
/// node index.
void init_2(Node* leaves, int n_leaves_);
/// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
/// for all nodes, we simply divide the leaves into parts with the same size
/// simply using the node index.
void init_3(Node* leaves, int n_leaves_);
size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split,
int bits);
size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split,
int bits);
size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
/// @brief update one leaf node's bounding volume
void update_(size_t leaf, const BV& bv);
/// @brief Insert a leaf node and also update its ancestors
void insertLeaf(size_t root, size_t leaf);
/// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the
/// unnecessary internal nodes are deleted. return the node with the smallest
/// depth and is influenced by the remove operation
size_t removeLeaf(size_t leaf);
/// @brief Delete all internal nodes and return all leaves nodes with given
/// depth from root
void fetchLeaves(size_t root, Node*& leaves, int depth = -1);
size_t indexOf(size_t node);
size_t allocateNode();
/// @brief create one node (leaf or internal)
size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data);
size_t createNode(size_t parent, const BV& bv, void* data);
size_t createNode(size_t parent, void* data);
void deleteNode(size_t node);
void recurseRefit(size_t node);
protected:
size_t root_node;
Node* nodes;
size_t n_nodes;
size_t n_nodes_alloc;
size_t n_leaves;
size_t freelist;
unsigned int opath;
int max_lookahead_level;
public:
/// @brief decide which topdown algorithm to use
int topdown_level;
/// @brief decide the depth to use expensive bottom-up algorithm
int bu_threshold;
public:
static const size_t NULL_NODE = std::numeric_limits<size_t>::max();
};
template <typename BV>
const size_t HierarchyTree<BV>::NULL_NODE;
/// @brief Functor comparing two nodes
template <typename BV>
struct nodeBaseLess {
nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
bool operator()(size_t i, size_t j) const;
private:
/// @brief the nodes array
const NodeBase<BV>* nodes;
/// @brief the dimension to compare
size_t d;
};
/// @brief select the node from node1 and node2 which is close to the query-th
/// node in the nodes. 0 for node1 and 1 for node2.
template <typename BV>
size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
/// @brief select the node from node1 and node2 which is close to the query
/// AABB. 0 for node1 and 1 for node2.
template <typename BV>
size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes);
} // namespace implementation_array
} // namespace detail
} // namespace coal
#include "coal/broadphase/detail/hierarchy_tree_array-inl.h"
#endif
include/fcl/broadphase/interval_tree.h → include/coal/broadphase/detail/interval_tree.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -32,75 +33,35 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
/** @author Jia Pan */
#ifndef FCL_INTERVAL_TREE_H
#define FCL_INTERVAL_TREE_H
#ifndef COAL_INTERVAL_TREE_H
#define COAL_INTERVAL_TREE_H
#include <deque>
#include <limits>
#include <cstdlib>
namespace fcl
{
/// @brief Interval trees implemented using red-black-trees as described in
/// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest.
/// Can be replaced in part by boost::icl::interval_set, which is only supported after boost 1.46 and does not support delete node routine.
struct SimpleInterval
{
public:
virtual ~SimpleInterval() {}
virtual void print() {}
/// @brief interval is defined as [low, high]
double low, high;
};
/// @brief The node for interval tree
class IntervalTreeNode
{
friend class IntervalTree;
public:
/// @brief Print the interval node information: set left = nil and right = root
void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
/// @brief Create an empty node
IntervalTreeNode();
/// @brief Create an node storing the interval
IntervalTreeNode(SimpleInterval* new_interval);
~IntervalTreeNode();
protected:
/// @brief interval stored in the node
SimpleInterval* stored_interval;
double key;
#include "coal/broadphase/detail/interval_tree_node.h"
double high;
namespace coal {
namespace detail {
double max_high;
/// @brief Class describes the information needed when we take the
/// right branch in searching for intervals but possibly come back
/// and check the left branch as well.
struct COAL_DLLAPI it_recursion_node {
public:
IntervalTreeNode* start_node;
/// @brief red or black node: if red = false then the node is black
bool red;
unsigned int parent_index;
IntervalTreeNode* left;
IntervalTreeNode* right;
IntervalTreeNode* parent;
bool try_right_branch;
};
struct it_recursion_node;
/// @brief Interval tree
class IntervalTree
{
public:
class COAL_DLLAPI IntervalTree {
public:
IntervalTree();
~IntervalTree();
......@@ -124,13 +85,12 @@ public:
IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const;
/// @brief Return result for a given query
std::deque<SimpleInterval*> query(double low, double high);
protected:
std::deque<SimpleInterval*> query(CoalScalar low, CoalScalar high);
protected:
IntervalTreeNode* root;
IntervalTreeNode* nil;
IntervalTreeNode* invalid_node;
/// @brief left rotation of tree node
void leftRotate(IntervalTreeNode* node);
......@@ -138,29 +98,30 @@ protected:
/// @brief right rotation of tree node
void rightRotate(IntervalTreeNode* node);
/// @brief recursively insert a node
/// @brief Inserts node into the tree as if it were a regular binary tree
void recursiveInsert(IntervalTreeNode* node);
/// @brief recursively print a subtree
/// @brief recursively print a subtree
void recursivePrint(IntervalTreeNode* node) const;
/// @brief recursively find the node corresponding to the interval
IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const;
IntervalTreeNode* recursiveSearch(IntervalTreeNode* node,
SimpleInterval* ivl) const;
/// @brief Travels up to the root fixing the max_high fields after an insertion or deletion
/// @brief Travels up to the root fixing the max_high fields after an
/// insertion or deletion
void fixupMaxHigh(IntervalTreeNode* node);
void deleteFixup(IntervalTreeNode* node);
private:
private:
unsigned int recursion_node_stack_size;
it_recursion_node* recursion_node_stack;
unsigned int current_parent;
unsigned int recursion_node_stack_top;
};
}
} // namespace detail
} // namespace coal
#endif
include/coal/broadphase/detail/interval_tree_node-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_INL_H
#include "coal/broadphase/detail/interval_tree_node.h"
#include <iostream>
#include <algorithm>
namespace coal {
namespace detail {
//==============================================================================
IntervalTreeNode::IntervalTreeNode() {
// Do nothing
}
//==============================================================================
IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval)
: stored_interval(new_interval),
key(new_interval->low),
high(new_interval->high),
max_high(high) {
// Do nothing
}
//==============================================================================
IntervalTreeNode::~IntervalTreeNode() {
// Do nothing
}
//==============================================================================
void IntervalTreeNode::print(IntervalTreeNode* nil,
IntervalTreeNode* root) const {
stored_interval->print();
std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high;
std::cout << " l->key = ";
if (left == nil)
std::cout << "nullptr";
else
std::cout << left->key;
std::cout << " r->key = ";
if (right == nil)
std::cout << "nullptr";
else
std::cout << right->key;
std::cout << " p->key = ";
if (parent == root)
std::cout << "nullptr";
else
std::cout << parent->key;
std::cout << " red = " << (int)red << std::endl;
}
} // namespace detail
} // namespace coal
#endif
include/fcl/ccd/interpolation/interpolation.h → include/coal/broadphase/detail/interval_tree_node.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -32,59 +33,58 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Dalibor Matura, Jia Pan */
/** @author Jia Pan */
#ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_H
#define FCL_CCD_INTERPOLATION_INTERPOLATION_H
#ifndef COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H
#define COAL_BROADPHASE_DETAIL_INTERVALTREENODE_H
#include "fcl/data_types.h"
#include "coal/broadphase/detail/simple_interval.h"
#include "coal/fwd.hh"
namespace fcl
{
namespace coal {
enum InterpolationType
{
LINEAR,
STANDARD
};
class Interpolation
{
public:
Interpolation();
namespace detail {
virtual ~Interpolation() {}
class COAL_DLLAPI IntervalTree;
Interpolation(FCL_REAL start_value, FCL_REAL end_value);
/// @brief The node for interval tree
class COAL_DLLAPI IntervalTreeNode {
public:
friend class IntervalTree;
void setStartValue(FCL_REAL start_value);
void setEndValue(FCL_REAL end_value);
/// @brief Create an empty node
IntervalTreeNode();
virtual FCL_REAL getValue(FCL_REAL time) const = 0;
/// @brief Create an node storing the interval
IntervalTreeNode(SimpleInterval* new_interval);
/// @brief return the smallest value in time interval [0, 1]
virtual FCL_REAL getValueLowerBound() const = 0;
~IntervalTreeNode();
/// @brief return the biggest value in time interval [0, 1]
virtual FCL_REAL getValueUpperBound() const = 0;
/// @brief Print the interval node information: set left = invalid_node and
/// right = root
void print(IntervalTreeNode* left, IntervalTreeNode* right) const;
virtual InterpolationType getType() const = 0;
protected:
/// @brief interval stored in the node
SimpleInterval* stored_interval;
bool operator == (const Interpolation& interpolation) const;
bool operator != (const Interpolation& interpolation) const;
CoalScalar key;
virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const = 0;
CoalScalar high;
virtual FCL_REAL getVelocityBound(FCL_REAL time) const = 0;
CoalScalar max_high;
protected:
FCL_REAL value_0_; // value at time = 0.0
FCL_REAL value_1_; // value at time = 1.0
/// @brief red or black node: if red = false then the node is black
bool red;
};
IntervalTreeNode* left;
IntervalTreeNode* right;
IntervalTreeNode* parent;
};
}
} // namespace detail
} // namespace coal
#endif
include/coal/broadphase/detail/morton-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* Copyright (c) 2016, Toyota Research Institute
* 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 COAL_MORTON_INL_H
#define COAL_MORTON_INL_H
#include "coal/broadphase/detail/morton.h"
namespace coal { /// @cond IGNORE
namespace detail {
//==============================================================================
template <typename S>
uint32_t quantize(S x, uint32_t n) {
return std::max(std::min((uint32_t)(x * (S)n), uint32_t(n - 1)), uint32_t(0));
}
//==============================================================================
template <typename S>
morton_functor<S, uint32_t>::morton_functor(const AABB& bbox)
: base(bbox.min_),
inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
1.0 / (bbox.max_[1] - bbox.min_[1]),
1.0 / (bbox.max_[2] - bbox.min_[2])) {
// Do nothing
}
//==============================================================================
template <typename S>
uint32_t morton_functor<S, uint32_t>::operator()(const Vec3s& point) const {
uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1024u);
uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1024u);
uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1024u);
return detail::morton_code(x, y, z);
}
//==============================================================================
template <typename S>
morton_functor<S, uint64_t>::morton_functor(const AABB& bbox)
: base(bbox.min_),
inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
1.0 / (bbox.max_[1] - bbox.min_[1]),
1.0 / (bbox.max_[2] - bbox.min_[2])) {
// Do nothing
}
//==============================================================================
template <typename S>
uint64_t morton_functor<S, uint64_t>::operator()(const Vec3s& point) const {
uint32_t x = detail::quantize((point[0] - base[0]) * inv[0], 1u << 20);
uint32_t y = detail::quantize((point[1] - base[1]) * inv[1], 1u << 20);
uint32_t z = detail::quantize((point[2] - base[2]) * inv[2], 1u << 20);
return detail::morton_code60(x, y, z);
}
//==============================================================================
template <typename S>
constexpr size_t morton_functor<S, uint64_t>::bits() {
return 60;
}
//==============================================================================
template <typename S>
constexpr size_t morton_functor<S, uint32_t>::bits() {
return 30;
}
//==============================================================================
template <typename S, size_t N>
morton_functor<S, std::bitset<N>>::morton_functor(const AABB& bbox)
: base(bbox.min_),
inv(1.0 / (bbox.max_[0] - bbox.min_[0]),
1.0 / (bbox.max_[1] - bbox.min_[1]),
1.0 / (bbox.max_[2] - bbox.min_[2])) {
// Do nothing
}
//==============================================================================
template <typename S, size_t N>
std::bitset<N> morton_functor<S, std::bitset<N>>::operator()(
const Vec3s& point) const {
S x = (point[0] - base[0]) * inv[0];
S y = (point[1] - base[1]) * inv[1];
S z = (point[2] - base[2]) * inv[2];
int start_bit = bits() - 1;
std::bitset<N> bset;
x *= 2;
y *= 2;
z *= 2;
for (size_t i = 0; i < bits() / 3; ++i) {
bset[start_bit--] = ((z < 1) ? 0 : 1);
bset[start_bit--] = ((y < 1) ? 0 : 1);
bset[start_bit--] = ((x < 1) ? 0 : 1);
x = ((x >= 1) ? 2 * (x - 1) : 2 * x);
y = ((y >= 1) ? 2 * (y - 1) : 2 * y);
z = ((z >= 1) ? 2 * (z - 1) : 2 * z);
}
return bset;
}
//==============================================================================
template <typename S, size_t N>
constexpr size_t morton_functor<S, std::bitset<N>>::bits() {
return N;
}
} // namespace detail
/// @endcond
} // namespace coal
#endif
include/coal/broadphase/detail/morton.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* Copyright (c) 2016, Toyota Research Institute
* 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 COAL_MORTON_H
#define COAL_MORTON_H
#include "coal/BV/AABB.h"
#include <cstdint>
#include <bitset>
namespace coal {
/// @cond IGNORE
namespace detail {
template <typename S>
uint32_t quantize(S x, uint32_t n);
/// @brief compute 30 bit morton code
COAL_DLLAPI
uint32_t morton_code(uint32_t x, uint32_t y, uint32_t z);
/// @brief compute 60 bit morton code
COAL_DLLAPI
uint64_t morton_code60(uint32_t x, uint32_t y, uint32_t z);
/// @brief Functor to compute the morton code for a given AABB
/// This is specialized for 32- and 64-bit unsigned integers giving
/// a 30- or 60-bit code, respectively, and for `std::bitset<N>` where
/// N is the length of the code and must be a multiple of 3.
template <typename S, typename T>
struct morton_functor {};
/// @brief Functor to compute 30 bit morton code for a given AABB
template <typename S>
struct morton_functor<S, uint32_t> {
morton_functor(const AABB& bbox);
uint32_t operator()(const Vec3s& point) const;
const Vec3s base;
const Vec3s inv;
static constexpr size_t bits();
};
using morton_functoru32f = morton_functor<float, uint32_t>;
using morton_functoru32d = morton_functor<CoalScalar, uint32_t>;
/// @brief Functor to compute 60 bit morton code for a given AABB
template <typename S>
struct morton_functor<S, uint64_t> {
morton_functor(const AABB& bbox);
uint64_t operator()(const Vec3s& point) const;
const Vec3s base;
const Vec3s inv;
static constexpr size_t bits();
};
/// @brief Functor to compute N bit morton code for a given AABB
/// N must be a multiple of 3.
template <typename S, size_t N>
struct morton_functor<S, std::bitset<N>> {
static_assert(N % 3 == 0, "Number of bits must be a multiple of 3");
morton_functor(const AABB& bbox);
std::bitset<N> operator()(const Vec3s& point) const;
const Vec3s base;
const Vec3s inv;
static constexpr size_t bits();
};
} // namespace detail
/// @endcond
} // namespace coal
#include "coal/broadphase/detail/morton-inl.h"
#endif
include/coal/broadphase/detail/node_base-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_BROADPHASE_DETAIL_NODEBASE_INL_H
#define COAL_BROADPHASE_DETAIL_NODEBASE_INL_H
#include "coal/broadphase/detail/node_base.h"
namespace coal {
namespace detail {
//============================================================================//
// //
// Implementations //
// //
//============================================================================//
//==============================================================================
template <typename BV>
bool NodeBase<BV>::isLeaf() const {
return (children[1] == nullptr);
}
//==============================================================================
template <typename BV>
bool NodeBase<BV>::isInternal() const {
return !isLeaf();
}
//==============================================================================
template <typename BV>
NodeBase<BV>::NodeBase() {
parent = nullptr;
children[0] = nullptr;
children[1] = nullptr;
}
} // namespace detail
} // namespace coal
#endif
include/coal/broadphase/detail/node_base.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_BROADPHASE_DETAIL_NODEBASE_H
#define COAL_BROADPHASE_DETAIL_NODEBASE_H
#include "coal/data_types.h"
namespace coal {
namespace detail {
/// @brief dynamic AABB tree node
template <typename BV>
struct NodeBase {
/// @brief the bounding volume for the node
BV bv;
/// @brief pointer to parent node
NodeBase<BV>* parent;
/// @brief whether is a leaf
bool isLeaf() const;
/// @brief whether is internal node
bool isInternal() const;
union {
/// @brief for leaf node, children nodes
NodeBase<BV>* children[2];
void* data;
};
/// @brief morton code for current BV
uint32_t code;
NodeBase();
};
} // namespace detail
} // namespace coal
#include "coal/broadphase/detail/node_base-inl.h"
#endif
src/articulated_model/link.cpp → include/coal/broadphase/detail/node_base_array-inl.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -32,52 +33,33 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Dalibor Matura, Jia Pan */
/** @author Jia Pan */
#include "fcl/articulated_model/link.h"
#ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_INL_H
#include "fcl/articulated_model/joint.h"
#include "coal/broadphase/detail/node_base_array.h"
namespace fcl
{
namespace coal {
Link::Link(const std::string& name) : name_(name)
{}
namespace detail {
const std::string& Link::getName() const
{
return name_;
}
void Link::setName(const std::string& name)
{
name_ = name;
}
void Link::addChildJoint(const boost::shared_ptr<Joint>& joint)
{
children_joints_.push_back(joint);
}
namespace implementation_array {
void Link::setParentJoint(const boost::shared_ptr<Joint>& joint)
{
parent_joint_ = joint;
//==============================================================================
template <typename BV>
bool NodeBase<BV>::isLeaf() const {
return (children[1] == (size_t)(-1));
}
void Link::addObject(const boost::shared_ptr<CollisionObject>& object)
{
objects_.push_back(object);
//==============================================================================
template <typename BV>
bool NodeBase<BV>::isInternal() const {
return !isLeaf();
}
std::size_t Link::getNumChildJoints() const
{
return children_joints_.size();
}
std::size_t Link::getNumObjects() const
{
return objects_.size();
}
} // namespace implementation_array
} // namespace detail
} // namespace coal
}
#endif
include/fcl/ccd/conservative_advancement.h → include/coal/broadphase/detail/node_base_array.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -32,36 +33,43 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
/** @author Jia Pan */
#ifndef FCL_CONSERVATIVE_ADVANCEMENT_H
#define FCL_CONSERVATIVE_ADVANCEMENT_H
#ifndef COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H
#define COAL_BROADPHASE_DETAIL_NODEBASEARRAY_H
#include "coal/data_types.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
#include "fcl/ccd/motion_base.h"
namespace coal {
namespace detail {
namespace fcl
{
namespace implementation_array {
template<typename NarrowPhaseSolver>
struct ConservativeAdvancementFunctionMatrix
{
typedef FCL_REAL (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result);
template <typename BV>
struct NodeBase {
BV bv;
ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT];
ConservativeAdvancementFunctionMatrix();
};
union {
size_t parent;
size_t next;
};
union {
size_t children[2];
void* data;
};
uint32_t code;
}
#endif
bool isLeaf() const;
bool isInternal() const;
};
} // namespace implementation_array
} // namespace detail
} // namespace coal
#include "coal/broadphase/detail/node_base_array-inl.h"
#endif
include/fcl/BVH/BVH_utility.h → include/coal/broadphase/detail/simple_hash_table-inl.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -32,66 +33,79 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Jia Pan */
/** @author Jia Pan */
#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H
#define COAL_BROADPHASE_SIMPLEHASHTABLE_INL_H
#ifndef FCL_BVH_UTILITY_H
#define FCL_BVH_UTILITY_H
#include "coal/broadphase/detail/simple_hash_table.h"
#include "fcl/BVH/BVH_model.h"
#include <iterator>
namespace coal {
namespace detail {
namespace fcl
{
/// @brief Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node
template<typename BV>
void BVHExpand(BVHModel<BV>& model, const Variance3f* ucs, FCL_REAL r)
{
for(int i = 0; i < model.num_bvs; ++i)
{
BVNode<BV>& bvnode = model.getBV(i);
BV bv;
for(int j = 0; j < bvnode.num_primitives; ++j)
{
int v_id = bvnode.first_primitive + j;
const Variance3f& uc = ucs[v_id];
Vec3f& v = model.vertices[bvnode.first_primitive + j];
for(int k = 0; k < 3; ++k)
{
bv += (v + uc.axis[k] * (r * uc.sigma[k]));
bv += (v - uc.axis[k] * (r * uc.sigma[k]));
}
}
bvnode.bv = bv;
}
//==============================================================================
template <typename Key, typename Data, typename HashFnc>
SimpleHashTable<Key, Data, HashFnc>::SimpleHashTable(const HashFnc& h) : h_(h) {
// Do nothing
}
/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for OBB
void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS
void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
/// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
//==============================================================================
template <typename Key, typename Data, typename HashFnc>
void SimpleHashTable<Key, Data, HashFnc>::init(size_t size) {
if (size == 0) {
COAL_THROW_PRETTY("SimpleHashTable must have non-zero size.",
std::logic_error);
}
/// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
table_.resize(size);
table_size_ = size;
}
/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
//==============================================================================
template <typename Key, typename Data, typename HashFnc>
void SimpleHashTable<Key, Data, HashFnc>::insert(Key key, Data value) {
std::vector<unsigned int> indices = h_(key);
size_t range = table_.size();
for (size_t i = 0; i < indices.size(); ++i)
table_[indices[i] % range].push_back(value);
}
/// @brief Compute the center and radius for a triangle's circumcircle
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius);
//==============================================================================
template <typename Key, typename Data, typename HashFnc>
std::vector<Data> SimpleHashTable<Key, Data, HashFnc>::query(Key key) const {
size_t range = table_.size();
std::vector<unsigned int> indices = h_(key);
std::set<Data> result;
for (size_t i = 0; i < indices.size(); ++i) {
size_t index = indices[i] % range;
std::copy(table_[index].begin(), table_[index].end(),
std::inserter(result, result.end()));
}
/// @brief Compute the maximum distance from a given center point to a point cloud
FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
return std::vector<Data>(result.begin(), result.end());
}
//==============================================================================
template <typename Key, typename Data, typename HashFnc>
void SimpleHashTable<Key, Data, HashFnc>::remove(Key key, Data value) {
size_t range = table_.size();
std::vector<unsigned int> indices = h_(key);
for (size_t i = 0; i < indices.size(); ++i) {
size_t index = indices[i] % range;
table_[index].remove(value);
}
}
//==============================================================================
template <typename Key, typename Data, typename HashFnc>
void SimpleHashTable<Key, Data, HashFnc>::clear() {
table_.clear();
table_.resize(table_size_);
}
} // namespace detail
} // namespace coal
#endif
include/fcl/articulated_model/joint_config.h → include/coal/broadphase/detail/simple_hash_table.h
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
......@@ -14,7 +15,7 @@
* 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 Willow Garage, Inc. nor the names of its
* * 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.
*
......@@ -32,69 +33,55 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Dalibor Matura, Jia Pan */
/** @author Jia Pan */
#ifndef FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
#define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
#ifndef COAL_BROADPHASE_SIMPLEHASHTABLE_H
#define COAL_BROADPHASE_SIMPLEHASHTABLE_H
#include "fcl/data_types.h"
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <set>
#include <vector>
#include <list>
namespace fcl
{
class Joint;
class JointConfig
{
public:
JointConfig();
JointConfig(const JointConfig& joint_cfg);
JointConfig(const boost::shared_ptr<Joint>& joint,
FCL_REAL default_value = 0,
FCL_REAL default_value_min = 0,
FCL_REAL default_value_max = 0);
std::size_t getDim() const;
inline FCL_REAL operator [] (std::size_t i) const
{
return values_[i];
}
inline FCL_REAL& operator [] (std::size_t i)
{
return values_[i];
}
FCL_REAL getValue(std::size_t i) const;
FCL_REAL& getValue(std::size_t i);
FCL_REAL getLimitMin(std::size_t i) const;
FCL_REAL& getLimitMin(std::size_t i);
FCL_REAL getLimitMax(std::size_t i) const;
FCL_REAL& getLimitMax(std::size_t i);
boost::shared_ptr<Joint> getJoint() const;
private:
boost::weak_ptr<Joint> joint_;
std::vector<FCL_REAL> values_;
std::vector<FCL_REAL> limits_min_;
std::vector<FCL_REAL> limits_max_;
};
namespace coal {
namespace detail {
/// @brief A simple hash table implemented as multiple buckets. HashFnc is any
/// extended hash function: HashFnc(key) = {index1, index2, ..., }
template <typename Key, typename Data, typename HashFnc>
class SimpleHashTable {
protected:
typedef std::list<Data> Bin;
std::vector<Bin> table_;
HashFnc h_;
size_t table_size_;
}
public:
SimpleHashTable(const HashFnc& h);
/// @brief Init the number of bins in the hash table
void init(size_t size);
//// @brief Insert a key-value pair into the table
void insert(Key key, Data value);
/// @brief Find the elements in the hash table whose key is the same as query
/// key.
std::vector<Data> query(Key key) const;
/// @brief remove the key-value pair from the table
void remove(Key key, Data value);
/// @brief clear the hash table
void clear();
};
} // namespace detail
} // namespace coal
#include "coal/broadphase/detail/simple_hash_table-inl.h"
#endif
include/coal/broadphase/detail/simple_interval-inl.h 0 → 100644
View file @ 8d47200c
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2016, 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 COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
#define COAL_BROADPHASE_DETAIL_SIMPLEINTERVAL_INL_H
#include "coal/broadphase/detail/simple_interval.h"
#include <algorithm>
namespace coal {
namespace detail {
//==============================================================================
SimpleInterval::~SimpleInterval() {
// Do nothing
}
//==============================================================================
void SimpleInterval::print() {
// Do nothing
}
} // namespace detail
} // namespace coal
#endif