From 645a27dc56a9ee740be6611673431d6c67468051 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 1 Jun 2012 07:11:45 +0000
Subject: [PATCH] git-svn-id: https://kforge.ros.org/fcl/fcl_ros@88
253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 2 +-
trunk/fcl/include/fcl/AABB.h | 6 +
trunk/fcl/include/fcl/broad_phase_collision.h | 280 ++++++++++++++++++
trunk/fcl/include/fcl/collision_func_matrix.h | 2 +-
trunk/fcl/include/fcl/distance.h | 51 ++++
trunk/fcl/include/fcl/distance_func_matrix.h | 56 ++++
trunk/fcl/include/fcl/hash.h | 228 ++++++++++++++
trunk/fcl/src/broad_phase_collision.cpp | 14 +-
trunk/fcl/src/collision.cpp | 109 +------
trunk/fcl/src/distance.cpp | 82 +++++
trunk/fcl/src/distance_func_matrix.cpp | 117 ++++++++
11 files changed, 843 insertions(+), 104 deletions(-)
create mode 100644 trunk/fcl/include/fcl/distance.h
create mode 100644 trunk/fcl/include/fcl/distance_func_matrix.h
create mode 100644 trunk/fcl/include/fcl/hash.h
create mode 100644 trunk/fcl/src/distance.cpp
create mode 100644 trunk/fcl/src/distance_func_matrix.cpp
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index f8628eb8..5ea53d32 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/AABB.cpp src/OBB.cpp src/RSS.cpp src/kIOS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/simd_intersect.cpp)
+add_library(${PROJECT_NAME} SHARED src/AABB.cpp src/OBB.cpp src/RSS.cpp src/kIOS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/simd_intersect.cpp src/distance_func_matrix.cpp src/distance.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/AABB.h b/trunk/fcl/include/fcl/AABB.h
index 16574e25..620bb79b 100644
--- a/trunk/fcl/include/fcl/AABB.h
+++ b/trunk/fcl/include/fcl/AABB.h
@@ -81,8 +81,14 @@ public:
if(max_[2] < other.min_[2]) return false;
return true;
+ }
+
+ inline bool contain(const AABB& other) const
+ {
+ return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
}
+
/** \brief Check whether two AABB are overlapped along specific axis */
inline bool axisOverlap(const AABB& other, int axis_id) const
{
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index b99359cb..de15ffc1 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -43,8 +43,10 @@
#include "fcl/collision_data.h"
#include "fcl/AABB.h"
#include "fcl/interval_tree.h"
+#include "fcl/hash.h"
#include <vector>
#include <list>
+#include <iostream>
namespace fcl
{
@@ -53,6 +55,8 @@ namespace fcl
bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
+BVH_REAL defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
+
/** \brief return value is whether can stop now */
typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
@@ -60,6 +64,8 @@ typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void
class BroadPhaseCollisionManager
{
public:
+ virtual ~BroadPhaseCollisionManager() {}
+
/** \brief add one object to the manager */
virtual void registerObject(CollisionObject* obj) = 0;
@@ -91,6 +97,7 @@ public:
virtual size_t size() const = 0;
};
+
/** \brief Brute force N-body collision manager */
class NaiveCollisionManager : public BroadPhaseCollisionManager
{
@@ -133,6 +140,279 @@ protected:
std::list<CollisionObject*> objs;
};
+
+struct SpatialHash
+{
+ SpatialHash(const AABB& scene_limit_, BVH_REAL cell_size_)
+ {
+ cell_size = cell_size_;
+ scene_limit = scene_limit_;
+ width[0] = ceil(scene_limit.width() / cell_size);
+ width[1] = ceil(scene_limit.height() / cell_size);
+ width[2] = ceil(scene_limit.depth() / cell_size);
+ }
+
+ std::vector<unsigned int> operator() (const AABB& aabb) const
+ {
+ int min_x = floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size);
+ int max_x = ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size);
+ int min_y = floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size);
+ int max_y = ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size);
+ int min_z = floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size);
+ int max_z = 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:
+
+ BVH_REAL cell_size;
+ AABB scene_limit;
+ unsigned int width[3];
+};
+
+/** \brief spatial hashing collision mananger */
+template<typename HashTable = SimpleHashTable<AABB, CollisionObject*, SpatialHash> >
+class SpatialHashingCollisionManager : public BroadPhaseCollisionManager
+{
+public:
+ SpatialHashingCollisionManager(BVH_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000)
+ {
+ scene_limit = AABB(scene_min, scene_max);
+ SpatialHash hasher(scene_limit, cell_size);
+ hash_table = new HashTable(hasher);
+ hash_table->init(default_table_size);
+ }
+
+ ~SpatialHashingCollisionManager()
+ {
+ delete hash_table;
+ }
+
+ void registerObject(CollisionObject* obj);
+
+ void unregisterObject(CollisionObject* obj);
+
+ void setup();
+
+ void update();
+
+ void clear();
+
+ void getObjects(std::vector<CollisionObject*>& objs) const;
+
+ void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const;
+
+ void collide(void* cdata, CollisionCallBack callback) const;
+
+ bool empty() const;
+
+ size_t size() const;
+
+ 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:
+
+ // all objects in the scene
+ std::list<CollisionObject*> objs;
+
+ // objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table
+ HashTable* hash_table;
+ // objects outside the scene limit are in another list
+ std::list<CollisionObject*> objs_outside_scene_limit;
+
+ AABB scene_limit;
+};
+
+
+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_outside_scene_limit.push_back(obj);
+
+ hash_table->insert(overlap_aabb, obj);
+ }
+ else
+ {
+ objs_outside_scene_limit.push_back(obj);
+ }
+}
+
+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_outside_scene_limit.remove(obj);
+
+ hash_table->remove(overlap_aabb, obj);
+ }
+ else
+ {
+ objs_outside_scene_limit.remove(obj);
+ }
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::setup()
+{}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::update()
+{
+ hash_table->clear();
+ objs_outside_scene_limit.clear();
+
+ std::list<CollisionObject*>::const_iterator it;
+ for(it = objs.begin(); it != objs.end(); ++it)
+ {
+ registerObject(*it);
+ }
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::clear()
+{
+ objs.clear();
+ hash_table->clear();
+ objs_outside_scene_limit.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, void* cdata, CollisionCallBack callback) const
+{
+ const AABB& obj_aabb = obj->getAABB();
+ AABB overlap_aabb;
+
+ if(scene_limit.overlap(obj_aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(obj_aabb))
+ {
+ std::list<CollisionObject*>::const_iterator it;
+ for(it = objs_outside_scene_limit.begin(); it != objs_outside_scene_limit.end(); ++it)
+ {
+ if(callback(obj, *it, cdata)) return;
+ }
+ }
+
+ std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
+ for(unsigned int i = 0; i < query_result.size(); ++i)
+ {
+ if(callback(obj, query_result[i], cdata)) return;
+ }
+ }
+ else
+ {
+ std::list<CollisionObject*>::const_iterator it;
+ for(it = objs_outside_scene_limit.begin(); it != objs_outside_scene_limit.end(); ++it)
+ {
+ if(callback(obj, *it, cdata)) return;
+ }
+ }
+}
+
+template<typename HashTable>
+void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCallBack callback) const
+{
+ std::list<CollisionObject*>::const_iterator it1;
+ for(it1 = objs.begin(); it1 != objs.end(); ++it1)
+ {
+ const AABB& obj_aabb = (*it1)->getAABB();
+ AABB overlap_aabb;
+
+ if(scene_limit.overlap(obj_aabb, overlap_aabb))
+ {
+ if(!scene_limit.contain(obj_aabb))
+ {
+ std::list<CollisionObject*>::const_iterator it2;
+ for(it2 = objs_outside_scene_limit.begin(); it2 != objs_outside_scene_limit.end(); ++it2)
+ {
+ if(*it1 < *it2)
+ {
+ if(callback(*it1, *it2, cdata)) return;
+ }
+ }
+ }
+
+ std::vector<CollisionObject*> query_result = hash_table->query(overlap_aabb);
+ for(unsigned int i = 0; i < query_result.size(); ++i)
+ {
+ if(*it1 < query_result[i])
+ {
+ if(callback(*it1, query_result[i], cdata)) return;
+ }
+ }
+ }
+ else
+ {
+ std::list<CollisionObject*>::const_iterator it2;
+ for(it2 = objs_outside_scene_limit.begin(); it2 != objs_outside_scene_limit.end(); ++it2)
+ {
+ if(*it1 < *it2)
+ {
+ if(callback(*it1, *it2, cdata)) return;
+ }
+ }
+ }
+ }
+}
+
+template<typename HashTable>
+bool SpatialHashingCollisionManager<HashTable>::empty() const
+{
+ return objs.empty();
+}
+
+template<typename HashTable>
+size_t SpatialHashingCollisionManager<HashTable>::size() const
+{
+ return objs.size();
+}
+
+
/** Rigorous SAP collision manager */
class SaPCollisionManager : public BroadPhaseCollisionManager
{
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index 87cf1727..f675ffe0 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -51,7 +51,7 @@ typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform&
struct CollisionFunctionMatrix
{
- CollisionFunc collision_matrix[15][15];
+ CollisionFunc collision_matrix[16][16];
CollisionFunctionMatrix();
};
diff --git a/trunk/fcl/include/fcl/distance.h b/trunk/fcl/include/fcl/distance.h
new file mode 100644
index 00000000..f13fc532
--- /dev/null
+++ b/trunk/fcl/include/fcl/distance.h
@@ -0,0 +1,51 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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_DISTANCE_H
+#define FCL_DISTANCE_H
+
+#include "fcl/collision_object.h"
+
+namespace fcl
+{
+BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2);
+
+BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
+ const CollisionGeometry* o2, const SimpleTransform& tf2);
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
new file mode 100644
index 00000000..75c899f6
--- /dev/null
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -0,0 +1,56 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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_DISTANCE_FUNC_MATRIX_H
+#define FCL_DISTANCE_FUNC_MATRIX_H
+
+#include "fcl/collision_object.h"
+#include "fcl/collision_data.h"
+
+namespace fcl
+{
+typedef BVH_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2);
+
+struct DistanceFunctionMatrix
+{
+ DistanceFunc distance_matrix[16][16];
+
+ DistanceFunctionMatrix();
+};
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/hash.h b/trunk/fcl/include/fcl/hash.h
new file mode 100644
index 00000000..b82c91db
--- /dev/null
+++ b/trunk/fcl/include/fcl/hash.h
@@ -0,0 +1,228 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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
+
+#if USE_GOOGLEHASH
+#include <sparsehash/sparse_hash_map>
+#include <sparsehash/dense_hash_map>
+#endif
+#include <stdexcept>
+#include <hash_map>
+#include <set>
+#include <vector>
+#include <list>
+
+namespace fcl
+{
+
+// 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_;
+public:
+ SimpleHashTable(const HashFnc& h) : h_(h)
+ {
+ }
+
+ void init(size_t size)
+ {
+ if(size == 0)
+ {
+ throw std::logic_error("SimpleHashTable must have non-zero size.");
+ }
+
+ table_.resize(size);
+ }
+
+ void 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);
+ }
+
+ std::vector<Data> 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)
+ {
+ unsigned int index = indices[i] % range;
+ std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end()));
+ }
+
+ return std::vector<Data>(result.begin(), result.end());
+ }
+
+ void 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)
+ {
+ unsigned int index = indices[i] % range;
+ table_[index].remove(value);
+ }
+ }
+
+ void clear()
+ {
+ table_.clear();
+ }
+};
+
+#if USE_GOOGLEHASH
+
+template<typename Key, typename Data, typename HashFnc>
+class SparseHashTable
+{
+protected:
+ HashFnc h_;
+ typedef std::list<Data> Bin;
+ typedef google::sparse_hash_map<size_t, Bin, std::tr1::hash<size_t>, std::equal_to<size_t> > Table;
+
+ Table table_;
+public:
+ SparseHashTable(const HashFnc& h) : h_(h) {}
+
+ void init(size_t) { table_.clear(); }
+
+ void insert(Key key, Data value)
+ {
+ std::vector<unsigned int> indices = h_(key);
+ for(size_t i = 0; i < indices.size(); ++i)
+ table_[indices[i]].push_back(value);
+ }
+
+ std::vector<Data> query(Key key) const
+ {
+ std::vector<unsigned int> indices = h_(key);
+ std::set<Data> 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<Data>(result.begin(), result.end());
+ }
+
+ void remove(Key key, Data value)
+ {
+ std::vector<unsigned int> indices = h_(key);
+ for(size_t i = 0; i < indices.size(); ++i)
+ {
+ unsigned int index = indices[i];
+ table_[index].remove(value);
+ }
+ }
+
+ void clear()
+ {
+ table_.clear();
+ }
+};
+
+
+template<typename Key, typename Data, typename HashFnc>
+class DenseHashTable
+{
+protected:
+ HashFnc h_;
+ typedef std::list<Data> Bin;
+ typedef google::dense_hash_map<size_t, Bin, std::tr1::hash<size_t>, std::equal_to<size_t> > Table;
+
+ Table table_;
+public:
+
+ DenseHashTable(const HashFnc& h) : h_(h)
+ { table_.set_empty_key(NULL); }
+
+ void init(size_t) { table_.clear(); }
+
+ void insert(Key key, Data value)
+ {
+ std::vector<unsigned int> indices = h_(key);
+ for(size_t i = 0; i < indices.size(); ++i)
+ table_[indices[i]].push_back(value);
+ }
+
+ std::vector<Data> query(Key key) const
+ {
+ std::vector<unsigned int> indices = h_(key);
+ std::set<Data> 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<Data>(result.begin(), result.end());
+ }
+
+ void remove(Key key, Data value)
+ {
+ std::vector<unsigned int> indices = h_(key);
+ for(size_t i = 0; i < indices.size(); ++i)
+ {
+ unsigned int index = indices[i];
+ table_[index].remove(value);
+ }
+ }
+
+ void clear()
+ {
+ table_.clear();
+ }
+};
+
+#endif
+
+}
+
+#endif
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index d259f0a2..bd7b9dd3 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -69,6 +69,11 @@ bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cd
return cdata->done;
}
+BVH_REAL defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
+{
+ return 0.0;
+}
+
void NaiveCollisionManager::unregisterObject(CollisionObject* obj)
{
objs.remove(obj);
@@ -97,10 +102,7 @@ void NaiveCollisionManager::clear()
void NaiveCollisionManager::getObjects(std::vector<CollisionObject*>& objs_) const
{
objs_.resize(objs.size());
- std::list<CollisionObject*>::const_iterator it;
- int i = 0;
- for(it = objs.begin(); it != objs.end(); ++it, ++i)
- objs_[i] = *it;
+ std::copy(objs.begin(), objs.end(), objs_.begin());
}
void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
@@ -134,7 +136,6 @@ bool NaiveCollisionManager::empty() const
}
-
void SSaPCollisionManager::unregisterObject(CollisionObject* obj)
{
setup();
@@ -218,8 +219,7 @@ void SSaPCollisionManager::clear()
void SSaPCollisionManager::getObjects(std::vector<CollisionObject*>& objs) const
{
objs.resize(objs_x.size());
- for(unsigned int i = 0; i < objs.size(); ++i)
- objs[i] = objs_x[i];
+ std::copy(objs_x.begin(), objs_x.end(), objs.begin());
}
void SSaPCollisionManager::checkColl(std::vector<CollisionObject*>::const_iterator pos_start, std::vector<CollisionObject*>::const_iterator pos_end,
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index 6f5d8d0c..fc2d2542 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -45,71 +45,16 @@ namespace fcl
static CollisionFunctionMatrix CollisionFunctionLookTable;
+
+
int collide(const CollisionObject* o1, const CollisionObject* o2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts)
{
- if(num_max_contacts <= 0 && !exhaustive)
- {
- std::cerr << "Warning: should stop early as num_max_contact is " << num_max_contacts << " !" << std::endl;
- return 0;
- }
-
- const OBJECT_TYPE object_type1 = o1->getObjectType();
- const NODE_TYPE node_type1 = o1->getNodeType();
-
- const OBJECT_TYPE object_type2 = o2->getObjectType();
- const NODE_TYPE node_type2 = o2->getNodeType();
-
-
- if(object_type1 == OT_GEOM && object_type2 == OT_GEOM)
- {
- if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
- }
-
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
- }
- else if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
- {
- if(!CollisionFunctionLookTable.collision_matrix[node_type2][node_type1])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
- }
-
- return CollisionFunctionLookTable.collision_matrix[node_type2][node_type1](o2->getCollisionGeometry(), o2->getTransform(), o1->getCollisionGeometry(), o1->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
- }
- else if(object_type1 == OT_BVH && object_type2 == OT_GEOM)
- {
- if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
- }
-
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
- }
- else if(object_type1 == OT_BVH && object_type2 == OT_BVH)
- {
- if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
- }
-
- if(node_type1 == node_type2)
- {
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), num_max_contacts, exhaustive, enable_contact, contacts);
- }
- }
-
- return 0;
+ return collide(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(),
+ num_max_contacts, exhaustive, enable_contact, contacts);
}
-
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
int num_max_contacts, bool exhaustive, bool enable_contact,
@@ -121,58 +66,32 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
return 0;
}
- const OBJECT_TYPE object_type1 = o1->getObjectType();
- const NODE_TYPE node_type1 = o1->getNodeType();
-
- const OBJECT_TYPE object_type2 = o2->getObjectType();
- const NODE_TYPE node_type2 = o2->getNodeType();
-
-
- if(object_type1 == OT_GEOM && object_type2 == OT_GEOM)
- {
- if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
- }
-
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
- }
- else if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
- {
+ OBJECT_TYPE object_type1 = o1->getObjectType();
+ OBJECT_TYPE object_type2 = o2->getObjectType();
+ NODE_TYPE node_type1 = o1->getNodeType();
+ NODE_TYPE node_type2 = o2->getNodeType();
+
+ if(object_type1 == OT_GEOM & object_type2 == OT_BVH)
+ {
if(!CollisionFunctionLookTable.collision_matrix[node_type2][node_type1])
{
std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
return 0;
}
-
+
return CollisionFunctionLookTable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, num_max_contacts, exhaustive, enable_contact, contacts);
}
- else if(object_type1 == OT_BVH && object_type2 == OT_GEOM)
+ else
{
if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
{
std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
return 0;
}
-
+
return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
}
- else if(object_type1 == OT_BVH && object_type2 == OT_BVH)
- {
- if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
- }
-
- if(node_type1 == node_type2)
- {
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
- }
- }
- return 0;
}
diff --git a/trunk/fcl/src/distance.cpp b/trunk/fcl/src/distance.cpp
new file mode 100644
index 00000000..2d85f285
--- /dev/null
+++ b/trunk/fcl/src/distance.cpp
@@ -0,0 +1,82 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 */
+
+#include "fcl/distance.h"
+#include "fcl/distance_func_matrix.h"
+
+#include <iostream>
+
+namespace fcl
+{
+
+static DistanceFunctionMatrix DistanceFunctionLookTable;
+
+BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2)
+{
+ return distance(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform());
+}
+
+BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
+ const CollisionGeometry* o2, const SimpleTransform& tf2)
+{
+ OBJECT_TYPE object_type1 = o1->getObjectType();
+ NODE_TYPE node_type1 = o1->getNodeType();
+ OBJECT_TYPE object_type2 = o2->getObjectType();
+ NODE_TYPE node_type2 = o2->getNodeType();
+
+ if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
+ {
+ if(!DistanceFunctionLookTable.distance_matrix[node_type2][node_type1])
+ {
+ std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
+ return 0;
+ }
+
+ return DistanceFunctionLookTable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1);
+ }
+ else
+ {
+ if(!DistanceFunctionLookTable.distance_matrix[node_type1][node_type2])
+ {
+ std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
+ return 0;
+ }
+
+ return DistanceFunctionLookTable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2);
+ }
+}
+
+}
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
new file mode 100644
index 00000000..9264c8fc
--- /dev/null
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -0,0 +1,117 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 */
+
+#include "fcl/distance_func_matrix.h"
+
+#include "fcl/collision_node.h"
+#include "fcl/simple_setup.h"
+
+namespace fcl
+{
+
+template<typename T_BVH>
+BVH_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+{
+ MeshDistanceTraversalNode<T_BVH> node;
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
+ const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
+ BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
+ SimpleTransform tf1_tmp = tf1;
+ BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
+ SimpleTransform tf2_tmp = tf2;
+
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+template<>
+BVH_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+{
+ MeshDistanceTraversalNodeRSS node;
+ const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
+ const BVHModel<RSS>* obj2 = static_cast<const BVHModel<RSS>* >(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+template<>
+BVH_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+{
+ MeshDistanceTraversalNodekIOS node;
+ const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
+ const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+
+template<>
+BVH_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
+{
+ MeshDistanceTraversalNodeOBBRSS node;
+ const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
+ const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+DistanceFunctionMatrix::DistanceFunctionMatrix()
+{
+ for(int i = 0; i < 16; ++i)
+ {
+ for(int j = 0; j < 16; ++j)
+ distance_matrix[i][j] = NULL;
+ }
+
+ distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB>;
+ distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS>;
+ distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS>;
+ distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS>;
+}
+
+
+}
--
GitLab