add kIOS which can handle collision and distance simultaneously. and refactor...

add kIOS which can handle collision and distance simultaneously. and refactor the collision_func_matrix.cpp


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@82 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/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)
+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)
 
 target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
 

trunk/fcl/include/fcl/BVH_model.h

@@ -284,6 +284,9 @@ NODE_TYPE BVHModel<OBB>::getNodeType() const;
 template<>
 NODE_TYPE BVHModel<RSS>::getNodeType() const;
 
+template<>
+NODE_TYPE BVHModel<kIOS>::getNodeType() const;
+
 template<>
 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
 

trunk/fcl/include/fcl/BVH_utility.h

@@ -111,6 +111,15 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec
  */
 void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
 
+/** \brief Compute the center and radius for a triangle's circumcircle */
+void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius);
+
+/** \brief Compute the maximum distance from a given center point to a point cloud */
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query);
+
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
+
+
 }
 
 #endif

trunk/fcl/include/fcl/BV_fitter.h

@@ -43,6 +43,7 @@
 #include "fcl/vec_3f.h"
 #include "fcl/OBB.h"
 #include "fcl/RSS.h"
+#include "fcl/kIOS.h"
 #include <iostream>
 
 /** \brief Main namespace */
@@ -65,6 +66,9 @@ void fit<OBB>(Vec3f* ps, int n, OBB& bv);
 template<>
 void fit<RSS>(Vec3f* ps, int n, RSS& bv);
 
+template<>
+void fit<kIOS>(Vec3f* ps, int n, kIOS& bv);
+
 
 template<typename BV>
 class BVFitterBase
@@ -254,6 +258,52 @@ private:
   BVHModelType type;
 };
 
+
+/** \brief Specification of BVFitter for kIOS bounding volume */
+template<>
+class BVFitter<kIOS> : public BVFitterBase<kIOS>
+{
+public:
+  /** \brief Prepare the geometry primitive data for fitting */
+  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+  {
+    vertices = vertices_;
+    prev_vertices = NULL;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /** \brief Prepare the geometry primitive data for fitting */
+  void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
+  {
+    vertices = vertices_;
+    prev_vertices = prev_vertices_;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
+   * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
+   */
+  kIOS fit(unsigned int* primitive_indices, int num_primitives);
+
+  /** \brief Clear the geometry primitive data */
+  void clear()
+  {
+    vertices = NULL;
+    prev_vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+private:
+  Vec3f* vertices;
+  Vec3f* prev_vertices;
+  Triangle* tri_indices;
+  BVHModelType type;
+};
+
+
 }
 
 #endif

trunk/fcl/include/fcl/BV_splitter.h

@@ -42,6 +42,8 @@
 #include "fcl/primitive.h"
 #include "fcl/vec_3f.h"
 #include "fcl/OBB.h"
+#include "fcl/RSS.h"
+#include "fcl/kIOS.h"
 #include <vector>
 #include <iostream>
 
@@ -302,6 +304,153 @@ private:
   SplitMethodType split_method;
 };
 
+
+/** \brief BVHSplitRule specialization for RSS */
+template<>
+class BVSplitter<RSS> : public BVSplitterBase<RSS>
+{
+public:
+
+  BVSplitter(SplitMethodType method)
+  {
+    split_method = method;
+  }
+
+  /** \brief Set the geometry data needed by the split rule */
+  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+  {
+    vertices = vertices_;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
+  void computeRule(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
+  {
+    switch(split_method)
+    {
+      case SPLIT_METHOD_MEAN:
+        computeRule_mean(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_MEDIAN:
+        computeRule_median(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_BV_CENTER:
+        computeRule_bvcenter(bv, primitive_indices, num_primitives);
+        break;
+      default:
+        std::cerr << "Split method not supported" << std::endl;
+    }
+  }
+
+  /** \brief Apply the split rule on a given point */
+  bool apply(const Vec3f& q) const
+  {
+    return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+  }
+
+  /** \brief Clear the geometry data set before */
+  void clear()
+  {
+    vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+private:
+
+  /** \brief Split the node from center */
+  void computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+  /** \brief Split the node according to the mean of the data contained */
+  void computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+  /** \brief Split the node according to the median of the data contained */
+  void computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+  BVH_REAL split_value;
+  Vec3f split_vector;
+
+  Vec3f* vertices;
+  Triangle* tri_indices;
+  BVHModelType type;
+  SplitMethodType split_method;
+};
+
+
+/** \brief BVHSplitRule specialization for RSS */
+template<>
+class BVSplitter<kIOS> : public BVSplitterBase<kIOS>
+{
+public:
+
+  BVSplitter(SplitMethodType method)
+  {
+    split_method = method;
+  }
+
+  /** \brief Set the geometry data needed by the split rule */
+  void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+  {
+    vertices = vertices_;
+    tri_indices = tri_indices_;
+    type = type_;
+  }
+
+  /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
+  void computeRule(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
+  {
+    switch(split_method)
+    {
+      case SPLIT_METHOD_MEAN:
+        computeRule_mean(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_MEDIAN:
+        computeRule_median(bv, primitive_indices, num_primitives);
+        break;
+      case SPLIT_METHOD_BV_CENTER:
+        computeRule_bvcenter(bv, primitive_indices, num_primitives);
+        break;
+      default:
+        std::cerr << "Split method not supported" << std::endl;
+    }
+  }
+
+  /** \brief Apply the split rule on a given point */
+  bool apply(const Vec3f& q) const
+  {
+    return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+  }
+
+  /** \brief Clear the geometry data set before */
+  void clear()
+  {
+    vertices = NULL;
+    tri_indices = NULL;
+    type = BVH_MODEL_UNKNOWN;
+  }
+
+private:
+
+  /** \brief Split the node from center */
+  void computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+  /** \brief Split the node according to the mean of the data contained */
+  void computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+  /** \brief Split the node according to the median of the data contained */
+  void computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+  BVH_REAL split_value;
+  Vec3f split_vector;
+
+  Vec3f* vertices;
+  Triangle* tri_indices;
+  BVHModelType type;
+  SplitMethodType split_method;
+};
+
+
 }
 
 #endif

trunk/fcl/include/fcl/OBB.h

@@ -118,7 +118,7 @@ public:
   }
 
   /** \brief Center of the OBB */
-  inline Vec3f center() const
+  inline const Vec3f& center() const
   {
     return To;
   }

trunk/fcl/include/fcl/RSS.h

@@ -121,7 +121,7 @@ public:
   }
 
   /** \brief The RSS center */
-  inline Vec3f center() const
+  inline const Vec3f& center() const
   {
     return Tr;
   }

trunk/fcl/include/fcl/collision_node.h

@@ -57,6 +57,8 @@ void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL,
 
 void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list = NULL, int qsize = 2);
 
+void distance(MeshDistanceTraversalNodekIOS* node, BVHFrontList* front_list = NULL, int qsize = 2);
+
 }
 
 #endif

trunk/fcl/include/fcl/collision_object.h

@@ -47,7 +47,7 @@ namespace fcl
 
 enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
 
-enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
+enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KIOS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
                 GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
 
 class CollisionGeometry

trunk/fcl/include/fcl/kIOS.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_KIOS_H
+#define FCL_KIOS_H
+
+#include "fcl/BVH_internal.h"
+#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
+
+/** \brief Main namespace */
+namespace fcl
+{
+ 
+/** \brief kIOS class */
+class kIOS
+{
+  struct kIOS_Sphere
+  {
+    Vec3f o;
+    BVH_REAL r;
+  };
+
+  static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
+  {
+    Vec3f d = s1.o - s0.o;
+    BVH_REAL dist2 = d.sqrLength();
+    BVH_REAL diff_r = s1.r - s0.r;
+      
+    /** The sphere with the larger radius encloses the other */
+    if(diff_r * diff_r >= dist2)
+    {
+      if(s1.r > s0.r) return s1;
+      else return s0;
+    }
+    else /** spheres partially overlapping or disjoint */
+    {
+      float dist = sqrt(dist2);
+      kIOS_Sphere s;
+      s.r = dist + s0.r + s1.r;
+      s.o = s0.o;
+      if(dist > 0)
+        s.o += d * ((s.r - s0.r) / dist);
+
+      return s;
+    }
+  }
+public:
+    
+  /** \brief The (at most) three spheres for intersection */
+  kIOS_Sphere spheres[5];
+
+  unsigned int num_spheres; // num_spheres <= 5
+
+  kIOS() {}
+
+  /** \brief Check collision between two kIOS */
+  bool overlap(const kIOS& other) const;
+
+  /** \brief Check collision between two kIOS and return the overlap part.
+   * For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
+   */
+  bool overlap(const kIOS& other, kIOS& overlap_part) const
+  {
+    return overlap(other);
+  }
+
+  /** \brief Check whether the kIOS contains a point */
+  inline bool contain(const Vec3f& p) const;
+
+  /** \brief A simple way to merge the kIOS and a point */
+  kIOS& operator += (const Vec3f& p);
+
+  /** \brief Merge the kIOS and another kIOS */
+  kIOS& operator += (const kIOS& other)
+  {
+    *this = *this + other;
+    return *this;
+  }
+
+  /** \brief Return the merged kIOS of current kIOS and the other one */
+  kIOS operator + (const kIOS& other) const;
+
+  /** \brief Center of the kIOS */
+  const Vec3f& center() const
+  {
+    return spheres[0].o;
+  }
+
+  /** \brief width of the kIOS */
+  BVH_REAL width() const;
+
+  /** \brief height of the kIOS */
+  BVH_REAL height() const;
+
+  /** \brief depth of the kIOS */
+  BVH_REAL depth() const;
+
+  /** \brief volume of the kIOS */
+  BVH_REAL volume() const;
+
+  /** \brief size of the kIOS, for split order */
+  BVH_REAL size() const;
+
+  /** \brief The distance between two kIOS */
+  BVH_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+
+private:    
+    
+};
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
+
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+
+}
+
+#endif

trunk/fcl/include/fcl/simple_setup.h

@@ -279,6 +279,10 @@ bool initialize(MeshCollisionTraversalNodeRSS& node,
                 const BVHModel<RSS>& model2, const SimpleTransform& tf2,
                 int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
 
+bool initialize(MeshCollisionTraversalNodekIOS& node,
+                const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
+                const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+                int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
 
 #if USE_SVMLIGHT
 
@@ -544,6 +548,11 @@ bool initialize(MeshDistanceTraversalNodeRSS& node,
                 const BVHModel<RSS>& model2, const SimpleTransform& tf2);
 
 
+bool initialize(MeshDistanceTraversalNodekIOS& node,
+                const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
+                const BVHModel<kIOS>& model2, const SimpleTransform& tf2);
+
+
 
 /** \brief Initialize traversal node for continuous collision detection between two meshes */
 template<typename BV>

trunk/fcl/include/fcl/traversal_node_bvhs.h

@@ -301,7 +301,18 @@ public:
   Vec3f T;
 };
 
+class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
+{
+public:
+  MeshCollisionTraversalNodekIOS();
+ 
+  bool BVTesting(int b1, int b2) const;
+
+  void leafTesting(int b1, int b2) const;
 
+  Matrix3f R;
+  Vec3f T;
+};
 
 #if USE_SVMLIGHT
 
@@ -1048,6 +1059,19 @@ public:
 };
 
 
+class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
+{
+public:
+  MeshDistanceTraversalNodekIOS();
+
+  BVH_REAL BVTesting(int b1, int b2) const;
+
+  void leafTesting(int b1, int b2) const;
+
+  Matrix3f R;
+  Vec3f T;
+};
+
 
 struct ConservativeAdvancementStackData
 {

trunk/fcl/src/BVH_model.cpp

@@ -891,6 +891,12 @@ NODE_TYPE BVHModel<RSS>::getNodeType() const
   return BV_RSS;
 }
 
+template<>
+NODE_TYPE BVHModel<kIOS>::getNodeType() const
+{
+  return BV_KIOS;
+}
+
 template<>
 NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const
 {
@@ -917,5 +923,5 @@ template class BVHModel<KDOP<24> >;
 template class BVHModel<OBB>;
 template class BVHModel<AABB>;
 template class BVHModel<RSS>;
-
+template class BVHModel<kIOS>;
 }

trunk/fcl/src/BVH_utility.cpp

@@ -1026,6 +1026,81 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indic
 
 }
 
+void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius)
+{
+  Vec3f e1 = a - c;
+  Vec3f e2 = b - c;
+  BVH_REAL e1_len2 = e1.sqrLength();
+  BVH_REAL e2_len2 = e2.sqrLength();
+  Vec3f e3 = e1.cross(e2);
+  BVH_REAL e3_len2 = e3.sqrLength();
+  radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2;
+  radius = sqrt(radius) / 2.0;
+
+  center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
+}
+
+
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
+{
+  bool indirect_index = true;
+  if(!indices) indirect_index = false;
+  
+  BVH_REAL maxD = 0;
+  for(int i = 0; i < n; ++i)
+  {
+    unsigned int index = indirect_index ? indices[i] : i;
+    const Triangle& t = ts[index];
+
+    for(int j = 0; j < 3; ++j)
+    {
+      int point_id = t[j];
+      const Vec3f& p = ps[point_id];
+      
+      BVH_REAL d = (p - query).sqrLength();
+      if(d > maxD) maxD = d;
+    }
+
+    if(ps2)
+    {
+      for(int j = 0; j < 3; ++j)
+      {
+        int point_id = t[j];
+        const Vec3f& p = ps2[point_id];
+        
+        BVH_REAL d = (p - query).sqrLength();
+        if(d > maxD) maxD = d;
+      }
+    }
+  }
+
+  return sqrt(maxD);
+}
+
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query)
+{
+  bool indirect_index = true;
+  if(!indices) indirect_index = false;
+
+  BVH_REAL maxD = 0;
+  for(unsigned int i = 0; i < n; ++i)
+  {
+    int index = indirect_index ? indices[i] : i;