Commit de8666f6 authored by panjia1983's avatar panjia1983
Browse files

penetration depth

parent 89887e9c
......@@ -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})
......
# 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)
# 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
......@@ -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
{
......
......@@ -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;
......
......@@ -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_),
......@@ -468,34 +472,44 @@ 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)
{
}
};
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;
};
struct WeightEuclideanPDMetric : public PenetrationDepthMetricBase
{
enum KNNSolverType {KNN_LINEAR, KNN_GNAT, KNN_SQRTAPPROX};
};
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;
};
......
......@@ -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
......
......@@ -42,5 +42,7 @@
#cmakedefine01 FCL_HAVE_SSE
#cmakedefine01 FCL_HAVE_OCTOMAP
#cmakedefine01 FCL_HAVE_FLANN
#cmakedefine01 FCL_HAVE_TINYXML
#endif
......@@ -9,12 +9,12 @@ namespace fcl
{
/// @brief continous collision checking between two objects
FCL_REAL continuous_collide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end,
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,
FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end,
const CollisionObject* o2, const Transform3f& tf2_end,
const ContinuousCollisionRequest& request,
ContinuousCollisionResult& result);
......
#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
/*********************************************************************
* 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
/*********************************************************************
* 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
This diff is collapsed.
/*********************************************************************
* 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