diff --git a/CMakeLists.txt b/CMakeLists.txt
index 96c7ea6e7d66e74275329f4e8df41d2eabaf0f36..cb2d0eee3382b2b9882548df18b5caf5f47edd7b 100644
--- a/CMakeLists.txt
+++ b/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})
diff --git a/CMakeModules/Findflann.cmake b/CMakeModules/Findflann.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..3b3f6dc09474812f27f97268c9cd7e09c6fb20f6
--- /dev/null
+++ b/CMakeModules/Findflann.cmake
@@ -0,0 +1,14 @@
+# 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)
diff --git a/CMakeModules/Findtinyxml.cmake b/CMakeModules/Findtinyxml.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..4c7cbde895eaaeb3e324bad5bd073de695bb87c1
--- /dev/null
+++ b/CMakeModules/Findtinyxml.cmake
@@ -0,0 +1,27 @@
+# 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
diff --git a/include/fcl/BV/AABB.h b/include/fcl/BV/AABB.h
index ec04609b043b141f2aea2043d692d88cc31d500b..3a4800fdb065674e2cae90418a5184cbbeaa7263 100644
--- a/include/fcl/BV/AABB.h
+++ b/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
{
diff --git a/include/fcl/BVH/BVH_model.h b/include/fcl/BVH/BVH_model.h
index 4c6a0699d8c093618fb2d2ab5f69b1f3d2391f94..847db5bee0c6f9a94febf6e96dd98403e2d9f269 100644
--- a/include/fcl/BVH/BVH_model.h
+++ b/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;
diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h
index 9621d33d6daa97e74cd142a9e1fe83bb50bae3dc..5ea511434682cea1bb059a635af8a0350bce0387 100644
--- a/include/fcl/collision_data.h
+++ b/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;
};
diff --git a/include/fcl/collision_object.h b/include/fcl/collision_object.h
index 00716051a33e711a41cc3baf6d7b04e1b5adb0d5..7d54ffd65fafd5059ffa8f405710ccdb53702582 100644
--- a/include/fcl/collision_object.h
+++ b/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
diff --git a/include/fcl/config.h.in b/include/fcl/config.h.in
index ba1aed54e5c4e66a43e304ab1e80481b856d890f..e178b6b58f413a8dd3841d84d53b6cb0a672e3f2 100644
--- a/include/fcl/config.h.in
+++ b/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
diff --git a/include/fcl/continuous_collision.h b/include/fcl/continuous_collision.h
index 0a6ab1ab19fa06b9940be25422bc9d7b644d90a1..ef6812d0ac9a037eda0a974267902048580c9729 100644
--- a/include/fcl/continuous_collision.h
+++ b/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,
diff --git a/include/fcl/exception.h b/include/fcl/exception.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f1b11beb353ae9061c1c9d949de2a5457563ff8
--- /dev/null
+++ b/include/fcl/exception.h
@@ -0,0 +1,35 @@
+#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
diff --git a/include/fcl/knn/greedy_kcenters.h b/include/fcl/knn/greedy_kcenters.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2f64848e63d1564224e66dbfcdef2066677a8aa
--- /dev/null
+++ b/include/fcl/knn/greedy_kcenters.h
@@ -0,0 +1,128 @@
+/*********************************************************************
+ * 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
diff --git a/include/fcl/knn/nearest_neighbors.h b/include/fcl/knn/nearest_neighbors.h
new file mode 100644
index 0000000000000000000000000000000000000000..100d8f878b4d66b7cd0806656c2ab3b351d5c7a9
--- /dev/null
+++ b/include/fcl/knn/nearest_neighbors.h
@@ -0,0 +1,115 @@
+/*********************************************************************
+ * 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
diff --git a/include/fcl/knn/nearest_neighbors_GNAT.h b/include/fcl/knn/nearest_neighbors_GNAT.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7cd0d091a955bd1fcfc179ef182c0c123e82291
--- /dev/null
+++ b/include/fcl/knn/nearest_neighbors_GNAT.h
@@ -0,0 +1,696 @@
+/*********************************************************************
+ * 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_GNAT_H
+#define FCL_KNN_NEAREST_NEIGHBORS_GNAT_H
+
+#include "fcl/knn/nearest_neighbors.h"
+#include "fcl/knn/greedy_kcenters.h"
+#include "fcl/exception.h"
+#include <boost/unordered_set.hpp>
+#include <queue>
+#include <algorithm>
+
+
+namespace fcl
+{
+
+/** \brief Geometric Near-neighbor Access Tree (GNAT), a data
+ structure for nearest neighbor search.
+
+ See:
+ S. Brin, "Near neighbor search in large metric spaces," in Proc. 21st
+ Conf. on Very Large Databases (VLDB), pp. 574¡V584, 1995.
+
+*/
+template<typename _T>
+class NearestNeighborsGNAT : public NearestNeighbors<_T>
+{
+protected:
+ /// \cond IGNORE
+ // internally, we use a priority queue for nearest neighbors, paired
+ // with their distance to the query point
+ typedef std::pair<const _T*,double> DataDist;
+ struct DataDistCompare
+ {
+ bool operator()(const DataDist& d0, const DataDist& d1)
+ {
+ return d0.second < d1.second;
+ }
+ };
+ typedef std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare> NearQueue;
+
+ // another internal data structure is a priority queue of nodes to
+ // check next for possible nearest neighbors
+ class Node;
+ typedef std::pair<Node*,double> NodeDist;
+ struct NodeDistCompare
+ {
+ bool operator()(const NodeDist& n0, const NodeDist& n1) const
+ {
+ return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
+ }
+ };
+ typedef std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare> NodeQueue;
+ /// \endcond
+
+public:
+ NearestNeighborsGNAT(unsigned int degree = 4, unsigned int minDegree = 2,
+ unsigned int maxDegree = 6, unsigned int maxNumPtsPerLeaf = 50,
+ unsigned int removedCacheSize = 50, bool rebalancing = false)
+ : NearestNeighbors<_T>(), tree_(NULL), degree_(degree),
+ minDegree_(std::min(degree,minDegree)), maxDegree_(std::max(maxDegree,degree)),
+ maxNumPtsPerLeaf_(maxNumPtsPerLeaf), size_(0),
+ rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
+ removedCacheSize_(removedCacheSize)
+ {
+ }
+
+ virtual ~NearestNeighborsGNAT(void)
+ {
+ if (tree_)
+ delete tree_;
+ }
+ /// \brief Set the distance function to use
+ virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
+ {
+ NearestNeighbors<_T>::setDistanceFunction(distFun);
+ pivotSelector_.setDistanceFunction(distFun);
+ if (tree_)
+ rebuildDataStructure();
+ }
+ virtual void clear(void)
+ {
+ if (tree_)
+ {
+ delete tree_;
+ tree_ = NULL;
+ }
+ size_ = 0;
+ removed_.clear();
+ if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
+ rebuildSize_ = maxNumPtsPerLeaf_ * degree_;
+ }
+
+ virtual void add(const _T &data)
+ {
+ if (tree_)
+ {
+ if (isRemoved(data))
+ rebuildDataStructure();
+ tree_->add(*this, data);
+ }
+ else
+ {
+ tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
+ size_ = 1;
+ }
+ }
+ virtual void add(const std::vector<_T> &data)
+ {
+ if (tree_)
+ NearestNeighbors<_T>::add(data);
+ else if (data.size()>0)
+ {
+ tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
+ for (unsigned int i=1; i<data.size(); ++i)
+ tree_->data_.push_back(data[i]);
+ if (tree_->needToSplit(*this))
+ tree_->split(*this);
+ }
+ size_ += data.size();
+ }
+ /// \brief Rebuild the internal data structure.
+ void rebuildDataStructure()
+ {
+ std::vector<_T> lst;
+ list(lst);
+ clear();
+ add(lst);
+ }
+ /// \brief Remove data from the tree.
+ /// The element won't actually be removed immediately, but just marked
+ /// for removal in the removed_ cache. When the cache is full, the tree
+ /// will be rebuilt and the elements marked for removal will actually
+ /// be removed.
+ virtual bool remove(const _T &data)
+ {
+ if (!size_) return false;
+ NearQueue nbhQueue;
+ // find data in tree
+ bool isPivot = nearestKInternal(data, 1, nbhQueue);
+ if (*nbhQueue.top().first != data)
+ return false;
+ removed_.insert(nbhQueue.top().first);
+ size_--;
+ // if we removed a pivot or if the capacity of removed elements
+ // has been reached, we rebuild the entire GNAT
+ if (isPivot || removed_.size()>=removedCacheSize_)
+ rebuildDataStructure();
+ return true;
+ }
+
+ virtual _T nearest(const _T &data) const
+ {
+ if (size_)
+ {
+ std::vector<_T> nbh;
+ nearestK(data, 1, nbh);
+ if (!nbh.empty()) return nbh[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
+ {
+ nbh.clear();
+ if (k == 0) return;
+ if (size_)
+ {
+ NearQueue nbhQueue;
+ nearestKInternal(data, k, nbhQueue);
+ postprocessNearest(nbhQueue, nbh);
+ }
+ }
+
+ virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
+ {
+ nbh.clear();
+ if (size_)
+ {
+ NearQueue nbhQueue;
+ nearestRInternal(data, radius, nbhQueue);
+ postprocessNearest(nbhQueue, nbh);
+ }
+ }
+
+ virtual std::size_t size(void) const
+ {
+ return size_;
+ }
+
+ virtual void list(std::vector<_T> &data) const
+ {
+ data.clear();
+ data.reserve(size());
+ if (tree_)
+ tree_->list(*this, data);
+ }
+
+ /// \brief Print a GNAT structure (mostly useful for debugging purposes).
+ friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
+ {
+ if (gnat.tree_)
+ {
+ out << *gnat.tree_;
+ if (!gnat.removed_.empty())
+ {
+ out << "Elements marked for removal:\n";
+ for (typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
+ it != gnat.removed_.end(); it++)
+ out << **it << '\t';
+ out << std::endl;
+ }
+ }
+ return out;
+ }
+
+ // for debugging purposes
+ void integrityCheck()
+ {
+ std::vector<_T> lst;
+ boost::unordered_set<const _T*> tmp;
+ // get all elements, including those marked for removal
+ removed_.swap(tmp);
+ list(lst);
+ // check if every element marked for removal is also in the tree
+ for (typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
+ {
+ unsigned int i;
+ for (i=0; i<lst.size(); ++i)
+ if (lst[i]==**it)
+ break;
+ if (i == lst.size())
+ {
+ // an element marked for removal is not actually in the tree
+ std::cout << "***** FAIL!! ******\n" << *this << '\n';
+ for (unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<'\t';
+ std::cout<<std::endl;
+ }
+ assert(i != lst.size());
+ }
+ // restore
+ removed_.swap(tmp);
+ // get elements in the tree with elements marked for removal purged from the list
+ list(lst);
+ if (lst.size() != size_)
+ std::cout << "#########################################\n" << *this << std::endl;
+ assert(lst.size() == size_);
+ }
+protected:
+ typedef NearestNeighborsGNAT<_T> GNAT;
+
+ /// Return true iff data has been marked for removal.
+ bool isRemoved(const _T& data) const
+ {
+ return !removed_.empty() && removed_.find(&data) != removed_.end();
+ }
+
+ /// \brief Return in nbhQueue the k nearest neighbors of data.
+ /// For k=1, return true if the nearest neighbor is a pivot.
+ /// (which is important during removal; removing pivots is a
+ /// special case).
+ bool nearestKInternal(const _T &data, std::size_t k, NearQueue& nbhQueue) const
+ {
+ bool isPivot;
+ double dist;
+ NodeDist nodeDist;
+ NodeQueue nodeQueue;
+
+ isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data,
+ NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
+ tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
+ while (nodeQueue.size() > 0)
+ {
+ dist = nbhQueue.top().second; // note the difference with nearestRInternal
+ nodeDist = nodeQueue.top();
+ nodeQueue.pop();
+ if (nbhQueue.size() == k &&
+ (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
+ nodeDist.second < nodeDist.first->minRadius_ - dist))
+ break;
+ nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
+ }
+ return isPivot;
+ }
+ /// \brief Return in nbhQueue the elements that are within distance radius of data.
+ void nearestRInternal(const _T &data, double radius, NearQueue& nbhQueue) const
+ {
+ double dist = radius; // note the difference with nearestKInternal
+ NodeQueue nodeQueue;
+ NodeDist nodeDist;
+
+ tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
+ NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
+ tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
+ while (nodeQueue.size() > 0)
+ {
+ nodeDist = nodeQueue.top();
+ nodeQueue.pop();
+ if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
+ nodeDist.second < nodeDist.first->minRadius_ - dist)
+ break;
+ nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
+ }
+ }
+ /// \brief Convert the internal data structure used for storing neighbors
+ /// to the vector that NearestNeighbor API requires.
+ void postprocessNearest(NearQueue& nbhQueue, std::vector<_T> &nbh) const
+ {
+ typename std::vector<_T>::reverse_iterator it;
+ nbh.resize(nbhQueue.size());
+ for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
+ *it = *nbhQueue.top().first;
+ }
+
+ /// The class used internally to define the GNAT.
+ class Node
+ {
+ public:
+ /// \brief Construct a node of given degree with at most
+ /// \e capacity data elements and wit given pivot.
+ Node(int degree, int capacity, const _T& pivot)
+ : degree_(degree), pivot_(pivot),
+ minRadius_(std::numeric_limits<double>::infinity()),
+ maxRadius_(-minRadius_), minRange_(degree, minRadius_),
+ maxRange_(degree, maxRadius_)
+ {
+ // The "+1" is needed because we add an element before we check whether to split
+ data_.reserve(capacity+1);
+ }
+
+ ~Node()
+ {
+ for (unsigned int i=0; i<children_.size(); ++i)
+ delete children_[i];
+ }
+
+ /// \brief Update minRadius_ and maxRadius_, given that an element
+ /// was added with distance dist to the pivot.
+ void updateRadius(double dist)
+ {
+ if (minRadius_ > dist)
+ minRadius_ = dist;
+ if (maxRadius_ < dist)
+ maxRadius_ = dist;
+ }
+ /// \brief Update minRange_[i] and maxRange_[i], given that an
+ /// element was added to the i-th child of the parent that has
+ /// distance dist to this Node's pivot.
+ void updateRange(unsigned int i, double dist)
+ {
+ if (minRange_[i] > dist)
+ minRange_[i] = dist;
+ if (maxRange_[i] < dist)
+ maxRange_[i] = dist;
+ }
+ /// Add an element to the tree rooted at this node.
+ void add(GNAT& gnat, const _T& data)
+ {
+ if (children_.size()==0)
+ {
+ data_.push_back(data);
+ gnat.size_++;
+ if (needToSplit(gnat))
+ {
+ if (gnat.removed_.size() > 0)
+ gnat.rebuildDataStructure();
+ else if (gnat.size_ >= gnat.rebuildSize_)
+ {
+ gnat.rebuildSize_ <<= 1;
+ gnat.rebuildDataStructure();
+ }
+ else
+ split(gnat);
+ }
+ }
+ else
+ {
+ std::vector<double> dist(children_.size());
+ double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
+ int minInd = 0;
+
+ for (unsigned int i=1; i<children_.size(); ++i)
+ if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
+ {
+ minDist = dist[i];
+ minInd = i;
+ }
+ for (unsigned int i=0; i<children_.size(); ++i)
+ children_[i]->updateRange(minInd, dist[i]);
+ children_[minInd]->updateRadius(minDist);
+ children_[minInd]->add(gnat, data);
+ }
+ }
+ /// Return true iff the node needs to be split into child nodes.
+ bool needToSplit(const GNAT& gnat) const
+ {
+ unsigned int sz = data_.size();
+ return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
+ }
+ /// \brief The split operation finds pivot elements for the child
+ /// nodes and moves each data element of this node to the appropriate
+ /// child node.
+ void split(GNAT& gnat)
+ {
+ std::vector<std::vector<double> > dists;
+ std::vector<unsigned int> pivots;
+
+ children_.reserve(degree_);
+ gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
+ for(unsigned int i=0; i<pivots.size(); i++)
+ children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivots[i]]));
+ degree_ = pivots.size(); // in case fewer than degree_ pivots were found
+ for (unsigned int j=0; j<data_.size(); ++j)
+ {
+ unsigned int k = 0;
+ for (unsigned int i=1; i<degree_; ++i)
+ if (dists[j][i] < dists[j][k])
+ k = i;
+ Node* child = children_[k];
+ if (j != pivots[k])
+ {
+ child->data_.push_back(data_[j]);
+ child->updateRadius(dists[j][k]);
+ }
+ for (unsigned int i=0; i<degree_; ++i)
+ children_[i]->updateRange(k, dists[j][i]);
+ }
+
+ for (unsigned int i=0; i<degree_; ++i)
+ {
+ // make sure degree lies between minDegree_ and maxDegree_
+ children_[i]->degree_ = std::min(std::max(
+ degree_ * (unsigned int)(children_[i]->data_.size() / data_.size()),
+ gnat.minDegree_), gnat.maxDegree_);
+ // singleton
+ if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
+ children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
+ }
+ // this does more than clear(); it also sets capacity to 0 and frees the memory
+ std::vector<_T> tmp;
+ data_.swap(tmp);
+ // check if new leaves need to be split
+ for (unsigned int i=0; i<degree_; ++i)
+ if (children_[i]->needToSplit(gnat))
+ children_[i]->split(gnat);
+ }
+
+ /// Insert data in nbh if it is a near neighbor. Return true iff data was added to nbh.
+ bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const
+ {
+ if (nbh.size() < k)
+ {
+ nbh.push(std::make_pair(&data, dist));
+ return true;
+ }
+ else if (dist < nbh.top().second ||
+ (dist < std::numeric_limits<double>::epsilon() && data==key))
+ {
+ nbh.pop();
+ nbh.push(std::make_pair(&data, dist));
+ return true;
+ }
+ return false;
+ }
+
+ /// \brief Compute the k nearest neighbors of data in the tree.
+ /// For k=1, isPivot is true if the nearest neighbor is a pivot
+ /// (which is important during removal; removing pivots is a
+ /// special case). The nodeQueue, which contains other Nodes
+ /// that need to be checked for nearest neighbors, is updated.
+ void nearestK(const GNAT& gnat, const _T &data, std::size_t k,
+ NearQueue& nbh, NodeQueue& nodeQueue, bool& isPivot) const
+ {
+ for (unsigned int i=0; i<data_.size(); ++i)
+ if (!gnat.isRemoved(data_[i]))
+ {
+ if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
+ isPivot = false;
+ }
+ if (children_.size() > 0)
+ {
+ double dist;
+ Node* child;
+ std::vector<double> distToPivot(children_.size());
+ std::vector<int> permutation(children_.size());
+
+ for (unsigned int i=0; i<permutation.size(); ++i)
+ permutation[i] = i;
+ std::random_shuffle(permutation.begin(), permutation.end());
+
+ for (unsigned int i=0; i<children_.size(); ++i)
+ if (permutation[i] >= 0)
+ {
+ child = children_[permutation[i]];
+ distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
+ if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
+ isPivot = true;
+ if (nbh.size()==k)
+ {
+ dist = nbh.top().second; // note difference with nearestR
+ for (unsigned int j=0; j<children_.size(); ++j)
+ if (permutation[j] >=0 && i != j &&
+ (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
+ distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
+ permutation[j] = -1;
+ }
+ }
+
+ dist = nbh.top().second;
+ for (unsigned int i=0; i<children_.size(); ++i)
+ if (permutation[i] >= 0)
+ {
+ child = children_[permutation[i]];
+ if (nbh.size()<k ||
+ (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
+ distToPivot[permutation[i]] + dist >= child->minRadius_))
+ nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
+ }
+ }
+ }
+ /// Insert data in nbh if it is a near neighbor.
+ void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const
+ {
+ if (dist <= r)
+ nbh.push(std::make_pair(&data, dist));
+ }
+ /// \brief Return all elements that are within distance r in nbh.
+ /// The nodeQueue, which contains other Nodes that need to
+ /// be checked for nearest neighbors, is updated.
+ void nearestR(const GNAT& gnat, const _T &data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const
+ {
+ double dist = r; //note difference with nearestK
+
+ for (unsigned int i=0; i<data_.size(); ++i)
+ if (!gnat.isRemoved(data_[i]))
+ insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
+ if (children_.size() > 0)
+ {
+ Node* child;
+ std::vector<double> distToPivot(children_.size());
+ std::vector<int> permutation(children_.size());
+
+ for (unsigned int i=0; i<permutation.size(); ++i)
+ permutation[i] = i;
+ std::random_shuffle(permutation.begin(), permutation.end());
+
+ for (unsigned int i=0; i<children_.size(); ++i)
+ if (permutation[i] >= 0)
+ {
+ child = children_[permutation[i]];
+ distToPivot[i] = gnat.distFun_(data, child->pivot_);
+ insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
+ for (unsigned int j=0; j<children_.size(); ++j)
+ if (permutation[j] >=0 && i != j &&
+ (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
+ distToPivot[i] + dist < child->minRange_[permutation[j]]))
+ permutation[j] = -1;
+ }
+
+ for (unsigned int i=0; i<children_.size(); ++i)
+ if (permutation[i] >= 0)
+ {
+ child = children_[permutation[i]];
+ if (distToPivot[i] - dist <= child->maxRadius_ &&
+ distToPivot[i] + dist >= child->minRadius_)
+ nodeQueue.push(std::make_pair(child, distToPivot[i]));
+ }
+ }
+ }
+
+ void list(const GNAT& gnat, std::vector<_T> &data) const
+ {
+ if (!gnat.isRemoved(pivot_))
+ data.push_back(pivot_);
+ for (unsigned int i=0; i<data_.size(); ++i)
+ if(!gnat.isRemoved(data_[i]))
+ data.push_back(data_[i]);
+ for (unsigned int i=0; i<children_.size(); ++i)
+ children_[i]->list(gnat, data);
+ }
+
+ friend std::ostream& operator<<(std::ostream& out, const Node& node)
+ {
+ out << "\ndegree:\t" << node.degree_;
+ out << "\nminRadius:\t" << node.minRadius_;
+ out << "\nmaxRadius:\t" << node.maxRadius_;
+ out << "\nminRange:\t";
+ for (unsigned int i=0; i<node.minRange_.size(); ++i)
+ out << node.minRange_[i] << '\t';
+ out << "\nmaxRange: ";
+ for (unsigned int i=0; i<node.maxRange_.size(); ++i)
+ out << node.maxRange_[i] << '\t';
+ out << "\npivot:\t" << node.pivot_;
+ out << "\ndata: ";
+ for (unsigned int i=0; i<node.data_.size(); ++i)
+ out << node.data_[i] << '\t';
+ out << "\nthis:\t" << &node;
+ out << "\nchildren:\n";
+ for (unsigned int i=0; i<node.children_.size(); ++i)
+ out << node.children_[i] << '\t';
+ out << '\n';
+ for (unsigned int i=0; i<node.children_.size(); ++i)
+ out << *node.children_[i] << '\n';
+ return out;
+ }
+
+ /// Number of child nodes
+ unsigned int degree_;
+ /// Data element stored in this Node
+ const _T pivot_;
+ /// Minimum distance between the pivot element and the elements stored in data_
+ double minRadius_;
+ /// Maximum distance between the pivot element and the elements stored in data_
+ double maxRadius_;
+ /// \brief The i-th element in minRange_ is the minimum distance between the
+ /// pivot and any data_ element in the i-th child node of this node's parent.
+ std::vector<double> minRange_;
+ /// \brief The i-th element in maxRange_ is the maximum distance between the
+ /// pivot and any data_ element in the i-th child node of this node's parent.
+ std::vector<double> maxRange_;
+ /// \brief The data elements stored in this node (in addition to the pivot
+ /// element). An internal node has no elements stored in data_.
+ std::vector<_T> data_;
+ /// \brief The child nodes of this node. By definition, only internal nodes
+ /// have child nodes.
+ std::vector<Node*> children_;
+ };
+
+ /// \brief The data structure containing the elements stored in this structure.
+ Node* tree_;
+ /// The desired degree of each node.
+ unsigned int degree_;
+ /// \brief After splitting a Node, each child Node has degree equal to
+ /// the default degree times the fraction of data elements from the
+ /// original node that got assigned to that child Node. However, its
+ /// degree can be no less than minDegree_.
+ unsigned int minDegree_;
+ /// \brief After splitting a Node, each child Node has degree equal to
+ /// the default degree times the fraction of data elements from the
+ /// original node that got assigned to that child Node. However, its
+ /// degree can be no larger than maxDegree_.
+ unsigned int maxDegree_;
+ /// \brief Maximum number of elements allowed to be stored in a Node before
+ /// it needs to be split into several nodes.
+ unsigned int maxNumPtsPerLeaf_;
+ /// \brief Number of elements stored in the tree.
+ std::size_t size_;
+ /// \brief If size_ exceeds rebuildSize_, the tree will be rebuilt (and
+ /// automatically rebalanced), and rebuildSize_ will be doubled.
+ std::size_t rebuildSize_;
+ /// \brief Maximum number of removed elements that can be stored in the
+ /// removed_ cache. If the cache is full, the tree will be rebuilt with
+ /// the elements in removed_ actually removed from the tree.
+ std::size_t removedCacheSize_;
+ /// \brief The data structure used to split data into subtrees.
+ GreedyKCenters<_T> pivotSelector_;
+ /// \brief Cache of removed elements.
+ boost::unordered_set<const _T*> removed_;
+};
+}
+
+#endif
diff --git a/include/fcl/knn/nearest_neighbors_flann.h b/include/fcl/knn/nearest_neighbors_flann.h
new file mode 100644
index 0000000000000000000000000000000000000000..76380f3c39f4dae4277c29bf9d0d86b1d2186d01
--- /dev/null
+++ b/include/fcl/knn/nearest_neighbors_flann.h
@@ -0,0 +1,350 @@
+/*********************************************************************
+ * 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> ¶ms)
+ : 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> ¶ms)
+ {
+ 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
diff --git a/include/fcl/knn/nearest_neighbors_linear.h b/include/fcl/knn/nearest_neighbors_linear.h
new file mode 100644
index 0000000000000000000000000000000000000000..3275fa7f795cf964dcd48876a4542fd2fa778afa
--- /dev/null
+++ b/include/fcl/knn/nearest_neighbors_linear.h
@@ -0,0 +1,175 @@
+/*********************************************************************
+ * 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
diff --git a/include/fcl/knn/nearest_neighbors_sqrtapprox.h b/include/fcl/knn/nearest_neighbors_sqrtapprox.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b6c46c119631fe045caf1d1c9a0feb3dc3eeee6
--- /dev/null
+++ b/include/fcl/knn/nearest_neighbors_sqrtapprox.h
@@ -0,0 +1,141 @@
+/*********************************************************************
+ * 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
diff --git a/include/fcl/learning/classifier.h b/include/fcl/learning/classifier.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c88ffa7298b3d05fa5eab6c36f780308745a3b7
--- /dev/null
+++ b/include/fcl/learning/classifier.h
@@ -0,0 +1,146 @@
+#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
+
diff --git a/include/fcl/math/transform.h b/include/fcl/math/transform.h
index e82c1f69d07c3199635be04c51e87a7a1d3e3e33..d66ea8310d918b990cb12b8ad09413c53187c012 100644
--- a/include/fcl/math/transform.h
+++ b/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
diff --git a/include/fcl/math/vec_3f.h b/include/fcl/math/vec_3f.h
index ba60e0e9b410a908b12c8f01887bda8c09f47529..840ba8de3a915781b78df126104c81ca486e7eb3 100644
--- a/include/fcl/math/vec_3f.h
+++ b/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);
diff --git a/include/fcl/penetration_depth.h b/include/fcl/penetration_depth.h
index 97ccff5cab5042e06978a75f5f6ea0e863cdee6f..d61ed4f472fe6f482807a1fbc75843494e52fcbc 100644
--- a/include/fcl/penetration_depth.h
+++ b/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);
}
diff --git a/include/fcl/shape/geometric_shapes.h b/include/fcl/shape/geometric_shapes.h
index 818833ffdc8fb5843e9967203411e62134b7b653..f714129c1f1072003a6c10cdb8066ced80658ca8 100644
--- a/include/fcl/shape/geometric_shapes.h
+++ b/include/fcl/shape/geometric_shapes.h
@@ -93,6 +93,22 @@ public:
/// @brief Get node type: a box
NODE_TYPE getNodeType() const { return GEOM_BOX; }
+
+ FCL_REAL computeVolume() const
+ {
+ return side[0] * side[1] * side[2];
+ }
+
+ Matrix3f computeMomentofInertia() const
+ {
+ FCL_REAL V = computeVolume();
+ FCL_REAL a2 = side[0] * side[0] * V;
+ FCL_REAL b2 = side[1] * side[1] * V;
+ FCL_REAL c2 = side[2] * side[2] * V;
+ return Matrix3f((b2 + c2) / 12, 0, 0,
+ 0, (a2 + c2) / 12, 0,
+ 0, 0, (a2 + b2) / 12);
+ }
};
/// @brief Center at zero point sphere
@@ -111,6 +127,19 @@ public:
/// @brief Get node type: a sphere
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
+
+ Matrix3f computeMomentofInertia() const
+ {
+ FCL_REAL I = 0.4 * radius * radius * computeVolume();
+ return Matrix3f(I, 0, 0,
+ 0, I, 0,
+ 0, 0, I);
+ }
+
+ FCL_REAL computeVolume() const
+ {
+ return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius / 3;
+ }
};
/// @brief Center at zero point capsule
@@ -132,6 +161,25 @@ public:
/// @brief Get node type: a capsule
NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
+
+ FCL_REAL computeVolume() const
+ {
+ return boost::math::constants::pi<FCL_REAL>() * radius * radius *(lz + radius * 4/3.0);
+ }
+
+ Matrix3f computeMomentofInertia() const
+ {
+ FCL_REAL v_cyl = radius * radius * lz * boost::math::constants::pi<FCL_REAL>();
+ FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
+
+ FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz;
+ FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
+
+ return Matrix3f(ix, 0, 0,
+ 0, ix, 0,
+ 0, 0, iz);
+ }
+
};
/// @brief Center at zero cone
@@ -153,6 +201,27 @@ public:
/// @brief Get node type: a cone
NODE_TYPE getNodeType() const { return GEOM_CONE; }
+
+ FCL_REAL computeVolume() const
+ {
+ return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz / 3;
+ }
+
+ Matrix3f computeMomentofInertia() const
+ {
+ FCL_REAL V = computeVolume();
+ FCL_REAL ix = V * (0.1 * lz * lz + 3 * radius * radius / 20);
+ FCL_REAL iz = 0.3 * V * radius * radius;
+
+ return Matrix3f(ix, 0, 0,
+ 0, ix, 0,
+ 0, 0, iz);
+ }
+
+ Vec3f computeCOM() const
+ {
+ return Vec3f(0, 0, -0.25 * lz);
+ }
};
/// @brief Center at zero cylinder
@@ -175,6 +244,21 @@ public:
/// @brief Get node type: a cylinder
NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
+
+ FCL_REAL computeVolume() const
+ {
+ return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz;
+ }
+
+ Matrix3f computeMomentofInertia() const
+ {
+ FCL_REAL V = computeVolume();
+ FCL_REAL ix = V * (3 * radius * radius + lz * lz) / 12;
+ FCL_REAL iz = V * radius * radius / 2;
+ return Matrix3f(ix, 0, 0,
+ 0, ix, 0,
+ 0, 0, iz);
+ }
};
/// @brief Convex polytope
@@ -253,6 +337,124 @@ public:
/// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex)
Vec3f center;
+ /// based on http://number-none.com/blow/inertia/bb_inertia.doc
+ 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);
+
+ int* points_in_poly = polygons;
+ int* index = polygons + 1;
+ for(int i = 0; i < num_planes; ++i)
+ {
+ Vec3f plane_center;
+
+ // compute the center of the polygon
+ for(int j = 0; j < *points_in_poly; ++j)
+ plane_center += points[index[j]];
+ plane_center = plane_center * (1.0 / *points_in_poly);
+
+ // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
+ const Vec3f& v3 = plane_center;
+ for(int j = 0; j < *points_in_poly; ++j)
+ {
+ int e_first = index[j];
+ int e_second = index[(j+1)%*points_in_poly];
+ const Vec3f& v1 = points[e_first];
+ const Vec3f& v2 = points[e_second];
+ FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
+ Matrix3f A(v1, v2, v3); // this is A' in the original document
+ C += transpose(A) * C_canonical * A * d_six_vol; // change accordingly
+ }
+
+ points_in_poly += (*points_in_poly + 1);
+ index = points_in_poly + 1;
+ }
+
+ 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));
+
+ }
+
+ Vec3f computeCOM() const
+ {
+ Vec3f com;
+ FCL_REAL vol = 0;
+ int* points_in_poly = polygons;
+ int* index = polygons + 1;
+ for(int i = 0; i < num_planes; ++i)
+ {
+ Vec3f plane_center;
+
+ // compute the center of the polygon
+ for(int j = 0; j < *points_in_poly; ++j)
+ plane_center += points[index[j]];
+ plane_center = plane_center * (1.0 / *points_in_poly);
+
+ // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape
+ const Vec3f& v3 = plane_center;
+ for(int j = 0; j < *points_in_poly; ++j)
+ {
+ int e_first = index[j];
+ int e_second = index[(j+1)%*points_in_poly];
+ const Vec3f& v1 = points[e_first];
+ const Vec3f& v2 = points[e_second];
+ FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
+ vol += d_six_vol;
+ com += (points[e_first] + points[e_second] + plane_center) * d_six_vol;
+ }
+
+ points_in_poly += (*points_in_poly + 1);
+ index = points_in_poly + 1;
+ }
+
+ return com / (vol * 4); // here we choose zero as the reference
+ }
+
+ FCL_REAL computeVolume() const
+ {
+ FCL_REAL vol = 0;
+ int* points_in_poly = polygons;
+ int* index = polygons + 1;
+ for(int i = 0; i < num_planes; ++i)
+ {
+ Vec3f plane_center;
+
+ // compute the center of the polygon
+ for(int j = 0; j < *points_in_poly; ++j)
+ plane_center += points[index[j]];
+ plane_center = plane_center * (1.0 / *points_in_poly);
+
+ // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape
+ const Vec3f& v3 = plane_center;
+ for(int j = 0; j < *points_in_poly; ++j)
+ {
+ int e_first = index[j];
+ int e_second = index[(j+1)%*points_in_poly];
+ const Vec3f& v1 = points[e_first];
+ const Vec3f& v2 = points[e_second];
+ FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
+ vol += d_six_vol;
+ }
+
+ points_in_poly += (*points_in_poly + 1);
+ index = points_in_poly + 1;
+ }
+
+ return vol / 6;
+ }
+
+
+
protected:
/// @brief Get edge information
void fillEdges();
diff --git a/src/continuous_collision.cpp b/src/continuous_collision.cpp
index 260145fa55b98bee2332b75d2bc56a8f0790e360..fef5635806f88e329a3d87d72d194e223bb53875 100644
--- a/src/continuous_collision.cpp
+++ b/src/continuous_collision.cpp
@@ -50,6 +50,9 @@ FCL_REAL continuousCollideNaive(const CollisionGeometry* o1, const MotionBase* m
for(std::size_t i = 0; i < n_iter; ++i)
{
FCL_REAL t = i / (FCL_REAL) (n_iter - 1);
+ motion1->integrate(t);
+ motion2->integrate(t);
+
motion1->getCurrentTransform(cur_tf1);
motion2->getCurrentTransform(cur_tf2);
@@ -60,6 +63,8 @@ FCL_REAL continuousCollideNaive(const CollisionGeometry* o1, const MotionBase* m
{
result.is_collide = true;
result.time_of_contact = t;
+ result.contact_tf1 = cur_tf1;
+ result.contact_tf2 = cur_tf2;
return t;
}
}
@@ -113,6 +118,16 @@ FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1_, const Tran
result.is_collide = (node.pairs.size() > 0);
result.time_of_contact = node.time_of_contact;
+ if(result.is_collide)
+ {
+ motion1->integrate(node.time_of_contact);
+ motion2->integrate(node.time_of_contact);
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+ result.contact_tf1 = tf1;
+ result.contact_tf2 = tf2;
+ }
+
return result.time_of_contact;
}
@@ -199,6 +214,18 @@ FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, c
if(!nsolver_)
delete nsolver;
+ if(result.is_collide)
+ {
+ motion1->integrate(result.time_of_contact);
+ motion2->integrate(result.time_of_contact);
+
+ Transform3f tf1, tf2;
+ motion1->getCurrentTransform(tf1);
+ motion2->getCurrentTransform(tf2);
+ result.contact_tf1 = tf1;
+ result.contact_tf2 = tf2;
+ }
+
return res;
}
diff --git a/src/math/sampling.cpp b/src/math/sampling.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4838eda44474c783022a0d0f4979f3b9106a66f5
--- /dev/null
+++ b/src/math/sampling.cpp
@@ -0,0 +1,147 @@
+#include "fcl/math/sampling.h"
+#include <boost/random/lagged_fibonacci.hpp>
+#include <boost/random/uniform_int.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/math/constants/constants.hpp>
+
+namespace fcl
+{
+
+/// The seed the user asked for (cannot be 0)
+static boost::uint32_t userSetSeed = 0;
+
+/// Flag indicating whether the first seed has already been generated or not
+static bool firstSeedGenerated = false;
+
+/// The value of the first seed
+static boost::uint32_t firstSeedValue = 0;
+
+/// Compute the first seed to be used; this function should be called only once
+static boost::uint32_t firstSeed()
+{
+ static boost::mutex fsLock;
+ boost::mutex::scoped_lock slock(fsLock);
+
+ if(firstSeedGenerated)
+ return firstSeedValue;
+
+ if(userSetSeed != 0)
+ firstSeedValue = userSetSeed;
+ else firstSeedValue = (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() - boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds();
+ firstSeedGenerated = true;
+
+ return firstSeedValue;
+}
+
+/// We use a different random number generator for the seeds of the
+/// Other random generators. The root seed is from the number of
+/// nano-seconds in the current time.
+static boost::uint32_t nextSeed()
+{
+ static boost::mutex rngMutex;
+ boost::mutex::scoped_lock slock(rngMutex);
+ static boost::lagged_fibonacci607 sGen(firstSeed());
+ static boost::uniform_int<> sDist(1, 1000000000);
+ static boost::variate_generator<boost::lagged_fibonacci607&, boost::uniform_int<> > s(sGen, sDist);
+ return s();
+}
+
+boost::uint32_t RNG::getSeed()
+{
+ return firstSeed();
+}
+
+void RNG::setSeed(boost::uint32_t seed)
+{
+ if(firstSeedGenerated)
+ {
+ std::cerr << "Random number generation already started. Changing seed now will not lead to deterministic sampling." << std::endl;
+ }
+ if(seed == 0)
+ {
+ std::cerr << "Random generator seed cannot be 0. Using 1 instead." << std::endl;
+ userSetSeed = 1;
+ }
+ else
+ userSetSeed = seed;
+}
+
+RNG::RNG() : generator_(nextSeed()),
+ uniDist_(0, 1),
+ normalDist_(0, 1),
+ uni_(generator_, uniDist_),
+ normal_(generator_, normalDist_)
+{
+}
+
+double RNG::halfNormalReal(double r_min, double r_max, double focus)
+{
+ assert(r_min <= r_max);
+
+ const double mean = r_max - r_min;
+ double v = gaussian(mean, mean / focus);
+
+ if(v > mean) v = 2.0 * mean - v;
+ double r = v >= 0.0 ? v + r_min : r_min;
+ return r > r_max ? r_max : r;
+}
+
+int RNG::halfNormalInt(int r_min, int r_max, double focus)
+{
+ int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
+ return (r > r_max) ? r_max : r;
+}
+
+// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
+// pg. 124-132
+void RNG::quaternion(double value[4])
+{
+ double x0 = uni_();
+ double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
+ double t1 = 2.0 * boost::math::constants::pi<double>() * uni_(), t2 = 2.0 * boost::math::constants::pi<double>() * uni_();
+ double c1 = cos(t1), s1 = sin(t1);
+ double c2 = cos(t2), s2 = sin(t2);
+ value[0] = s1 * r1;
+ value[1] = c1 * r1;
+ value[2] = s2 * r2;
+ value[3] = c2 * r2;
+}
+
+// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
+void RNG::eulerRPY(double value[3])
+{
+ value[0] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
+ value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi<double>() / 2.0;
+ value[2] = boost::math::constants::pi<double>() * (2.0 * uni_() - 1.0);
+}
+
+void RNG::disk(double r_min, double r_max, double& x, double& y)
+{
+ double a = uniform01();
+ double b = uniform01();
+ double r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min);
+ double theta = 2 * boost::math::constants::pi<double>() * b;
+ x = r * std::cos(theta);
+ y = r * std::sin(theta);
+}
+
+void RNG::ball(double r_min, double r_max, double& x, double& y, double& z)
+{
+ double a = uniform01();
+ double b = uniform01();
+ double c = uniform01();
+ double r = std::pow(a * r_max * r_max * r_max + (1 - a) * r_min * r_min * r_min, 1 / 3.0);
+ double theta = std::acos(1 - 2 * b);
+ double phi = 2 * boost::math::constants::pi<double>() * c;
+
+ double costheta = std::cos(theta);
+ double sintheta = std::sin(theta);
+ double cosphi = std::cos(phi);
+ double sinphi = std::sin(phi);
+ x = r * costheta;
+ y = r * sintheta * cosphi;
+ z = r * sintheta * sinphi;
+}
+
+}
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index 665fe7ccca1c94e400eebb3605cb41a92e28a777..7670db042cbf8697e3efdc529bcb4eda9bab6169 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -36,6 +36,7 @@
#include "fcl/math/transform.h"
+#include <boost/math/constants/constants.hpp>
namespace fcl
{
@@ -327,6 +328,37 @@ Quaternion3f inverse(const Quaternion3f& q)
return res.inverse();
}
+void Quaternion3f::fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c)
+{
+ Matrix3f R;
+ R.setEulerYPR(a, b, c);
+
+ fromRotation(R);
+}
+
+void Quaternion3f::toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const
+{
+ Matrix3f R;
+ toRotation(R);
+ a = atan2(R(1, 0), R(0, 0));
+ b = asin(-R(2, 0));
+ c = atan2(R(2, 1), R(2, 2));
+
+ if(b == boost::math::constants::pi<double>() * 0.5)
+ {
+ if(a > 0)
+ a -= boost::math::constants::pi<double>();
+ else
+ a += boost::math::constants::pi<double>();
+
+ if(c > 0)
+ c -= boost::math::constants::pi<double>();
+ else
+ c += boost::math::constants::pi<double>();
+ }
+}
+
+
Vec3f Quaternion3f::getColumn(std::size_t i) const
{
switch(i)
diff --git a/src/penetration_depth.cpp b/src/penetration_depth.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6859e3d33d340fca6795113decfd7b69d9510ae8
--- /dev/null
+++ b/src/penetration_depth.cpp
@@ -0,0 +1,419 @@
+#include "fcl/penetration_depth.h"
+#include "fcl/learning/classifier.h"
+#include "fcl/math/sampling.h"
+#include "fcl/collision.h"
+#include "fcl/exception.h"
+
+#include "fcl/knn/nearest_neighbors_linear.h"
+#include "fcl/knn/nearest_neighbors_GNAT.h"
+#include "fcl/knn/nearest_neighbors_sqrtapprox.h"
+#if FCL_HAVE_FLANN
+#include "fcl/knn/nearest_neighbors_flann.h"
+#endif
+
+#include "fcl/ccd/motion.h"
+#include "fcl/continuous_collision.h"
+
+namespace fcl
+{
+
+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)
+{
+ std::vector<Transform3f> support_transforms_positive;
+ std::vector<Transform3f> support_transforms_negative;
+
+ switch(pd_type)
+ {
+ case PDT_TRANSLATIONAL:
+ {
+ SVMClassifier<3>* classifier = static_cast<SVMClassifier<3>*>(classifier_);
+ SamplerR<3> sampler;
+ Vecnf<3> lower_bound, upper_bound;
+ AABB aabb1 = o1->getAABB();
+ AABB aabb2 = o2->getAABB();
+ lower_bound[0] = aabb1.min_[0] - aabb2.max_[0] - margin;
+ lower_bound[1] = aabb1.min_[1] - aabb2.max_[1] - margin;
+ lower_bound[2] = aabb1.min_[2] - aabb2.max_[2] - margin;
+ upper_bound[0] = aabb1.max_[0] - aabb2.min_[0] + margin;
+ upper_bound[1] = aabb1.max_[1] - aabb2.min_[1] + margin;
+ upper_bound[2] = aabb1.max_[2] - aabb2.min_[2] + margin;
+
+ sampler.setBound(lower_bound, upper_bound);
+
+ std::vector<Item<3> > data(n_samples);
+ for(std::size_t i = 0; i < n_samples; ++i)
+ {
+ Vecnf<3> q = sampler.sample();
+ Transform3f tf(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2]));
+
+ CollisionRequest request;
+ CollisionResult result;
+
+ int n_collide = collide(o1->getCollisionGeometry(), o1->getTransform(),
+ o2->getCollisionGeometry(), tf, request, result);
+
+ data[i] = Item<3>(q, (n_collide > 0));
+ }
+
+ // classifier.setScaler(Scaler<3>(lower_bound, upper_bound));
+ classifier->setScaler(computeScaler(data));
+
+ classifier->learn(data);
+
+ std::vector<Item<3> > svs = classifier->getSupportVectors();
+ for(std::size_t i = 0; i < svs.size(); ++i)
+ {
+ const Vecnf<3>& q = svs[i].q;
+ if(svs[i].label)
+ support_transforms_positive.push_back(Transform3f(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2])));
+ else
+ support_transforms_negative.push_back(Transform3f(o2->getQuatRotation(), Vec3f(q[0], q[1], q[2])));
+ }
+
+ break;
+ }
+ case PDT_GENERAL_EULER:
+ {
+ SVMClassifier<6>* classifier = static_cast<SVMClassifier<6>*>(classifier_);
+ SamplerSE3Euler sampler;
+
+ FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
+ FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
+ Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
+ Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
+
+ FCL_REAL r = r1 + r2 + c1.length() + c2.length() + margin;
+ sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
+
+ std::vector<Item<6> > data(n_samples);
+ for(std::size_t i = 0; i < n_samples; ++i)
+ {
+ Vecnf<6> q = sampler.sample();
+
+ Quaternion3f quat;
+ quat.fromEuler(q[3], q[4], q[5]);
+ Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
+
+ CollisionRequest request;
+ CollisionResult result;
+
+ int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
+ o2->getCollisionGeometry(), tf, request, result);
+
+ data[i] = Item<6>(q, (n_collide > 0));
+ }
+
+ /*
+ Vecnf<6> scaler_lower_bound, scaler_upper_bound;
+ for(std::size_t i = 0; i < 3; ++i)
+ {
+ scaler_lower_bound[i] = -r;
+ scaler_upper_bound[i] = r;
+ }
+
+ for(std::size_t i = 3; i < 6; ++i)
+ {
+ scaler_lower_bound[i] = -boost::math::constants::pi<FCL_REAL>();
+ scaler_upper_bound[i] = boost::math::constants::pi<FCL_REAL>();
+ }
+
+ classifier.setScaler(Scaler<6>(scaler_lower_bound, scaler_upper_bound));
+ */
+
+ classifier->setScaler(computeScaler(data));
+
+ classifier->learn(data);
+
+ std::vector<Item <6> > svs = classifier->getSupportVectors();
+ for(std::size_t i = 0; i < svs.size(); ++i)
+ {
+ const Vecnf<6>& q = svs[i].q;
+ Quaternion3f quat;
+ quat.fromEuler(q[3], q[4], q[5]);
+ if(svs[i].label)
+ support_transforms_positive.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ else
+ support_transforms_negative.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ }
+
+ break;
+ }
+ case PDT_GENERAL_QUAT:
+ {
+ SVMClassifier<7>* classifier = static_cast<SVMClassifier<7>*>(classifier_);
+ SamplerSE3Quat sampler;
+
+ FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
+ FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
+ Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
+ Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
+
+ FCL_REAL r = r1 + r2 + c1.length() + c2.length() + margin;
+
+ sampler.setBound(Vec3f(-r, -r, -r), Vec3f(r, r, r));
+
+ std::vector<Item<7> > data(n_samples);
+ for(std::size_t i = 0; i < n_samples; ++i)
+ {
+ Vecnf<7> q = sampler.sample();
+
+ Quaternion3f quat(q[3], q[4], q[5], q[6]);
+ Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
+
+ CollisionRequest request;
+ CollisionResult result;
+
+ int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
+ o2->getCollisionGeometry(), tf, request, result);
+
+ data[i] = Item<7>(q, (n_collide > 0));
+ }
+
+ classifier->setScaler(computeScaler(data));
+
+ classifier->learn(data);
+
+ std::vector<Item<7> > svs = classifier->getSupportVectors();
+ for(std::size_t i = 0; i < svs.size(); ++i)
+ {
+ const Vecnf<7>& q = svs[i].q;
+ if(svs[i].label)
+ support_transforms_positive.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ else
+ support_transforms_negative.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ }
+
+ break;
+ }
+ case PDT_GENERAL_EULER_BALL:
+ {
+ SVMClassifier<6>* classifier = static_cast<SVMClassifier<6>*>(classifier_);
+ SamplerSE3Euler_ball sampler;
+
+ FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
+ FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
+ Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
+ Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
+
+ sampler.setBound(c1, r1 + margin, r2 + margin, c2);
+
+ std::vector<Item<6> > data(n_samples);
+ for(std::size_t i = 0; i < n_samples; ++i)
+ {
+ Vecnf<6> q = sampler.sample();
+
+ Quaternion3f quat;
+ quat.fromEuler(q[3], q[4], q[5]);
+ Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
+
+ CollisionRequest request;
+ CollisionResult result;
+
+ int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
+ o2->getCollisionGeometry(), tf, request, result);
+
+ data[i] = Item<6>(q, (n_collide > 0));
+ }
+
+ classifier->setScaler(computeScaler(data));
+
+ classifier->learn(data);
+
+ std::vector<Item<6> > svs = classifier->getSupportVectors();
+ for(std::size_t i = 0; i < svs.size(); ++i)
+ {
+ const Vecnf<6>& q = svs[i].q;
+ Quaternion3f quat;
+ quat.fromEuler(q[3], q[4], q[5]);
+ if(svs[i].label)
+ support_transforms_positive.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ else
+ support_transforms_negative.push_back(Transform3f(quat, Vec3f(q[0], q[1], q[2])));
+ }
+
+ break;
+ }
+ case PDT_GENERAL_QUAT_BALL:
+ {
+ SVMClassifier<7>* classifier = static_cast<SVMClassifier<7>*>(classifier_);
+ SamplerSE3Quat_ball sampler;
+
+ FCL_REAL r1 = o1->getCollisionGeometry()->aabb_radius;
+ FCL_REAL r2 = o2->getCollisionGeometry()->aabb_radius;
+ Vec3f c1 = o1->getCollisionGeometry()->aabb_center;
+ Vec3f c2 = o2->getCollisionGeometry()->aabb_center;
+
+ sampler.setBound(c1, r1 + margin, r2 + margin, c2);
+
+ std::vector<Item<7> > data(n_samples);
+ for(std::size_t i = 0; i < n_samples; ++i)
+ {
+ Vecnf<7> q = sampler.sample();
+
+ Quaternion3f quat(q[3], q[4], q[5], q[6]);
+
+ Transform3f tf(quat, Vec3f(q[0], q[1], q[2]));
+
+ CollisionRequest request;
+ CollisionResult result;
+
+ int n_collide = collide(o1->getCollisionGeometry(), Transform3f(),
+ o2->getCollisionGeometry(), tf, request, result);
+
+ data[i] = Item<7>(q, (n_collide > 0));
+ }
+
+ classifier->setScaler(computeScaler(data));
+
+ classifier->learn(data);
+
+ std::vector<Item<7> > svs = classifier->getSupportVectors();
+ for(std::size_t i = 0; i < svs.size(); ++i)
+ {
+ const Vecnf<7>& q = svs[i].q;
+ if(svs[i].label)
+ support_transforms_positive.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ else
+ support_transforms_negative.push_back(Transform3f(Quaternion3f(q[3], q[4], q[5], q[6]), Vec3f(q[0], q[1], q[2])));
+ }
+
+ break;
+ }
+ default:
+ ;
+ }
+
+ // from support transforms compute contact transforms
+
+ NearestNeighbors<Transform3f>* knn_solver_positive = NULL;
+ NearestNeighbors<Transform3f>* knn_solver_negative = NULL;
+
+ switch(knn_solver_type)
+ {
+ case KNN_LINEAR:
+ knn_solver_positive = new NearestNeighborsLinear<Transform3f>();
+ knn_solver_negative = new NearestNeighborsLinear<Transform3f>();
+ break;
+ case KNN_GNAT:
+ knn_solver_positive = new NearestNeighborsGNAT<Transform3f>();
+ knn_solver_negative = new NearestNeighborsGNAT<Transform3f>();
+ break;
+ case KNN_SQRTAPPROX:
+ knn_solver_positive = new NearestNeighborsSqrtApprox<Transform3f>();
+ knn_solver_negative = new NearestNeighborsSqrtApprox<Transform3f>();
+ break;
+ }
+
+
+ knn_solver_positive->setDistanceFunction(distance_func);
+ knn_solver_negative->setDistanceFunction(distance_func);
+
+ knn_solver_positive->add(support_transforms_positive);
+ knn_solver_negative->add(support_transforms_negative);
+
+ std::vector<Transform3f> contact_vectors;
+
+ for(std::size_t i = 0; i < support_transforms_positive.size(); ++i)
+ {
+ std::vector<Transform3f> nbh;
+ knn_solver_negative->nearestK(support_transforms_positive[i], knn_k, nbh);
+
+ for(std::size_t j = 0; j < nbh.size(); ++j)
+ {
+ ContinuousCollisionRequest request;
+ request.ccd_motion_type = CCDM_LINEAR;
+ request.num_max_iterations = 100;
+ ContinuousCollisionResult result;
+ continuousCollide(o1->getCollisionGeometry(), Transform3f(), Transform3f(),
+ o2->getCollisionGeometry(), nbh[j], support_transforms_positive[i],
+ request, result);
+
+ //std::cout << result.time_of_contact << std::endl;
+
+ contact_vectors.push_back(result.contact_tf2);
+ }
+ }
+
+ for(std::size_t i = 0; i < support_transforms_negative.size(); ++i)
+ {
+ std::vector<Transform3f> nbh;
+ knn_solver_positive->nearestK(support_transforms_negative[i], knn_k, nbh);
+
+ for(std::size_t j = 0; j < nbh.size(); ++j)
+ {
+ ContinuousCollisionRequest request;
+ request.ccd_motion_type = CCDM_LINEAR;
+ request.num_max_iterations = 100;
+ ContinuousCollisionResult result;
+ continuousCollide(o1->getCollisionGeometry(), Transform3f(), Transform3f(),
+ o2->getCollisionGeometry(), support_transforms_negative[i], nbh[j],
+ request, result);
+
+ //std::cout << result.time_of_contact << std::endl;
+
+ contact_vectors.push_back(result.contact_tf2);
+ }
+ }
+
+ delete knn_solver_positive;
+ delete knn_solver_negative;
+
+ return contact_vectors;
+
+}
+
+
+
+FCL_REAL penetrationDepth(const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const PenetrationDepthRequest& request,
+ PenetrationDepthResult& result)
+{
+ NearestNeighbors<Transform3f>* knn_solver = NULL;
+ switch(request.knn_solver_type)
+ {
+ case KNN_LINEAR:
+ knn_solver = new NearestNeighborsLinear<Transform3f>();
+ break;
+ case KNN_GNAT:
+ knn_solver = new NearestNeighborsGNAT<Transform3f>();
+ break;
+ case KNN_SQRTAPPROX:
+ knn_solver = new NearestNeighborsSqrtApprox<Transform3f>();
+ break;
+ }
+
+ knn_solver->setDistanceFunction(request.distance_func);
+
+ knn_solver->add(request.contact_vectors);
+
+ Transform3f tf;
+ relativeTransform(tf1, tf2, tf);
+
+ result.resolved_tf = knn_solver->nearest(tf) * tf1;
+ result.pd_value = request.distance_func(result.resolved_tf, tf2);
+ result.pd_value = std::sqrt(result.pd_value);
+
+ return result.pd_value;
+}
+
+FCL_REAL penetrationDepth(const CollisionObject* o1,
+ const CollisionObject* o2,
+ const PenetrationDepthRequest& request,
+ PenetrationDepthResult& result)
+{
+ return penetrationDepth(o1->getCollisionGeometry(), o1->getTransform(),
+ o2->getCollisionGeometry(), o2->getTransform(),
+ request,
+ result);
+}
+
+}