diff --git a/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h index 46215b014..f69841b8c 100644 --- a/include/fcl/broadphase/broadphase_spatialhash.h +++ b/include/fcl/broadphase/broadphase_spatialhash.h @@ -35,66 +35,23 @@ /** \author Jia Pan */ -#ifndef FCL_BROAD_PHASE_SPATIAL_HASH_H -#define FCL_BROAD_PHASE_SPATIAL_HASH_H +#ifndef FCL_BROADPHASE_BROADPAHSESPATIALHASH_H +#define FCL_BROADPHASE_BROADPAHSESPATIALHASH_H -#include "fcl/broadphase/broadphase.h" -#include "fcl/broadphase/hash.h" -#include "fcl/BV/AABB.h" #include #include +#include "fcl/BV/AABB.h" +#include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/simple_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" namespace fcl { -/// @brief Spatial hash function: hash an AABB to a set of integer values -template -struct SpatialHash -{ - SpatialHash(const AABB& scene_limit_, S 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 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 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: - - S cell_size; - AABB scene_limit; - unsigned int width[3]; -}; - -using SpatialHashf = SpatialHash; -using SpatialHashd = SpatialHash; - /// @brief spatial hashing collision mananger -template, CollisionObject*, SpatialHash> > +template, CollisionObject*, SpatialHash> > class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { public: @@ -102,17 +59,9 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager S cell_size, const Vector3& scene_min, const Vector3& 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); - } + unsigned int default_table_size = 1000); - ~SpatialHashingCollisionManager() - { - delete hash_table; - } + ~SpatialHashingCollisionManager(); /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -163,15 +112,7 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager size_t size() const; /// @brief compute the bound for the environent - static void computeBound(std::vector*>& objs, Vector3& l, Vector3& u) - { - AABB bound; - for(unsigned int i = 0; i < objs.size(); ++i) - bound += objs[i]->getAABB(); - - l = bound.min_; - u = bound.max_; - } + static void computeBound(std::vector*>& objs, Vector3& l, Vector3& u); protected: @@ -181,7 +122,6 @@ class SpatialHashingCollisionManager : public BroadPhaseCollisionManager /// @brief perform distance computation between one object and all the objects belonging ot the manager bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; - /// @brief all objects in the scene std::list*> objs; @@ -213,7 +153,28 @@ using SpatialHashingCollisionManagerd = SpatialHashingCollisionManager -void SpatialHashingCollisionManager::registerObject(CollisionObject* obj) +SpatialHashingCollisionManager::SpatialHashingCollisionManager( + S cell_size, + const Vector3& scene_min, + const Vector3& scene_max, + unsigned int default_table_size) + : scene_limit(AABB(scene_min, scene_max)), + hash_table(new HashTable(SpatialHash(scene_limit, cell_size))) +{ + hash_table->init(default_table_size); +} + +//============================================================================== +template +SpatialHashingCollisionManager::~SpatialHashingCollisionManager() +{ + delete hash_table; +} + +//============================================================================== +template +void SpatialHashingCollisionManager::registerObject( + CollisionObject* obj) { objs.push_back(obj); @@ -233,6 +194,7 @@ void SpatialHashingCollisionManager::registerObject(CollisionObjec obj_aabb_map[obj] = obj_aabb; } +//============================================================================== template void SpatialHashingCollisionManager::unregisterObject(CollisionObject* obj) { @@ -254,10 +216,14 @@ void SpatialHashingCollisionManager::unregisterObject(CollisionObj obj_aabb_map.erase(obj); } +//============================================================================== template void SpatialHashingCollisionManager::setup() -{} +{ + // Do nothing +} +//============================================================================== template void SpatialHashingCollisionManager::update() { @@ -284,6 +250,7 @@ void SpatialHashingCollisionManager::update() } } +//============================================================================== template void SpatialHashingCollisionManager::update(CollisionObject* updated_obj) { @@ -315,6 +282,7 @@ void SpatialHashingCollisionManager::update(CollisionObject* up obj_aabb_map[updated_obj] = new_aabb; } +//============================================================================== template void SpatialHashingCollisionManager::update(const std::vector*>& updated_objs) { @@ -322,7 +290,7 @@ void SpatialHashingCollisionManager::update(const std::vector void SpatialHashingCollisionManager::clear() { @@ -332,6 +300,7 @@ void SpatialHashingCollisionManager::clear() obj_aabb_map.clear(); } +//============================================================================== template void SpatialHashingCollisionManager::getObjects(std::vector*>& objs_) const { @@ -339,6 +308,7 @@ void SpatialHashingCollisionManager::getObjects(std::vector void SpatialHashingCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { @@ -346,6 +316,7 @@ void SpatialHashingCollisionManager::collide(CollisionObject* o collide_(obj, cdata, callback); } +//============================================================================== template void SpatialHashingCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { @@ -354,6 +325,7 @@ void SpatialHashingCollisionManager::distance(CollisionObject* distance_(obj, cdata, callback, min_dist); } +//============================================================================== template bool SpatialHashingCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { @@ -393,6 +365,7 @@ bool SpatialHashingCollisionManager::collide_(CollisionObject* return false; } +//============================================================================== template bool SpatialHashingCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const { @@ -509,6 +482,7 @@ bool SpatialHashingCollisionManager::distance_(CollisionObject* return false; } +//============================================================================== template void SpatialHashingCollisionManager::collide(void* cdata, CollisionCallBack callback) const { @@ -547,6 +521,7 @@ void SpatialHashingCollisionManager::collide(void* cdata, Collisio } } +//============================================================================== template void SpatialHashingCollisionManager::distance(void* cdata, DistanceCallBack callback) const { @@ -564,6 +539,7 @@ void SpatialHashingCollisionManager::distance(void* cdata, Distanc this->tested_set.clear(); } +//============================================================================== template void SpatialHashingCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { @@ -589,6 +565,7 @@ void SpatialHashingCollisionManager::collide(BroadPhaseCollisionMa } } +//============================================================================== template void SpatialHashingCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { @@ -616,18 +593,33 @@ void SpatialHashingCollisionManager::distance(BroadPhaseCollisionM } } +//============================================================================== template bool SpatialHashingCollisionManager::empty() const { return objs.empty(); } +//============================================================================== template size_t SpatialHashingCollisionManager::size() const { return objs.size(); } +//============================================================================== +template +void SpatialHashingCollisionManager::computeBound( + std::vector*>& objs, Vector3& l, Vector3& u) +{ + AABB bound; + for(unsigned int i = 0; i < objs.size(); ++i) + bound += objs[i]->getAABB(); + + l = bound.min_; + u = bound.max_; +} + } // namespace fcl #endif diff --git a/include/fcl/broadphase/broadphase_spatialhash.hxx b/include/fcl/broadphase/broadphase_spatialhash.hxx deleted file mode 100644 index 33a403346..000000000 --- a/include/fcl/broadphase/broadphase_spatialhash.hxx +++ /dev/null @@ -1,43 +0,0 @@ -/* - * 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 */ - -namespace fcl -{ - - - -} diff --git a/include/fcl/broadphase/hash.h b/include/fcl/broadphase/hash.h deleted file mode 100644 index ff5fbcffc..000000000 --- a/include/fcl/broadphase/hash.h +++ /dev/null @@ -1,189 +0,0 @@ -/* - * 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 FCL_HASH_H -#define FCL_HASH_H - -#include -#include -#include -#include -#include - -namespace fcl -{ - -/// @brief A simple hash table implemented as multiple buckets. HashFnc is any extended hash function: HashFnc(key) = {index1, index2, ..., } -template -class SimpleHashTable -{ -protected: - typedef std::list Bin; - - std::vector table_; - - HashFnc h_; - - size_t table_size_; - -public: - SimpleHashTable(const HashFnc& h) : h_(h) - { - } - - /// @brief Init the number of bins in the hash table - void init(size_t size) - { - if(size == 0) - { - throw std::logic_error("SimpleHashTable must have non-zero size."); - } - - table_.resize(size); - table_size_ = size; - } - - //// @brief Insert a key-value pair into the table - void insert(Key key, Data value) - { - std::vector 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 Find the elements in the hash table whose key is the same as query key. - std::vector query(Key key) const - { - size_t range = table_.size(); - std::vector indices = h_(key); - std::set result; - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i] % range; - std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end())); - } - - return std::vector(result.begin(), result.end()); - } - - /// @brief remove the key-value pair from the table - void remove(Key key, Data value) - { - size_t range = table_.size(); - std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i] % range; - table_[index].remove(value); - } - } - - /// @brief clear the hash table - void clear() - { - table_.clear(); - table_.resize(table_size_); - } -}; - - -template -class unordered_map_hash_table : public std::unordered_map {}; - -/// @brief A hash table implemented using unordered_map -template class TableT = unordered_map_hash_table> -class SparseHashTable -{ -protected: - HashFnc h_; - typedef std::list Bin; - typedef TableT Table; - - Table table_; -public: - SparseHashTable(const HashFnc& h) : h_(h) {} - - /// @brief Init the hash table. The bucket size is dynamically decided. - void init(size_t) { table_.clear(); } - - /// @brief insert one key-value pair into the hash table - void insert(Key key, Data value) - { - std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - table_[indices[i]].push_back(value); - } - - /// @brief find the elements whose key is the same as the query - std::vector query(Key key) const - { - std::vector indices = h_(key); - std::set result; - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i]; - typename Table::const_iterator p = table_.find(index); - if(p != table_.end()) - std::copy((*p).second.begin(), (*p).second.end(), std::inserter(result, result.end())); - } - - return std::vector(result.begin(), result.end()); - } - - /// @brief remove one key-value pair from the hash table - void remove(Key key, Data value) - { - std::vector indices = h_(key); - for(size_t i = 0; i < indices.size(); ++i) - { - unsigned int index = indices[i]; - table_[index].remove(value); - } - } - - /// @brief clear the hash table - void clear() - { - table_.clear(); - } -}; - - -} - -#endif diff --git a/include/fcl/broadphase/simple_hash_table.h b/include/fcl/broadphase/simple_hash_table.h new file mode 100644 index 000000000..584df9b81 --- /dev/null +++ b/include/fcl/broadphase/simple_hash_table.h @@ -0,0 +1,160 @@ +/* + * 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 FCL_BROADPHASE_SIMPLEHASHTABLE_H +#define FCL_BROADPHASE_SIMPLEHASHTABLE_H + +#include +#include +#include +#include + +namespace fcl +{ + +/// @brief A simple hash table implemented as multiple buckets. HashFnc is any +/// extended hash function: HashFnc(key) = {index1, index2, ..., } +template +class SimpleHashTable +{ +protected: + typedef std::list Bin; + + std::vector 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 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(); +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SimpleHashTable::SimpleHashTable(const HashFnc& h) + : h_(h) +{ + // Do nothing +} + +//============================================================================== +template +void SimpleHashTable::init(size_t size) +{ + if(size == 0) + { + throw std::logic_error("SimpleHashTable must have non-zero size."); + } + + table_.resize(size); + table_size_ = size; +} + +//============================================================================== +template +void SimpleHashTable::insert(Key key, Data value) +{ + std::vector indices = h_(key); + size_t range = table_.size(); + for(size_t i = 0; i < indices.size(); ++i) + table_[indices[i] % range].push_back(value); +} + +//============================================================================== +template +std::vector SimpleHashTable::query(Key key) const +{ + size_t range = table_.size(); + std::vector indices = h_(key); + std::set result; + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i] % range; + std::copy(table_[index].begin(), table_[index].end(), + std::inserter(result, result.end())); + } + + return std::vector(result.begin(), result.end()); +} + +//============================================================================== +template +void SimpleHashTable::remove(Key key, Data value) +{ + size_t range = table_.size(); + std::vector indices = h_(key); + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i] % range; + table_[index].remove(value); + } +} + +//============================================================================== +template +void SimpleHashTable::clear() +{ + table_.clear(); + table_.resize(table_size_); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/broadphase/sparse_hash_table.h b/include/fcl/broadphase/sparse_hash_table.h new file mode 100644 index 000000000..7a21e3198 --- /dev/null +++ b/include/fcl/broadphase/sparse_hash_table.h @@ -0,0 +1,160 @@ +/* + * 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 FCL_BROADPHASE_SPARSEHASHTABLE_H +#define FCL_BROADPHASE_SPARSEHASHTABLE_H + +#include +#include +#include +#include +#include + +namespace fcl +{ + +template +class unordered_map_hash_table : public std::unordered_map {}; + +/// @brief A hash table implemented using unordered_map +template class TableT = unordered_map_hash_table> +class SparseHashTable +{ +protected: + HashFnc h_; + typedef std::list Bin; + typedef TableT Table; + + Table table_; +public: + SparseHashTable(const HashFnc& h); + + /// @brief Init the hash table. The bucket size is dynamically decided. + void init(size_t); + + /// @brief insert one key-value pair into the hash table + void insert(Key key, Data value); + + /// @brief find the elements whose key is the same as the query + std::vector query(Key key) const; + + /// @brief remove one key-value pair from the hash table + void remove(Key key, Data value); + + /// @brief clear the hash table + void clear(); +}; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template class TableT> +SparseHashTable::SparseHashTable(const HashFnc& h) + : h_(h) +{ + // Do nothing +} + +//============================================================================== +template class TableT> +void SparseHashTable::init(size_t) +{ + table_.clear(); +} + +//============================================================================== +template class TableT> +void SparseHashTable::insert(Key key, Data value) +{ + std::vector indices = h_(key); + for(size_t i = 0; i < indices.size(); ++i) + table_[indices[i]].push_back(value); +} + +//============================================================================== +template class TableT> +std::vector SparseHashTable::query(Key key) const +{ + std::vector indices = h_(key); + std::set result; + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i]; + typename Table::const_iterator p = table_.find(index); + if(p != table_.end()) + { + std::copy((*p).second.begin(), (*p).second.end(), std + ::inserter(result, result.end())); + } + } + + return std::vector(result.begin(), result.end()); +} + +//============================================================================== +template class TableT> +void SparseHashTable::remove(Key key, Data value) +{ + std::vector indices = h_(key); + for(size_t i = 0; i < indices.size(); ++i) + { + unsigned int index = indices[i]; + table_[index].remove(value); + } +} + +//============================================================================== +template class TableT> +void SparseHashTable::clear() +{ + table_.clear(); +} + +} // namespace fcl + +#endif diff --git a/include/fcl/broadphase/spatial_hash.h b/include/fcl/broadphase/spatial_hash.h new file mode 100644 index 000000000..2a766f4f0 --- /dev/null +++ b/include/fcl/broadphase/spatial_hash.h @@ -0,0 +1,112 @@ +/* + * 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 FCL_BROADPHASE_SPATIALHASH_H +#define FCL_BROADPHASE_SPATIALHASH_H + +#include "fcl/BV/AABB.h" + +namespace fcl +{ + +/// @brief Spatial hash function: hash an AABB to a set of integer values +template +struct SpatialHash +{ + using S = S_; + + SpatialHash(const AABB& scene_limit_, S cell_size_); + + std::vector operator() (const AABB& aabb) const; + +private: + + S cell_size; + AABB scene_limit; + unsigned int width[3]; +}; + +using SpatialHashf = SpatialHash; +using SpatialHashd = SpatialHash; + +//============================================================================// +// // +// Implementations // +// // +//============================================================================// + +//============================================================================== +template +SpatialHash::SpatialHash( + const AABB& scene_limit_, SpatialHash::S 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); +} + +//============================================================================== +template +std::vector SpatialHash::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 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; +} + +} // namespace fcl + +#endif diff --git a/test/test_fcl_broadphase_collision_1.cpp b/test/test_fcl_broadphase_collision_1.cpp index d8171dcb8..eaa30fe3b 100644 --- a/test/test_fcl_broadphase_collision_1.cpp +++ b/test/test_fcl_broadphase_collision_1.cpp @@ -39,6 +39,8 @@ #include "fcl/config.h" #include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/sparse_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" #include "fcl/shape/geometric_shape_to_BVH_model.h" #include "test_fcl_utility.h" diff --git a/test/test_fcl_broadphase_collision_2.cpp b/test/test_fcl_broadphase_collision_2.cpp index a4430f54f..b8dcb58ec 100644 --- a/test/test_fcl_broadphase_collision_2.cpp +++ b/test/test_fcl_broadphase_collision_2.cpp @@ -39,6 +39,8 @@ #include "fcl/config.h" #include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/sparse_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" #include "fcl/shape/geometric_shape_to_BVH_model.h" #include "test_fcl_utility.h" diff --git a/test/test_fcl_broadphase_distance.cpp b/test/test_fcl_broadphase_distance.cpp index ccd0e6998..a7e46832d 100644 --- a/test/test_fcl_broadphase_distance.cpp +++ b/test/test_fcl_broadphase_distance.cpp @@ -39,6 +39,8 @@ #include "fcl/config.h" #include "fcl/broadphase/broadphase.h" +#include "fcl/broadphase/sparse_hash_table.h" +#include "fcl/broadphase/spatial_hash.h" #include "fcl/shape/geometric_shape_to_BVH_model.h" #include "test_fcl_utility.h"