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.
+ *
+ *  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_NEAREST_NEIGHBORS_FLANN_H
+#define FCL_KNN_NEAREST_NEIGHBORS_FLANN_H
+
+#include "fcl/config.h"
+#if FCL_HAVE_FLANN == 0
+# error FLANN is not available. Please use a different NearestNeighbors data structure
+#else
+
+#include "fcl/knn/nearest_neighbors.h"
+#include <flann/flann.hpp>
+#include "fcl/exception.h"
+
+namespace fcl
+{
+/** \brief Wrapper class to allow FLANN access to the
+    NearestNeighbors::distFun_ callback function
+*/
+template<typename _T>
+class FLANNDistance
+{
+public:
+  typedef _T ElementType;
+  typedef double ResultType;
+
+  FLANNDistance(const typename NearestNeighbors<_T>::DistanceFunction& distFun)
+    : distFun_(distFun)
+  {
+  }
+
+  template <typename Iterator1, typename Iterator2>
+  ResultType operator()(Iterator1 a, Iterator2 b,
+                        size_t /*size*/, ResultType /*worst_dist*/ = -1) const
+  {
+    return distFun_(*a, *b);
+  }
+protected:
+  const typename NearestNeighbors<_T>::DistanceFunction& distFun_;
+};
+
+/** \brief Wrapper class for nearest neighbor data structures in the
+    FLANN library.
+
+    See:
+    M. Muja and D.G. Lowe, "Fast Approximate Nearest Neighbors with
+    Automatic Algorithm Configuration", in International Conference
+    on Computer Vision Theory and Applications (VISAPP'09), 2009.
+    http://people.cs.ubc.ca/~mariusm/index.php/FLANN/FLANN
+*/
+template<typename _T, typename _Dist = FLANNDistance<_T> >
+class NearestNeighborsFLANN : public NearestNeighbors<_T>
+{
+public:
+
+  NearestNeighborsFLANN(const boost::shared_ptr<flann::IndexParams> &params)
+    : index_(0), params_(params), searchParams_(32, 0., true), dimension_(1)
+  {
+  }
+
+  virtual ~NearestNeighborsFLANN(void)
+  {
+    if (index_)
+      delete index_;
+  }
+
+  virtual void clear(void)
+  {
+    if (index_)
+    {
+      delete index_;
+      index_ = NULL;
+    }
+    data_.clear();
+  }
+
+  virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
+  {
+    NearestNeighbors<_T>::setDistanceFunction(distFun);
+    rebuildIndex();
+  }
+
+  virtual void add(const _T &data)
+  {
+    bool rebuild = index_ && (data_.size() + 1 > data_.capacity());
+
+    if (rebuild)
+      rebuildIndex(2 * data_.capacity());
+
+    data_.push_back(data);
+    const flann::Matrix<_T> mat(&data_.back(), 1, dimension_);
+
+    if (index_)
+      index_->addPoints(mat, std::numeric_limits<float>::max()/size());
+    else
+      createIndex(mat);
+  }
+  virtual void add(const std::vector<_T> &data)
+  {
+    unsigned int oldSize = data_.size();
+    unsigned int newSize = oldSize + data.size();
+    bool rebuild = index_ && (newSize > data_.capacity());
+
+    if (rebuild)
+      rebuildIndex(std::max(2 * oldSize, newSize));
+
+    if (index_)
+    {
+      std::copy(data.begin(), data.end(), data_.begin() + oldSize);
+      const flann::Matrix<_T> mat(&data_[oldSize], data.size(), dimension_);
+      index_->addPoints(mat, std::numeric_limits<float>::max()/size());
+    }
+    else
+    {
+      data_ = data;
+      const flann::Matrix<_T> mat(&data_[0], data_.size(), dimension_);
+      createIndex(mat);
+    }
+  }
+  virtual bool remove(const _T& data)
+  {
+    if (!index_) return false;
+    _T& elt = const_cast<_T&>(data);
+    const flann::Matrix<_T> query(&elt, 1, dimension_);
+    std::vector<std::vector<size_t> > indices(1);
+    std::vector<std::vector<double> > dists(1);
+    index_->knnSearch(query, indices, dists, 1, searchParams_);
+    if (*index_->getPoint(indices[0][0]) == data)
+    {
+      index_->removePoint(indices[0][0]);
+      rebuildIndex();
+      return true;
+    }
+    return false;
+  }
+  virtual _T nearest(const _T &data) const
+  {
+    if (size())
+    {
+      _T& elt = const_cast<_T&>(data);
+      const flann::Matrix<_T> query(&elt, 1, dimension_);
+      std::vector<std::vector<size_t> > indices(1);
+      std::vector<std::vector<double> > dists(1);
+      index_->knnSearch(query, indices, dists, 1, searchParams_);
+      return *index_->getPoint(indices[0][0]);
+    }
+    throw Exception("No elements found in nearest neighbors data structure");
+  }
+  virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
+  {
+    _T& elt = const_cast<_T&>(data);
+    const flann::Matrix<_T> query(&elt, 1, dimension_);
+    std::vector<std::vector<size_t> > indices;
+    std::vector<std::vector<double> > dists;
+    k = index_ ? index_->knnSearch(query, indices, dists, k, searchParams_) : 0;
+    nbh.resize(k);
+    for (std::size_t i = 0 ; i < k ; ++i)
+      nbh[i] = *index_->getPoint(indices[0][i]);
+  }
+
+  virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
+  {
+    _T& elt = const_cast<_T&>(data);
+    flann::Matrix<_T> query(&elt, 1, dimension_);
+    std::vector<std::vector<size_t> > indices;
+    std::vector<std::vector<double> > dists;
+    int k = index_ ? index_->radiusSearch(query, indices, dists, radius, searchParams_) : 0;
+    nbh.resize(k);
+    for (int i = 0 ; i < k ; ++i)
+      nbh[i] = *index_->getPoint(indices[0][i]);
+  }
+
+  virtual std::size_t size(void) const
+  {
+    return index_ ? index_->size() : 0;
+  }
+
+  virtual void list(std::vector<_T> &data) const
+  {
+    std::size_t sz = size();
+    if (sz == 0)
+    {
+      data.resize(0);
+      return;
+    }
+    const _T& dummy = *index_->getPoint(0);
+    int checks = searchParams_.checks;
+    searchParams_.checks = size();
+    nearestK(dummy, sz, data);
+    searchParams_.checks = checks;
+  }
+
+  /// \brief Set the FLANN index parameters.
+  ///
+  /// The parameters determine the type of nearest neighbor
+  /// data structure to be constructed.
+  virtual void setIndexParams(const boost::shared_ptr<flann::IndexParams> &params)
+  {
+    params_ = params;
+    rebuildIndex();
+  }
+
+  /// \brief Get the FLANN parameters used to build the current index.
+  virtual const boost::shared_ptr<flann::IndexParams>& getIndexParams(void) const
+  {
+    return params_;
+  }
+
+  /// \brief Set the FLANN parameters to be used during nearest neighbor
+  /// searches
+  virtual void setSearchParams(const flann::SearchParams& searchParams)
+  {
+    searchParams_ = searchParams;
+  }
+
+  /// \brief Get the FLANN parameters used during nearest neighbor
+  /// searches
+  flann::SearchParams& getSearchParams(void)
+  {
+    return searchParams_;
+  }
+
+  /// \brief Get the FLANN parameters used during nearest neighbor
+  /// searches
+  const flann::SearchParams& getSearchParams(void) const
+  {
+    return searchParams_;
+  }
+
+  unsigned int getContainerSize(void) const
+  {
+    return dimension_;
+  }
+
+protected:
+
+  /// \brief Internal function to construct nearest neighbor
+  /// data structure with initial elements stored in mat.
+  void createIndex(const flann::Matrix<_T>& mat)
+  {
+    index_ = new flann::Index<_Dist>(mat, *params_, _Dist(NearestNeighbors<_T>::distFun_));
+    index_->buildIndex();
+  }
+
+  /// \brief Rebuild the nearest neighbor data structure (necessary when
+  /// changing the distance function or index parameters).
+  void rebuildIndex(unsigned int capacity = 0)
+  {
+    if (index_)
+    {
+      std::vector<_T> data;
+      list(data);
+      clear();
+      if (capacity)
+        data_.reserve(capacity);
+      add(data);
+    }
+  }
+
+  /// \brief vector of data stored in FLANN's index. FLANN only indexes
+  /// references, so we need store the original data.
+  std::vector<_T>                       data_;
+
+  /// \brief The FLANN index (the actual index type depends on params_).
+  flann::Index<_Dist>*                  index_;
+
+  /// \brief The FLANN index parameters. This contains both the type of
+  /// index and the parameters for that type.
+  boost::shared_ptr<flann::IndexParams> params_;
+
+  /// \brief The parameters used to seach for nearest neighbors.
+  mutable flann::SearchParams           searchParams_;
+
+  /// \brief If each element has an array-like structure that is exposed
+  /// to FLANN, then the dimension_ needs to be set to the length of
+  /// this array.
+  unsigned int                          dimension_;
+};
+
+template<>
+void NearestNeighborsFLANN<double, flann::L2<double> >::createIndex(
+                                                                    const flann::Matrix<double>& mat)
+{
+  index_ = new flann::Index<flann::L2<double> >(mat, *params_);
+  index_->buildIndex();
+}
+
+template<typename _T, typename _Dist = FLANNDistance<_T> >
+class NearestNeighborsFLANNLinear : public NearestNeighborsFLANN<_T, _Dist>
+{
+public:
+  NearestNeighborsFLANNLinear()
+    : NearestNeighborsFLANN<_T, _Dist>(
+                                       boost::shared_ptr<flann::LinearIndexParams>(
+                                                                                   new flann::LinearIndexParams()))
+  {
+  }
+};
+
+template<typename _T, typename _Dist = FLANNDistance<_T> >
+class NearestNeighborsFLANNHierarchicalClustering : public NearestNeighborsFLANN<_T, _Dist>
+{
+public:
+  NearestNeighborsFLANNHierarchicalClustering()
+    : NearestNeighborsFLANN<_T, _Dist>(
+                                       boost::shared_ptr<flann::HierarchicalClusteringIndexParams>(
+                                                                                                   new flann::HierarchicalClusteringIndexParams()))
+  {
+  }
+};
+
+}
+
+#endif
+
+#endif

include/fcl/knn/nearest_neighbors_linear.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_LINEAR_H
+#define FCL_KNN_NEAREST_NEIGHBORS_LINEAR_H
+
+#include "fcl/knn/nearest_neighbors.h"
+#include "fcl/exception.h"
+#include <algorithm>
+
+namespace fcl
+{
+/** \brief A nearest neighbors datastructure that uses linear
+    search.
+
+    \li Search for nearest neighbor is O(n).
+    \li Search for k-nearest neighbors is  O(n log(k)).
+    \li Search for neighbors within a range is O(n log(n)).
+    \li Adding an element to the datastructure is O(1).
+    \li Removing an element from the datastructure O(n).
+*/
+template<typename _T>
+class NearestNeighborsLinear : public NearestNeighbors<_T>
+{
+public:
+  NearestNeighborsLinear(void) : NearestNeighbors<_T>()
+  {
+  }
+
+  virtual ~NearestNeighborsLinear(void)
+  {
+  }
+
+  virtual void clear(void)
+  {
+    data_.clear();
+  }
+
+  virtual void add(const _T &data)
+  {
+    data_.push_back(data);
+  }
+
+  virtual void add(const std::vector<_T> &data)
+  {
+    data_.reserve(data_.size() + data.size());
+    data_.insert(data_.end(), data.begin(), data.end());
+  }
+
+  virtual bool remove(const _T &data)
+  {
+    if (!data_.empty())
+      for (int i = data_.size() - 1 ; i >= 0 ; --i)
+        if (data_[i] == data)
+        {
+          data_.erase(data_.begin() + i);
+          return true;
+        }
+    return false;
+  }
+
+  virtual _T nearest(const _T &data) const
+  {
+    const std::size_t sz = data_.size();
+    std::size_t pos = sz;
+    double dmin = 0.0;
+    for (std::size_t i = 0 ; i < sz ; ++i)
+    {
+      double distance = NearestNeighbors<_T>::distFun_(data_[i], data);
+      if (pos == sz || dmin > distance)
+      {
+        pos = i;
+        dmin = distance;
+      }
+    }
+    if (pos != sz)
+      return data_[pos];
+
+    throw Exception("No elements found in nearest neighbors data structure");
+  }
+
+  virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
+  {
+    nbh = data_;
+    if (nbh.size() > k)
+    {
+      std::partial_sort(nbh.begin(), nbh.begin() + k, nbh.end(),
+                        ElemSort(data, NearestNeighbors<_T>::distFun_));
+      nbh.resize(k);
+    }
+    else
+    {
+      std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
+    }
+  }
+
+  virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
+  {
+    nbh.clear();
+    for (std::size_t i = 0 ; i < data_.size() ; ++i)
+      if (NearestNeighbors<_T>::distFun_(data_[i], data) <= radius)
+        nbh.push_back(data_[i]);
+    std::sort(nbh.begin(), nbh.end(), ElemSort(data, NearestNeighbors<_T>::distFun_));
+  }
+
+  virtual std::size_t size(void) const
+  {
+    return data_.size();
+  }
+
+  virtual void list(std::vector<_T> &data) const
+  {
+    data = data_;
+  }
+
+protected:
+
+  /** \brief The data elements stored in this structure */
+  std::vector<_T>   data_;
+
+private:
+
+  struct ElemSort
+  {
+    ElemSort(const _T &e, const typename NearestNeighbors<_T>::DistanceFunction &df) : e_(e), df_(df)
+    {
+    }
+
+    bool operator()(const _T &a, const _T &b) const
+    {
+      return df_(a, e_) < df_(b, e_);
+    }
+
+    const _T                                              &e_;
+    const typename NearestNeighbors<_T>::DistanceFunction &df_;
+  };
+
+};
+
+}
+
+#endif

include/fcl/knn/nearest_neighbors_sqrtapprox.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_SQRT_APPROX_H
+#define FCL_KNN_NEAREST_NEIGHBORS_SQRT_APPROX_H
+
+#include "fcl/knn/nearest_neighbors_linear.h"
+#include "fcl/exception.h"
+#include <algorithm>
+#include <cmath>
+
+namespace fcl
+{
+/** \brief A nearest neighbors datastructure that uses linear
+    search. The linear search is done over sqrt(n) elements
+    only. (Every sqrt(n) elements are skipped).
+
+    \li Search for nearest neighbor is O(sqrt(n)).
+    \li Search for k-nearest neighbors is  O(n log(k)).
+    \li Search for neighbors within a range is O(n log(n)).
+    \li Adding an element to the datastructure is O(1).
+    \li Removing an element from the datastructure O(n).
+*/
+template<typename _T>
+class NearestNeighborsSqrtApprox : public NearestNeighborsLinear<_T>
+{
+public:
+  NearestNeighborsSqrtApprox(void) : NearestNeighborsLinear<_T>(), checks_(0), offset_(0)
+  {
+  }
+
+  virtual ~NearestNeighborsSqrtApprox(void)
+  {
+  }
+
+  virtual void clear(void)
+  {
+    NearestNeighborsLinear<_T>::clear();
+    checks_ = 0;
+    offset_ = 0;
+  }
+
+  virtual void add(const _T &data)
+  {
+    NearestNeighborsLinear<_T>::add(data);
+    updateCheckCount();
+  }
+
+  virtual void add(const std::vector<_T> &data)
+  {
+    NearestNeighborsLinear<_T>::add(data);
+    updateCheckCount();
+  }
+
+  virtual bool remove(const _T &data)
+  {
+    bool result = NearestNeighborsLinear<_T>::remove(data);
+    if (result)
+      updateCheckCount();
+    return result;
+  }
+
+  virtual _T nearest(const _T &data) const
+  {
+    const std::size_t n = NearestNeighborsLinear<_T>::data_.size();
+    std::size_t pos = n;
+
+    if (checks_ > 0 && n > 0)
+    {
+      double dmin = 0.0;
+      for (std::size_t j = 0 ; j < checks_ ; ++j)
+      {
+        std::size_t i = (j * checks_ + offset_) % n;
+
+        double distance = NearestNeighbors<_T>::distFun_(NearestNeighborsLinear<_T>::data_[i], data);
+        if (pos == n || dmin > distance)
+        {
+          pos = i;
+          dmin = distance;
+        }
+      }
+      offset_ = (offset_ + 1) % checks_;
+    }
+    if (pos != n)
+      return NearestNeighborsLinear<_T>::data_[pos];
+
+    throw Exception("No elements found in nearest neighbors data structure");
+  }
+
+protected:
+
+  /** \brief The maximum number of checks to perform when searching for a nearest neighbor */
+  inline void updateCheckCount(void)
+  {
+    checks_ = 1 + (std::size_t)floor(sqrt((double)NearestNeighborsLinear<_T>::data_.size()));
+  }
+
+  /** \brief The number of checks to be performed when looking for a nearest neighbor */
+  std::size_t checks_;
+
+  /** \brief The offset to start checking at (between 0 and \e checks_) */
+  mutable std::size_t offset_;
+
+};
+
+}
+
+#endif

include/fcl/learning/classifier.h

+#ifndef FCL_LEARNING_CLASSIFIER_H
+#define FCL_LEARNING_CLASSIFIER_H
+
+#include "fcl/math/vec_nf.h"
+
+namespace fcl
+{
+template<std::size_t N>
+struct Item
+{
+  Vecnf<N> q;
+  bool label;
+  FCL_REAL w;
+
+  Item(const Vecnf<N>& q_, bool label_, FCL_REAL w_ = 1) : q(q_),
+                                                           label(label_),
+                                                           w(w_)
+  {}
+
+  Item() {}
+};
+
+template<std::size_t N>
+struct Scaler
+{
+  Vecnf<N> v_min, v_max;
+  Scaler()
+  {
+    // default no scale
+    for(std::size_t i = 0; i < N; ++i)
+    {
+      v_min[i] = 0;
+      v_max[i] = 1;
+    }
+  }
+
+  Scaler(const Vecnf<N>& v_min_, const Vecnf<N>& v_max_) : v_min(v_min_),
+                                                           v_max(v_max_)
+  {}
+
+  Vecnf<N> scale(const Vecnf<N>& v) const
+  {
+    Vecnf<N> res;
+    for(std::size_t i = 0; i < N; ++i)
+      res[i] = (v[i] - v_min[i]) / (v_max[i] - v_min[i]);
+    return res;
+  }
+
+  Vecnf<N> unscale(const Vecnf<N>& v) const
+  {
+    Vecnf<N> res;
+    for(std::size_t i = 0; i < N; ++i)
+      res[i] = v[i] * (v_max[i] - v_min[i]) + v_min[i];
+    return res;
+  }
+};
+
+
+struct PredictResult
+{
+  bool label;
+  FCL_REAL prob;
+
+  PredictResult() {}
+  PredictResult(bool label_, FCL_REAL prob_ = 1) : label(label_),
+                                                   prob(prob_)
+  {}
+};
+
+template<std::size_t N>
+class SVMClassifier
+{
+public:
+  
+  virtual PredictResult predict(const Vecnf<N>& q) const = 0;
+  virtual std::vector<PredictResult> predict(const std::vector<Vecnf<N> >& qs) const = 0;
+
+  virtual std::vector<Item<N> > getSupportVectors() const = 0;
+  virtual void setScaler(const Scaler<N>& scaler) = 0;
+
+  virtual void learn(const std::vector<Item<N> >& data) = 0;
+
+  FCL_REAL error_rate(const std::vector<Item<N> >& data) const
+  {
+    std::size_t num = data.size();
+
+    std::size_t error_num = 0;
+    for(std::size_t i = 0; i < data.size(); ++i)
+    {
+      PredictResult res = predict(data[i].q);
+      if(res.label != data[i].label)
+        error_num++;
+    }
+
+    return error_num / (FCL_REAL)num;
+  }
+};
+
+template<std::size_t N>
+Scaler<N> computeScaler(const std::vector<Item<N> >& data)
+{
+  Vecnf<N> lower_bound, upper_bound;
+  for(std::size_t j = 0; j < N; ++j)
+  {
+    lower_bound[j] = std::numeric_limits<FCL_REAL>::max();
+    upper_bound[j] = -std::numeric_limits<FCL_REAL>::max();
+  }
+  
+  for(std::size_t i = 0; i < data.size(); ++i)
+  {
+    for(std::size_t j = 0; j < N; ++j)
+    {
+      if(data[i].q[j] < lower_bound[j]) lower_bound[j] = data[i].q[j];
+      if(data[i].q[j] > upper_bound[j]) upper_bound[j] = data[i].q[j];
+    }
+  }
+
+  return Scaler<N>(lower_bound, upper_bound);
+}
+
+template<std::size_t N>
+Scaler<N> computeScaler(const std::vector<Vecnf<N> >& data)
+{
+  Vecnf<N> lower_bound, upper_bound;
+  for(std::size_t j = 0; j < N; ++j)
+  {
+    lower_bound[j] = std::numeric_limits<FCL_REAL>::max();
+    upper_bound[j] = -std::numeric_limits<FCL_REAL>::max();
+  }
+  
+  for(std::size_t i = 0; i < data.size(); ++i)
+  {
+    for(std::size_t j = 0; j < N; ++j)
+    {
+      if(data[i][j] < lower_bound[j]) lower_bound[j] = data[i][j];
+      if(data[i][j] > upper_bound[j]) upper_bound[j] = data[i][j];
+    }
+  }
+
+  return Scaler<N>(lower_bound, upper_bound);  
+}
+
+}
+
+#endif
+

include/fcl/math/transform.h

@@ -77,6 +77,12 @@ public:
   /// @brief Quaternion to matrix
   void toRotation(Matrix3f& R) const;
 
+  /// @brief Euler to quaternion
+  void fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c);
+
+  /// @brief Quaternion to Euler
+  void toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const;
+
   /// @brief Axes to quaternion
   void fromAxes(const Vec3f axis[3]);
 
@@ -134,6 +140,26 @@ public:
 
   Vec3f getRow(std::size_t i) const;
 
+  bool operator == (const Quaternion3f& other) const
+  {
+    for(std::size_t i = 0; i < 4; ++i)
+    {
+      if(data[i] != other[i])
+        return false;
+    }
+    return true;
+  }
+
+  bool operator != (const Quaternion3f& other) const
+  {
+    return !(*this == other);
+  }
+
+  FCL_REAL operator [] (std::size_t i) const
+  {
+    return data[i];
+  }
+
 private:
 
   FCL_REAL data[4];
@@ -145,6 +171,7 @@ Quaternion3f conj(const Quaternion3f& q);
 /// @brief inverse of quaternion
 Quaternion3f inverse(const Quaternion3f& q);
 
+
 /// @brief Simple transform class used locally by InterpMotion
 class Transform3f
 {
@@ -333,6 +360,16 @@ public:
     matrix_set = true;
   }
 
+  bool operator == (const Transform3f& other) const
+  {
+    return (q == other.getQuatRotation()) && (T == other.getTranslation());
+  }
+
+  bool operator != (const Transform3f& other) const
+  {
+    return !(*this == other);
+  }
+
 };
 
 /// @brief inverse the transform

include/fcl/math/vec_3f.h

@@ -131,6 +131,17 @@ public:
   inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
   inline Vec3fX<T>& negate() { data.negate(); return *this; }
 
+  bool operator == (const Vec3fX& other) const
+  {
+    return equal(other, 0);
+  }
+
+  bool operator != (const Vec3fX& other) const
+  {
+    return !(*this == other);
+  }
+
+
   inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
   {
     data.ubound(u.data);

include/fcl/penetration_depth.h

@@ -4,20 +4,109 @@
 #include "fcl/math/vec_3f.h"
 #include "fcl/collision_object.h"
 #include "fcl/collision_data.h"
+#include "fcl/learning/classifier.h"
+#include "fcl/knn/nearest_neighbors.h"
+#include <boost/mem_fn.hpp>
+#include <iostream>
 
 namespace fcl
 {
 
+typedef double (*TransformDistance)(const Transform3f&, const Transform3f&);
+
+class DefaultTransformDistancer
+{
+public:
+  const CollisionGeometry* o1;
+  const CollisionGeometry* o2;
+
+  FCL_REAL rot_x_weight, rot_y_weight, rot_z_weight;
+
+  DefaultTransformDistancer() : o1(NULL), o2(NULL)
+  {
+    rot_x_weight = rot_y_weight = rot_z_weight = 1;
+  }
+
+  DefaultTransformDistancer(const CollisionGeometry* o2_)
+  {
+    o2 = o2_;
+    init();
+  }
+
+  DefaultTransformDistancer(const CollisionGeometry* o1_, const CollisionGeometry* o2_)
+  {
+    o1 = o1_;
+    o2 = o2_;
+    init();
+  }
+
+  void init()
+  {
+    rot_x_weight = rot_y_weight = rot_z_weight = 1;
+
+    if(o2)
+    {
+      FCL_REAL V = o2->computeVolume();
+      Matrix3f C = o2->computeMomentofInertiaRelatedToCOM();
+      FCL_REAL eigen_values[3];
+      Vec3f eigen_vectors[3];
+      eigen(C, eigen_values, eigen_vectors);
+      rot_x_weight = eigen_values[0] * 4 / V;
+      rot_y_weight = eigen_values[1] * 4 / V;
+      rot_z_weight = eigen_values[2] * 4 / V;
+
+      // std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
+    }
+  }
+
+  void printRotWeight() const
+  {
+    std::cout << rot_x_weight << " " << rot_y_weight << " " << rot_z_weight << std::endl;
+  }
+
+  double distance(const Transform3f& tf1, const Transform3f& tf2)
+  {
+    Transform3f tf;
+    relativeTransform(tf1, tf2, tf);
+
+    const Quaternion3f& quat = tf.getQuatRotation();
+    const Vec3f& trans = tf.getTranslation();
+
+    FCL_REAL d = rot_x_weight * quat[1] * quat[1] + rot_y_weight * quat[2] * quat[2] + rot_z_weight * quat[3] * quat[3] + trans[0] * trans[0] + trans[1] * trans[1] + trans[2] * trans[2];
+    return d;
+  }
+};
+
+static DefaultTransformDistancer default_transform_distancer;
+
+static NearestNeighbors<Transform3f>::DistanceFunction default_transform_distance_func = boost::bind(&DefaultTransformDistancer::distance, &default_transform_distancer, _1, _2);
+
+
+
+/// @brief precompute a learning model for penetration computation
+std::vector<Transform3f> penetrationDepthModelLearning(const CollisionObject* o1,
+                                                       const CollisionObject* o2,
+                                                       PenetrationDepthType pd_type,
+                                                       void* classifier,
+                                                       std::size_t n_samples,
+                                                       FCL_REAL margin,
+                                                       KNNSolverType knn_solver_type,
+                                                       std::size_t knn_k,
+                                                       NearestNeighbors<Transform3f>::DistanceFunction distance_func = default_transform_distance_func);
+
+
+
+
 /// @brief penetration depth computation between two in-collision objects
-FCL_REAL penetration_depth(const CollisionGeometry* o1, const Transform3f& tf1,
-                           const CollisionGeometry* o2, const Transform3f& tf2,
-                           const PenetrationDepthRequest& request,
-                           PenetrationDepthResult& result);
-
-FCL_REAL penetration_depth(const CollisionObject* o1,
-                           const CollisionObject* o2,
-                           const PenetrationDepthRequest& request,
-                           PenetrationDepthResult& result);
+FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
+                          const CollisionGeometry* o2, const Transform3f& tf2,
+                          const PenetrationDepthRequest& request,
+                          PenetrationDepthResult& result);
+
+FCL_REAL penetrationDepth(const CollisionObject* o1,
+                          const CollisionObject* o2,
+                          const PenetrationDepthRequest& request,
+                          PenetrationDepthResult& result);
 
 }