Program Listing for File collision_data.h
↰ Return to documentation for file (/tmp/ws/src/hpp-fcl/include/hpp/fcl/collision_data.h
)
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* 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 Open Source Robotics Foundation 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.
*/
#ifndef HPP_FCL_COLLISION_DATA_H
#define HPP_FCL_COLLISION_DATA_H
#include <vector>
#include <set>
#include <limits>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/config.hh>
#include <hpp/fcl/data_types.h>
#include <hpp/fcl/timings.h>
namespace hpp {
namespace fcl {
struct HPP_FCL_DLLAPI Contact {
const CollisionGeometry* o1;
const CollisionGeometry* o2;
int b1;
int b2;
Vec3f normal;
Vec3f pos;
FCL_REAL penetration_depth;
static const int NONE = -1;
Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
int b2_)
: o1(o1_), o2(o2_), b1(b1_), b2(b2_) {}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_)
: o1(o1_),
o2(o2_),
b1(b1_),
b2(b2_),
normal(normal_),
pos(pos_),
penetration_depth(depth_) {}
bool operator<(const Contact& other) const {
if (b1 == other.b1) return b2 < other.b2;
return b1 < other.b1;
}
bool operator==(const Contact& other) const {
return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
b2 == other.b2 && normal == other.normal && pos == other.pos &&
penetration_depth == other.penetration_depth;
}
bool operator!=(const Contact& other) const { return !(*this == other); }
};
struct QueryResult;
struct HPP_FCL_DLLAPI QueryRequest {
// @briefInitial guess to use for the GJK algorithm
GJKInitialGuess gjk_initial_guess;
HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead")
bool enable_cached_gjk_guess;
GJKVariant gjk_variant;
GJKConvergenceCriterion gjk_convergence_criterion;
GJKConvergenceCriterionType gjk_convergence_criterion_type;
FCL_REAL gjk_tolerance;
size_t gjk_max_iterations;
Vec3f cached_gjk_guess;
support_func_guess_t cached_support_func_guess;
bool enable_timings;
FCL_REAL collision_distance_threshold;
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
QueryRequest()
: gjk_initial_guess(GJKInitialGuess::DefaultGuess),
enable_cached_gjk_guess(false),
gjk_variant(GJKVariant::DefaultGJK),
gjk_convergence_criterion(GJKConvergenceCriterion::VDB),
gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
gjk_tolerance(1e-6),
gjk_max_iterations(128),
cached_gjk_guess(1, 0, 0),
cached_support_func_guess(support_func_guess_t::Zero()),
enable_timings(false),
collision_distance_threshold(
Eigen::NumTraits<FCL_REAL>::dummy_precision()) {}
QueryRequest(const QueryRequest& other) = default;
QueryRequest& operator=(const QueryRequest& other) = default;
HPP_FCL_COMPILER_DIAGNOSTIC_POP
void updateGuess(const QueryResult& result);
inline bool operator==(const QueryRequest& other) const {
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
return gjk_initial_guess == other.gjk_initial_guess &&
enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
cached_gjk_guess == other.cached_gjk_guess &&
cached_support_func_guess == other.cached_support_func_guess &&
enable_timings == other.enable_timings;
HPP_FCL_COMPILER_DIAGNOSTIC_POP
}
};
struct HPP_FCL_DLLAPI QueryResult {
Vec3f cached_gjk_guess;
support_func_guess_t cached_support_func_guess;
CPUTimes timings;
QueryResult()
: cached_gjk_guess(Vec3f::Zero()),
cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
};
inline void QueryRequest::updateGuess(const QueryResult& result) {
if (gjk_initial_guess == GJKInitialGuess::CachedGuess) {
cached_gjk_guess = result.cached_gjk_guess;
cached_support_func_guess = result.cached_support_func_guess;
}
// TODO: use gjk_initial_guess instead
HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
if (enable_cached_gjk_guess) {
cached_gjk_guess = result.cached_gjk_guess;
cached_support_func_guess = result.cached_support_func_guess;
}
HPP_FCL_COMPILER_DIAGNOSTIC_POP
}
struct CollisionResult;
enum CollisionRequestFlag {
CONTACT = 0x00001,
DISTANCE_LOWER_BOUND = 0x00002,
NO_REQUEST = 0x01000
};
struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest {
size_t num_max_contacts;
bool enable_contact;
bool enable_distance_lower_bound;
FCL_REAL security_margin;
FCL_REAL break_distance;
FCL_REAL distance_upper_bound;
CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
: num_max_contacts(num_max_contacts_),
enable_contact(flag & CONTACT),
enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND),
security_margin(0),
break_distance(1e-3),
distance_upper_bound((std::numeric_limits<FCL_REAL>::max)()) {}
CollisionRequest()
: num_max_contacts(1),
enable_contact(false),
enable_distance_lower_bound(false),
security_margin(0),
break_distance(1e-3),
distance_upper_bound((std::numeric_limits<FCL_REAL>::max)()) {}
bool isSatisfied(const CollisionResult& result) const;
inline bool operator==(const CollisionRequest& other) const {
return QueryRequest::operator==(other) &&
num_max_contacts == other.num_max_contacts &&
enable_contact == other.enable_contact &&
enable_distance_lower_bound == other.enable_distance_lower_bound &&
security_margin == other.security_margin &&
break_distance == other.break_distance &&
distance_upper_bound == other.distance_upper_bound;
}
};
struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
private:
std::vector<Contact> contacts;
public:
FCL_REAL distance_lower_bound;
Vec3f nearest_points[2];
public:
CollisionResult()
: distance_lower_bound((std::numeric_limits<FCL_REAL>::max)()) {}
inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) {
if (distance_lower_bound_ < distance_lower_bound)
distance_lower_bound = distance_lower_bound_;
}
inline void addContact(const Contact& c) { contacts.push_back(c); }
inline bool operator==(const CollisionResult& other) const {
return contacts == other.contacts &&
distance_lower_bound == other.distance_lower_bound;
}
bool isCollision() const { return contacts.size() > 0; }
size_t numContacts() const { return contacts.size(); }
const Contact& getContact(size_t i) const {
if (contacts.size() == 0)
throw std::invalid_argument(
"The number of contacts is zero. No Contact can be returned.");
if (i < contacts.size())
return contacts[i];
else
return contacts.back();
}
void setContact(size_t i, const Contact& c) {
if (contacts.size() == 0)
throw std::invalid_argument(
"The number of contacts is zero. No Contact can be returned.");
if (i < contacts.size())
contacts[i] = c;
else
contacts.back() = c;
}
void getContacts(std::vector<Contact>& contacts_) const {
contacts_.resize(contacts.size());
std::copy(contacts.begin(), contacts.end(), contacts_.begin());
}
const std::vector<Contact>& getContacts() const { return contacts; }
void clear() {
distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
contacts.clear();
distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
timings.clear();
}
void swapObjects();
};
struct DistanceResult;
struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest {
bool enable_nearest_points;
FCL_REAL rel_err; // relative error, between 0 and 1
FCL_REAL abs_err; // absolute error
DistanceRequest(bool enable_nearest_points_ = false, FCL_REAL rel_err_ = 0.0,
FCL_REAL abs_err_ = 0.0)
: enable_nearest_points(enable_nearest_points_),
rel_err(rel_err_),
abs_err(abs_err_) {}
bool isSatisfied(const DistanceResult& result) const;
inline bool operator==(const DistanceRequest& other) const {
return QueryRequest::operator==(other) &&
enable_nearest_points == other.enable_nearest_points &&
rel_err == other.rel_err && abs_err == other.abs_err;
}
};
struct HPP_FCL_DLLAPI DistanceResult : QueryResult {
public:
FCL_REAL min_distance;
Vec3f nearest_points[2];
Vec3f normal;
const CollisionGeometry* o1;
const CollisionGeometry* o2;
int b1;
int b2;
static const int NONE = -1;
DistanceResult(
FCL_REAL min_distance_ = (std::numeric_limits<FCL_REAL>::max)())
: min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
const Vec3f nan(
Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
nearest_points[0] = nearest_points[1] = normal = nan;
}
void update(FCL_REAL distance, const CollisionGeometry* o1_,
const CollisionGeometry* o2_, int b1_, int b2_) {
if (min_distance > distance) {
min_distance = distance;
o1 = o1_;
o2 = o2_;
b1 = b1_;
b2 = b2_;
}
}
void update(FCL_REAL distance, const CollisionGeometry* o1_,
const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1,
const Vec3f& p2, const Vec3f& normal_) {
if (min_distance > distance) {
min_distance = distance;
o1 = o1_;
o2 = o2_;
b1 = b1_;
b2 = b2_;
nearest_points[0] = p1;
nearest_points[1] = p2;
normal = normal_;
}
}
void update(const DistanceResult& other_result) {
if (min_distance > other_result.min_distance) {
min_distance = other_result.min_distance;
o1 = other_result.o1;
o2 = other_result.o2;
b1 = other_result.b1;
b2 = other_result.b2;
nearest_points[0] = other_result.nearest_points[0];
nearest_points[1] = other_result.nearest_points[1];
normal = other_result.normal;
}
}
void clear() {
const Vec3f nan(
Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
min_distance = (std::numeric_limits<FCL_REAL>::max)();
o1 = NULL;
o2 = NULL;
b1 = NONE;
b2 = NONE;
nearest_points[0] = nearest_points[1] = normal = nan;
timings.clear();
}
inline bool operator==(const DistanceResult& other) const {
bool is_same = min_distance == other.min_distance &&
nearest_points[0] == other.nearest_points[0] &&
nearest_points[1] == other.nearest_points[1] &&
normal == other.normal && o1 == other.o1 && o2 == other.o2 &&
b1 == other.b1 && b2 == other.b2;
// TODO: check also that two GeometryObject are indeed equal.
if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
is_same &= (o1 == other.o1);
// else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1;
if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
is_same &= (o2 == other.o2);
// else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2;
return is_same;
}
};
namespace internal {
inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
CollisionResult& res,
const FCL_REAL& sqrDistLowerBound) {
// BV cannot find negative distance.
if (res.distance_lower_bound <= 0) return;
FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin;
if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
}
inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
CollisionResult& res,
const FCL_REAL& distance,
const Vec3f& p0, const Vec3f& p1) {
if (distance < res.distance_lower_bound) {
res.distance_lower_bound = distance;
res.nearest_points[0] = p0;
res.nearest_points[1] = p1;
}
}
} // namespace internal
inline CollisionRequestFlag operator~(CollisionRequestFlag a) {
return static_cast<CollisionRequestFlag>(~static_cast<int>(a));
}
inline CollisionRequestFlag operator|(CollisionRequestFlag a,
CollisionRequestFlag b) {
return static_cast<CollisionRequestFlag>(static_cast<int>(a) |
static_cast<int>(b));
}
inline CollisionRequestFlag operator&(CollisionRequestFlag a,
CollisionRequestFlag b) {
return static_cast<CollisionRequestFlag>(static_cast<int>(a) &
static_cast<int>(b));
}
inline CollisionRequestFlag operator^(CollisionRequestFlag a,
CollisionRequestFlag b) {
return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^
static_cast<int>(b));
}
inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a,
CollisionRequestFlag b) {
return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b));
}
inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a,
CollisionRequestFlag b) {
return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b));
}
inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a,
CollisionRequestFlag b) {
return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b));
}
} // namespace fcl
} // namespace hpp
#endif