git-svn-id: https://kforge.ros.org/fcl/fcl_ros@88 253336fb-580f-4252-a368-f3cef5a2a82b

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})
 

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
   {

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
 {

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();
 };

trunk/fcl/include/fcl/distance.h

+/*
+ * 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

trunk/fcl/include/fcl/distance_func_matrix.h

+/*
+ * 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

trunk/fcl/include/fcl/hash.h

+/*
+ * 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

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,

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;
 }
 
 

trunk/fcl/src/distance.cpp

+/*
+ * 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);    
+  }
+}
+
+}

trunk/fcl/src/distance_func_matrix.cpp

+/*
+ * 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>;
+}
+
+
+}