penetration depth

CMakeLists.txt

@@ -60,6 +60,29 @@ else()
   message(STATUS "FCL does not use Octomap")
 endif()
 
+# Find FLANN (optional)
+find_package(flann QUIET)
+if (FLANN_FOUND)
+  set(FCL_HAVE_FLANN 1)
+  include_directories(${FLANN_INCLUDE_DIRS})
+  link_directories(${FLANN_LIBRARY_DIRS})
+  message(STATUS "FCL uses Flann")
+else()
+  message(STATUS "FCL does not use Flann")
+endif()
+
+
+find_package(tinyxml QUIET)
+if (TINYXML_FOUND)
+  set(FCL_HAVE_TINYXML 1)
+  include_directories(${TINYXML_INCLUDE_DIRS})
+  link_directories(${TINYXML_LIBRARY_DIRS})
+  message(STATUS "FCL uses tinyxml")
+else()
+  message(STATUS "FCL does not use tinyxml")
+endif()
+
+
 find_package(Boost COMPONENTS thread date_time filesystem system unit_test_framework REQUIRED)
 include_directories(${Boost_INCLUDE_DIR})
 

CMakeModules/Findflann.cmake

+# Find FLANN, a Fast Library for Approximate Nearest Neighbors
+
+include(FindPackageHandleStandardArgs)
+
+find_path(FLANN_INCLUDE_DIR flann.hpp PATH_SUFFIXES flann)
+if (FLANN_INCLUDE_DIR)
+    file(READ "${FLANN_INCLUDE_DIR}/config.h" FLANN_CONFIG)
+    string(REGEX REPLACE ".*FLANN_VERSION_ \"([0-9.]+)\".*" "\\1" FLANN_VERSION ${FLANN_CONFIG})
+    if(NOT FLANN_VERSION VERSION_LESS flann_FIND_VERSION)
+        string(REGEX REPLACE "/flann$" "" FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR})
+    endif()
+endif()
+
+find_package_handle_standard_args(flann DEFAULT_MSG FLANN_INCLUDE_DIRS)

CMakeModules/Findtinyxml.cmake

+# this module was taken from http://trac.evemu.org/browser/trunk/cmake/FindTinyXML.cmake
+# - Find TinyXML
+# Find the native TinyXML includes and library
+#
+# TINYXML_FOUND - True if TinyXML found.
+# TINYXML_INCLUDE_DIR - where to find tinyxml.h, etc.
+# TINYXML_LIBRARIES - List of libraries when using TinyXML.
+#
+
+IF( TINYXML_INCLUDE_DIRS)
+# Already in cache, be silent
+SET( TinyXML_FIND_QUIETLY TRUE )
+ENDIF( TINYXML_INCLUDE_DIRS )
+
+FIND_PATH( TINYXML_INCLUDE_DIRS "tinyxml.h"
+PATH_SUFFIXES "tinyxml" )
+
+FIND_LIBRARY( TINYXML_LIBRARY_DIRS
+NAMES "tinyxml"
+PATH_SUFFIXES "tinyxml" )
+
+# handle the QUIETLY and REQUIRED arguments and set TINYXML_FOUND to TRUE if
+# all listed variables are TRUE
+INCLUDE( "FindPackageHandleStandardArgs" )
+FIND_PACKAGE_HANDLE_STANDARD_ARGS( "TinyXML" DEFAULT_MSG TINYXML_INCLUDE_DIRS TINYXML_LIBRARY_DIRS )
+
+MARK_AS_ADVANCED( TINYXML_INCLUDE_DIRS TINYXML_LIBRARY_DIRS )
\ No newline at end of file

include/fcl/BV/AABB.h

@@ -186,6 +186,12 @@ public:
     return (max_ - min_).sqrLength();
   }
 
+  /// @brief Radius of the AABB
+  inline FCL_REAL radius() const
+  {
+    return (max_ - min_).length() / 2;
+  }
+
   /// @brief Center of the AABB
   inline  Vec3f center() const
   {

include/fcl/BVH/BVH_model.h

@@ -191,6 +191,62 @@ public:
     makeParentRelativeRecurse(0, I, Vec3f());
   }
 
+  Vec3f computeCOM() const
+  {
+    FCL_REAL vol = 0;
+    Vec3f com;
+    for(int i = 0; i < num_tris; ++i)
+    {
+      const Triangle& tri = tri_indices[i];
+      FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
+      vol += d_six_vol;
+      com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
+    }
+
+    return com / (vol * 4);
+  }
+
+  FCL_REAL computeVolume() const
+  {
+    FCL_REAL vol = 0;
+    for(int i = 0; i < num_tris; ++i)
+    {
+      const Triangle& tri = tri_indices[i];
+      FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
+      vol += d_six_vol;
+    }
+
+    return vol / 6;
+  }
+
+  Matrix3f computeMomentofInertia() const
+  {
+    Matrix3f C(0, 0, 0,
+               0, 0, 0,
+               0, 0, 0);
+
+    Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0,
+                         1/120.0, 1/60.0, 1/120.0,
+                         1/120.0, 1/120.0, 1/60.0);
+
+    for(int i = 0; i < num_tris; ++i)
+    {
+      const Triangle& tri = tri_indices[i];
+      const Vec3f& v1 = vertices[tri[0]];
+      const Vec3f& v2 = vertices[tri[1]];
+      const Vec3f& v3 = vertices[tri[2]];
+      FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
+      Matrix3f A(v1, v2, v3);
+      C += transpose(A) * C_canonical * A * d_six_vol;
+    }
+
+    FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2);
+
+    return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2),
+                    -C(1, 0), trace_C - C(1, 1), -C(1, 2),
+                    -C(2, 0), -C(2, 1), trace_C - C(2, 2));
+  }
+
 public:
   /// @brief Geometry point data
   Vec3f* vertices;

include/fcl/collision_data.h

@@ -39,6 +39,10 @@
 #define FCL_COLLISION_DATA_H
 
 #include "fcl/collision_object.h"
+#include "fcl/learning/classifier.h"
+#include "fcl/knn/nearest_neighbors.h"
+
+
 #include "fcl/math/vec_3f.h"
 #include <vector>
 #include <set>
@@ -447,7 +451,7 @@ struct ContinuousCollisionRequest
   CCDSolverType ccd_solver_type;
   
   ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10,
-                             FCL_REAL toc_err_ = 0,
+                             FCL_REAL toc_err_ = 0.0001,
                              CCDMotionType ccd_motion_type_ = CCDM_TRANS,
                              GJKSolverType gjk_solver_type_ = GST_LIBCCD,
                              CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_),
@@ -467,6 +471,8 @@ struct ContinuousCollisionResult
   
   /// @brief time of contact in [0, 1]
   FCL_REAL time_of_contact;
+
+  Transform3f contact_tf1, contact_tf2;
   
   ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0)
   {
@@ -474,28 +480,36 @@ struct ContinuousCollisionResult
 };
 
 
-enum PenetrationDepthType {PDT_LOCAL, PDT_AL};
+enum PenetrationDepthType {PDT_TRANSLATIONAL, PDT_GENERAL_EULER, PDT_GENERAL_QUAT, PDT_GENERAL_EULER_BALL, PDT_GENERAL_QUAT_BALL};
 
-struct PenetrationDepthMetricBase
-{
-  virtual FCL_REAL operator() (const Transform3f& tf1, const Transform3f& tf2) const = 0;
-};
+enum KNNSolverType {KNN_LINEAR, KNN_GNAT, KNN_SQRTAPPROX};
 
-struct WeightEuclideanPDMetric : public PenetrationDepthMetricBase
-{
-  
-};
 
 struct PenetrationDepthRequest
 {
+  void* classifier;
+
+  NearestNeighbors<Transform3f>::DistanceFunction distance_func;
+
+  /// @brief KNN solver type
+  KNNSolverType knn_solver_type;
+  
   /// @brief PD algorithm type
   PenetrationDepthType pd_type;
 
   /// @brief gjk solver type
   GJKSolverType gjk_solver_type;
 
-  PenetrationDepthRequest(PenetrationDepthType pd_type_ = PDT_LOCAL,
-                          GJKSolverType gjk_solver_type_ = GST_LIBCCD) : pd_type(pd_type_),
+  std::vector<Transform3f> contact_vectors;
+
+  PenetrationDepthRequest(void* classifier_,
+                          NearestNeighbors<Transform3f>::DistanceFunction distance_func_,
+                          KNNSolverType knn_solver_type_ = KNN_LINEAR,
+                          PenetrationDepthType pd_type_ = PDT_TRANSLATIONAL,
+                          GJKSolverType gjk_solver_type_ = GST_LIBCCD) : classifier(classifier_),
+                                                                         distance_func(distance_func_),
+                                                                         knn_solver_type(knn_solver_type_),
+                                                                         pd_type(pd_type_),
                                                                          gjk_solver_type(gjk_solver_type_)
   {
   }
@@ -507,9 +521,7 @@ struct PenetrationDepthResult
   FCL_REAL pd_value;
 
   /// @brief the transform where the collision is resolved
-  Transform3f resolve_trans;
-
-  
+  Transform3f resolved_tf; 
 };
 
 

include/fcl/collision_object.h

@@ -116,6 +116,33 @@ public:
   /// @brief threshold for free (<= is free)
   FCL_REAL threshold_free;
 
+  /// @brief compute center of mass
+  virtual Vec3f computeCOM() const { return Vec3f(); }
+
+  /// @brief compute the inertia matrix, related to the origin
+  virtual Matrix3f computeMomentofInertia() const { return Matrix3f(); }
+
+  /// @brief compute the volume
+  virtual FCL_REAL computeVolume() const { return 0; }
+
+  /// @brief compute the inertia matrix, related to the com
+  virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
+  {
+    Matrix3f C = computeMomentofInertia();
+    Vec3f com = computeCOM();
+    FCL_REAL V = computeVolume();
+
+    return Matrix3f(C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
+                    C(0, 1) + V * com[0] * com[1],
+                    C(0, 2) + V * com[0] * com[2],
+                    C(1, 0) + V * com[1] * com[0],
+                    C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
+                    C(1, 2) + V * com[1] * com[2],
+                    C(2, 0) + V * com[2] * com[0],
+                    C(2, 1) + V * com[2] * com[1],
+                    C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]));
+  }
+
 };
 
 /// @brief the object for collision or distance computation, contains the geometry and the transform information

include/fcl/config.h.in

@@ -42,5 +42,7 @@
 
 #cmakedefine01 FCL_HAVE_SSE
 #cmakedefine01 FCL_HAVE_OCTOMAP
+#cmakedefine01 FCL_HAVE_FLANN
+#cmakedefine01 FCL_HAVE_TINYXML
 
 #endif

include/fcl/continuous_collision.h

@@ -9,15 +9,15 @@ namespace fcl
 {
 
 /// @brief continous collision checking between two objects
-FCL_REAL continuous_collide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
-                            const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
-                            const ContinuousCollisionRequest& request,
-                            ContinuousCollisionResult& result);
+FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
+                           const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end,
+                           const ContinuousCollisionRequest& request,
+                           ContinuousCollisionResult& result);
 
-FCL_REAL continuous_collide(const CollisionObject* o1, const Transform3f& tf1_end,
-                            const CollisionObject* o2, const Transform3f& tf2_end,
-                            const ContinuousCollisionRequest& request,
-                            ContinuousCollisionResult& result);
+FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end,
+                           const CollisionObject* o2, const Transform3f& tf2_end,
+                           const ContinuousCollisionRequest& request,
+                           ContinuousCollisionResult& result);
 
 FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2,
                  const ContinuousCollisionRequest& request,

include/fcl/exception.h

+#ifndef FCL_EXCEPTION_H
+#define FCL_EXCEPTION_H
+
+#include <stdexcept>
+#include <string>
+
+namespace fcl
+{
+
+class Exception : public std::runtime_error
+{
+public:
+
+  /** \brief This is just a wrapper on std::runtime_error */
+  explicit
+  Exception(const std::string& what) : std::runtime_error(what)
+  {
+  }
+
+  /** \brief This is just a wrapper on std::runtime_error with a
+      prefix added */
+  Exception(const std::string &prefix, const std::string& what) : std::runtime_error(prefix + ": " + what)
+  {
+  }
+
+  virtual ~Exception(void) throw()
+  {
+  }
+
+};
+
+}
+
+
+#endif

include/fcl/knn/greedy_kcenters.h

+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Rice University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Rice University 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: Mark Moll */
+
+#ifndef FCL_KNN_GREEDY_KCENTERS_H
+#define FCL_KNN_GREEDY_KCENTERS_H
+
+
+#include "fcl/math/sampling.h"
+
+namespace fcl
+{
+/// @brief An instance of this class can be used to greedily select a given
+/// number of representatives from a set of data points that are all far
+/// apart from each other.
+template<typename _T>
+class GreedyKCenters
+{
+public:
+  /// @brief The definition of a distance function
+  typedef boost::function<double(const _T&, const _T&)> DistanceFunction;
+
+  GreedyKCenters(void)
+  {
+  }
+
+  virtual ~GreedyKCenters(void)
+  {
+  }
+
+  /// @brief Set the distance function to use
+  void setDistanceFunction(const DistanceFunction &distFun)
+  {
+    distFun_ = distFun;
+  }
+
+  /// @brief Get the distance function used
+  const DistanceFunction& getDistanceFunction(void) const
+  {
+    return distFun_;
+  }
+
+  /// @brief Greedy algorithm for selecting k centers
+  /// @param data a vector of data points
+  /// @param k the desired number of centers
+  /// @param centers a vector of length k containing the indices into data of the k centers
+  /// @param dists a 2-dimensional array such that dists[i][j] is the distance between data[i] and data[center[j]]
+  void kcenters(const std::vector<_T>& data, unsigned int k,
+                std::vector<unsigned int>& centers, std::vector<std::vector<double> >& dists)
+  {
+    // array containing the minimum distance between each data point
+    // and the centers computed so far
+    std::vector<double> minDist(data.size(), std::numeric_limits<double>::infinity());
+
+    centers.clear();
+    centers.reserve(k);
+    dists.resize(data.size(), std::vector<double>(k));
+    // first center is picked randomly
+    centers.push_back(rng_.uniformInt(0, data.size() - 1));
+    for (unsigned i=1; i<k; ++i)
+    {
+      unsigned ind;
+      const _T& center = data[centers[i - 1]];
+      double maxDist = -std::numeric_limits<double>::infinity();
+      for (unsigned j=0; j<data.size(); ++j)
+      {
+        if ((dists[j][i-1] = distFun_(data[j], center)) < minDist[j])
+          minDist[j] = dists[j][i - 1];
+        // the j-th center is the one furthest away from center 0,..,j-1
+        if (minDist[j] > maxDist)
+        {
+          ind = j;
+          maxDist = minDist[j];
+        }
+      }
+      // no more centers available
+      if (maxDist < std::numeric_limits<double>::epsilon()) break;
+      centers.push_back(ind);
+    }
+
+    const _T& center = data[centers.back()];
+    unsigned i = centers.size() - 1;
+    for (unsigned j = 0; j < data.size(); ++j)
+      dists[j][i] = distFun_(data[j], center);
+  }
+
+protected:
+  /// @brief The used distance function
+  DistanceFunction distFun_;
+
+  /// Random number generator used to select first center
+  RNG rng_;
+};
+}
+
+#endif

include/fcl/knn/nearest_neighbors.h

+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 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 the Willow Garage 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: Ioan Sucan */
+
+#ifndef FCL_KNN_NEAREST_NEIGHBORS_H
+#define FCL_KNN_NEAREST_NEIGHBORS_H
+
+#include <vector>
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
+
+namespace fcl
+{
+/// @brief Abstract representation of a container that can perform nearest neighbors queries
+template<typename _T>
+class NearestNeighbors
+{
+public:
+
+  /// @brief The definition of a distance function
+  typedef boost::function<double(const _T&, const _T&)> DistanceFunction;
+
+  NearestNeighbors(void)
+  {
+  }
+
+  virtual ~NearestNeighbors(void)
+  {
+  }
+
+  /// @brief Set the distance function to use
+  virtual void setDistanceFunction(const DistanceFunction &distFun)
+  {
+    distFun_ = distFun;
+  }
+
+  /// @brief Get the distance function used
+  const DistanceFunction& getDistanceFunction(void) const
+  {
+    return distFun_;
+  }
+
+  /// @brief Clear the datastructure
+  virtual void clear(void) = 0;
+
+  /// @brief Add an element to the datastructure
+  virtual void add(const _T &data) = 0;
+
+  /// @brief Add a vector of points
+  virtual void add(const std::vector<_T> &data)
+  {
+    for (typename std::vector<_T>::const_iterator elt = data.begin() ; elt != data.end() ; ++elt)
+      add(*elt);
+  }
+
+  /// @brief Remove an element from the datastructure
+  virtual bool remove(const _T &data) = 0;
+
+  /// @brief Get the nearest neighbor of a point
+  virtual _T nearest(const _T &data) const = 0;
+
+  /// @brief Get the k-nearest neighbors of a point
+  virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const = 0;
+
+  /// @brief Get the nearest neighbors of a point, within a specified radius
+  virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const = 0;
+
+  /// @brief Get the number of elements in the datastructure
+  virtual std::size_t size(void) const = 0;
+
+  /// @brief Get all the elements in the datastructure
+  virtual void list(std::vector<_T> &data) const = 0;
+
+protected:
+
+  /// @brief The used distance function
+  DistanceFunction distFun_;
+
+};
+}
+
+
+#endif

include/fcl/knn/nearest_neighbors_flann.h

+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Rice University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Rice University nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.