Program Listing for File collision_data.h
↰ Return to documentation for file (include/coal/collision_data.h)
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* Copyright (c) 2024, INRIA
* 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 COAL_COLLISION_DATA_H
#define COAL_COLLISION_DATA_H
#include <vector>
#include <array>
#include <set>
#include <limits>
#include <numeric>
#include "coal/collision_object.h"
#include "coal/config.hh"
#include "coal/data_types.h"
#include "coal/timings.h"
#include "coal/narrowphase/narrowphase_defaults.h"
#include "coal/logging.h"
#include "coal/collision_utility.h"
namespace coal {
struct COAL_DLLAPI Contact {
const CollisionGeometry* o1;
const CollisionGeometry* o2;
int b1;
int b2;
Vec3s normal;
std::array<Vec3s, 2> nearest_points;
Vec3s pos;
Scalar penetration_depth;
static const int NONE = -1;
Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
penetration_depth = (std::numeric_limits<Scalar>::max)();
nearest_points[0] = nearest_points[1] = normal = pos =
Vec3s::Constant(std::numeric_limits<Scalar>::max());
}
Contact(const Contact& other) = default;
Contact& operator=(const Contact& other) = default;
Contact(
const Contact& other,
std::pair<const CollisionGeometry*, const CollisionGeometry*> new_o1_o2)
: Contact(other) {
resolveReferences(new_o1_o2);
}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
int b2_)
: o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
penetration_depth = (std::numeric_limits<Scalar>::max)();
nearest_points[0] = nearest_points[1] = normal = pos =
Vec3s::Constant(std::numeric_limits<Scalar>::max());
}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
int b2_, const Vec3s& pos_, const Vec3s& normal_, Scalar depth_)
: o1(o1_),
o2(o2_),
b1(b1_),
b2(b2_),
normal(normal_),
nearest_points{pos_ - (depth_ * normal_ / 2),
pos_ + (depth_ * normal_ / 2)},
pos(pos_),
penetration_depth(depth_) {}
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_,
Scalar depth_)
: o1(o1_),
o2(o2_),
b1(b1_),
b2(b2_),
normal(normal_),
nearest_points{p1, p2},
pos((p1 + p2) / 2),
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 {
if (this == &other) return true;
COAL_EQUAL_OPERATOR_CHECK(((o1 == nullptr && other.o1 == nullptr) ||
(o1 != nullptr && other.o1 != nullptr)));
COAL_EQUAL_OPERATOR_CHECK(((o2 == nullptr && other.o2 == nullptr) ||
(o2 != nullptr && other.o2 != nullptr)));
if ((o1 && other.o1) && (o1 != other.o1))
COAL_EQUAL_OPERATOR_CHECK(*o1 == *other.o1);
if ((o2 && other.o2) && (o2 != other.o2))
COAL_EQUAL_OPERATOR_CHECK(*o2 == *other.o2);
COAL_EQUAL_OPERATOR_CHECK(b1 == other.b1);
COAL_EQUAL_OPERATOR_CHECK(b2 == other.b2);
COAL_EQUAL_OPERATOR_CHECK(normal == other.normal);
COAL_EQUAL_OPERATOR_CHECK(pos == other.pos);
COAL_EQUAL_OPERATOR_CHECK(nearest_points[0] == other.nearest_points[0]);
COAL_EQUAL_OPERATOR_CHECK(nearest_points[1] == other.nearest_points[1]);
COAL_EQUAL_OPERATOR_CHECK(penetration_depth == other.penetration_depth);
return true;
}
bool operator!=(const Contact& other) const { return !(*this == other); }
Scalar getDistanceToCollision(const CollisionRequest& request) const;
void resolveReferences(
std::pair<const CollisionGeometry*, const CollisionGeometry*> new_o1_o2) {
o1 = new_o1_o2.first;
o2 = new_o1_o2.second;
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
os << prefix
<< " o1 type: " << (o1 ? get_node_type_name(o1->getNodeType()) : "null")
<< ",\n";
os << prefix
<< " o2 type: " << (o2 ? get_node_type_name(o2->getNodeType()) : "null")
<< ",\n";
os << prefix << " b1: " << b1 << ",\n";
os << prefix << " b2: " << b2 << ",\n";
os << prefix << " normal: " << normal.transpose() << ",\n";
os << prefix << " nearest_points[0]: " << nearest_points[0].transpose()
<< ",\n";
os << prefix << " nearest_points[1]: " << nearest_points[1].transpose()
<< ",\n";
os << prefix << " pos: " << pos.transpose() << ",\n";
os << prefix << " penetration_depth: " << penetration_depth << ",\n";
}
friend std::ostream& operator<<(std::ostream& os, const Contact& contact) {
os << "Contact: {" << "\n";
contact.disp(os);
os << "}" << "\n";
return os;
}
};
struct QueryResult;
struct COAL_DLLAPI QueryRequest {
// @brief Initial guess to use for the GJK algorithm
GJKInitialGuess gjk_initial_guess;
COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
bool enable_cached_gjk_guess;
mutable Vec3s cached_gjk_guess;
mutable support_func_guess_t cached_support_func_guess;
size_t gjk_max_iterations;
Scalar gjk_tolerance;
GJKVariant gjk_variant;
GJKConvergenceCriterion gjk_convergence_criterion;
GJKConvergenceCriterionType gjk_convergence_criterion_type;
size_t epa_max_iterations;
Scalar epa_tolerance;
bool enable_timings;
Scalar collision_distance_threshold;
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
QueryRequest()
: gjk_initial_guess(GJKInitialGuess::DefaultGuess),
enable_cached_gjk_guess(false),
cached_gjk_guess(1, 0, 0),
cached_support_func_guess(support_func_guess_t::Zero()),
gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
gjk_tolerance(GJK_DEFAULT_TOLERANCE),
gjk_variant(GJKVariant::DefaultGJK),
gjk_convergence_criterion(GJKConvergenceCriterion::Default),
gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
epa_tolerance(EPA_DEFAULT_TOLERANCE),
enable_timings(false),
collision_distance_threshold(
Eigen::NumTraits<Scalar>::dummy_precision()) {}
QueryRequest(const QueryRequest& other) = default;
QueryRequest& operator=(const QueryRequest& other) = default;
COAL_COMPILER_DIAGNOSTIC_POP
void updateGuess(const QueryResult& result) const;
inline bool operator==(const QueryRequest& other) const {
if (this == &other) return true;
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
COAL_EQUAL_OPERATOR_CHECK(gjk_initial_guess == other.gjk_initial_guess);
COAL_EQUAL_OPERATOR_CHECK(enable_cached_gjk_guess ==
other.enable_cached_gjk_guess);
COAL_EQUAL_OPERATOR_CHECK(gjk_variant == other.gjk_variant);
COAL_EQUAL_OPERATOR_CHECK(gjk_convergence_criterion ==
other.gjk_convergence_criterion);
COAL_EQUAL_OPERATOR_CHECK(gjk_convergence_criterion_type ==
other.gjk_convergence_criterion_type);
COAL_EQUAL_OPERATOR_CHECK(gjk_tolerance == other.gjk_tolerance);
COAL_EQUAL_OPERATOR_CHECK(gjk_max_iterations == other.gjk_max_iterations);
COAL_EQUAL_OPERATOR_CHECK(cached_gjk_guess == other.cached_gjk_guess);
COAL_EQUAL_OPERATOR_CHECK(cached_support_func_guess ==
other.cached_support_func_guess);
COAL_EQUAL_OPERATOR_CHECK(epa_max_iterations == other.epa_max_iterations);
COAL_EQUAL_OPERATOR_CHECK(epa_tolerance == other.epa_tolerance);
COAL_EQUAL_OPERATOR_CHECK(enable_timings == other.enable_timings);
COAL_EQUAL_OPERATOR_CHECK(collision_distance_threshold ==
other.collision_distance_threshold);
COAL_COMPILER_DIAGNOSTIC_POP
return true;
}
inline bool operator!=(const QueryRequest& other) const {
return !(*this == other);
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
os << prefix << " gjk_initial_guess: "
<< (gjk_initial_guess == GJKInitialGuess::DefaultGuess
? "DefaultGuess"
: (gjk_initial_guess == GJKInitialGuess::CachedGuess
? "CachedGuess"
: "BoundingVolumeGuess"))
<< ",\n";
os << prefix << " cached_gjk_guess: " << cached_gjk_guess.transpose()
<< ",\n";
os << prefix << " cached_support_func_guess: "
<< cached_support_func_guess.transpose() << ",\n";
os << prefix << " gjk_max_iterations: " << gjk_max_iterations << ",\n";
os << prefix << " gjk_tolerance: " << gjk_tolerance << ",\n";
os << prefix << " gjk_variant: " << static_cast<int>(gjk_variant) << ",\n";
os << prefix << " gjk_convergence_criterion: "
<< static_cast<int>(gjk_convergence_criterion) << ",\n";
os << prefix << " gjk_convergence_criterion_type: "
<< static_cast<int>(gjk_convergence_criterion_type) << ",\n";
os << prefix << " epa_max_iterations: " << epa_max_iterations << ",\n";
os << prefix << " epa_tolerance: " << epa_tolerance << ",\n";
os << prefix << " enable_timings: " << enable_timings << ",\n";
os << prefix
<< " collision_distance_threshold: " << collision_distance_threshold
<< ",\n";
}
};
struct COAL_DLLAPI QueryResult {
Vec3s cached_gjk_guess;
support_func_guess_t cached_support_func_guess;
CPUTimes timings;
QueryResult()
: cached_gjk_guess(Vec3s::Zero()),
cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
inline bool operator==(const QueryResult& other) const {
if (this == &other) return true;
COAL_EQUAL_OPERATOR_CHECK(cached_gjk_guess == other.cached_gjk_guess);
COAL_EQUAL_OPERATOR_CHECK(cached_support_func_guess ==
other.cached_support_func_guess);
COAL_EQUAL_OPERATOR_CHECK(timings == other.timings);
return true;
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
os << prefix << " cached_gjk_guess: " << cached_gjk_guess.transpose()
<< ",\n";
os << prefix << " cached_support_func_guess: "
<< cached_support_func_guess.transpose() << ",\n";
os << prefix << " timings: " << timings.user << ",\n";
}
};
inline void QueryRequest::updateGuess(const QueryResult& result) const {
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
enable_cached_gjk_guess) {
cached_gjk_guess = result.cached_gjk_guess;
cached_support_func_guess = result.cached_support_func_guess;
}
COAL_COMPILER_DIAGNOSTIC_POP
}
struct CollisionResult;
enum CollisionRequestFlag {
CONTACT = 0x00001,
DISTANCE_LOWER_BOUND = 0x00002,
NO_REQUEST = 0x01000
};
struct COAL_DLLAPI CollisionRequest : QueryRequest {
size_t num_max_contacts;
bool enable_contact;
COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.)
bool enable_distance_lower_bound;
Scalar security_margin;
Scalar break_distance;
Scalar distance_upper_bound;
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
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(Scalar(1e-3)),
distance_upper_bound((std::numeric_limits<Scalar>::max)()) {}
CollisionRequest()
: num_max_contacts(1),
enable_contact(true),
enable_distance_lower_bound(false),
security_margin(0),
break_distance(Scalar(1e-3)),
distance_upper_bound((std::numeric_limits<Scalar>::max)()) {}
COAL_COMPILER_DIAGNOSTIC_POP
bool isSatisfied(const CollisionResult& result) const;
inline bool operator==(const CollisionRequest& other) const {
if (this == &other) return true;
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
COAL_EQUAL_OPERATOR_CHECK((QueryRequest::operator==(other)));
COAL_EQUAL_OPERATOR_CHECK(num_max_contacts == other.num_max_contacts);
COAL_EQUAL_OPERATOR_CHECK(enable_contact == other.enable_contact);
COAL_EQUAL_OPERATOR_CHECK(enable_distance_lower_bound ==
other.enable_distance_lower_bound);
COAL_EQUAL_OPERATOR_CHECK(security_margin == other.security_margin);
COAL_EQUAL_OPERATOR_CHECK(break_distance == other.break_distance);
COAL_EQUAL_OPERATOR_CHECK(distance_upper_bound ==
other.distance_upper_bound);
COAL_COMPILER_DIAGNOSTIC_POP
return true;
}
inline bool operator!=(const CollisionRequest& other) const {
return !(*this == other);
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
QueryRequest::disp(os, prefix);
os << prefix << " num_max_contacts: " << num_max_contacts << ",\n";
os << prefix << " enable_contact: " << enable_contact << ",\n";
os << prefix << " security_margin: " << security_margin << ",\n";
os << prefix << " break_distance: " << break_distance << ",\n";
os << prefix << " distance_upper_bound: " << distance_upper_bound << ",\n";
}
friend std::ostream& operator<<(std::ostream& os,
const CollisionRequest& request) {
os << "CollisionRequest: {" << "\n";
request.disp(os);
os << "}" << "\n";
return os;
}
};
inline Scalar Contact::getDistanceToCollision(
const CollisionRequest& request) const {
return penetration_depth - request.security_margin;
}
struct COAL_DLLAPI CollisionResult : QueryResult {
private:
std::vector<Contact> contacts;
public:
Scalar distance_lower_bound;
Vec3s normal;
std::array<Vec3s, 2> nearest_points;
public:
CollisionResult()
: distance_lower_bound((std::numeric_limits<Scalar>::max)()) {
nearest_points[0] = nearest_points[1] = normal =
Vec3s::Constant(std::numeric_limits<Scalar>::max());
}
CollisionResult(const CollisionResult& other) = default;
CollisionResult& operator=(const CollisionResult& other) = default;
CollisionResult(
const CollisionResult& other,
const std::vector<std::pair<const CollisionGeometry*,
const CollisionGeometry*>>& new_geometries)
: CollisionResult(other) {
resolveReferences(new_geometries);
}
void resolveReferences(
const std::vector<std::pair<const CollisionGeometry*,
const CollisionGeometry*>>& new_geometries) {
COAL_THROW_PRETTY_IF(
contacts.size() != new_geometries.size(), std::invalid_argument,
"The size of the vectors of collision geometries must be equal to "
"the number of contacts in the collision result.");
for (std::size_t i = 0; i < contacts.size(); ++i) {
contacts[i].resolveReferences(new_geometries[i]);
}
}
inline void updateDistanceLowerBound(const Scalar& 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 {
if (this == &other) return true;
COAL_EQUAL_OPERATOR_CHECK((QueryResult::operator==(other)));
COAL_EQUAL_OPERATOR_CHECK(contacts == other.contacts);
COAL_EQUAL_OPERATOR_CHECK(distance_lower_bound ==
other.distance_lower_bound);
COAL_EQUAL_OPERATOR_CHECK(nearest_points[0] == other.nearest_points[0]);
COAL_EQUAL_OPERATOR_CHECK(nearest_points[1] == other.nearest_points[1]);
COAL_EQUAL_OPERATOR_CHECK(normal == other.normal);
return true;
}
bool operator!=(const CollisionResult& other) const {
return !(*this == other);
}
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)
COAL_THROW_PRETTY(
"The number of contacts is zero. No Contact can be returned.",
std::invalid_argument);
if (i < contacts.size())
return contacts[i];
else
return contacts.back();
}
void setContact(size_t i, const Contact& c) {
if (contacts.size() == 0)
COAL_THROW_PRETTY(
"The number of contacts is zero. No Contact can be returned.",
std::invalid_argument);
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<Scalar>::max)();
contacts.clear();
timings.clear();
nearest_points[0] = nearest_points[1] = normal =
Vec3s::Constant(std::numeric_limits<Scalar>::max());
}
void swapObjects();
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
QueryResult::disp(os, prefix);
os << prefix << " distance_lower_bound: " << distance_lower_bound << ",\n";
os << prefix << " normal: " << normal.transpose() << ",\n";
os << prefix << " nearest_points[0]: " << nearest_points[0].transpose()
<< ",\n";
os << prefix << " nearest_points[1]: " << nearest_points[1].transpose()
<< ",\n";
os << prefix << " contacts: {" << "\n";
for (std::size_t i = 0; i < contacts.size(); ++i) {
os << prefix << " Contact " << i << ": {" << "\n";
const Contact& contact = contacts[i];
contact.disp(os, prefix + " ");
os << prefix << " }\n";
}
os << prefix << " }\n";
}
friend std::ostream& operator<<(std::ostream& os,
const CollisionResult& res) {
os << "CollisionResult:" << "\n";
res.disp(os);
os << "}" << "\n";
return os;
}
};
struct COAL_DLLAPI ContactPatch {
public:
using Polygon = std::vector<Vec2s>;
Transform3s tf;
enum PatchDirection { DEFAULT = 0, INVERTED = 1 };
PatchDirection direction;
Scalar penetration_depth;
static constexpr size_t default_preallocated_size = 12;
protected:
std::vector<Vec2s> m_points;
public:
explicit ContactPatch(size_t preallocated_size = default_preallocated_size)
: tf(Transform3s::Identity()),
direction(PatchDirection::DEFAULT),
penetration_depth(0) {
this->m_points.reserve(preallocated_size);
}
Vec3s getNormal() const {
if (this->direction == PatchDirection::INVERTED) {
return -this->tf.rotation().col(2);
}
return this->tf.rotation().col(2);
}
size_t size() const { return this->m_points.size(); }
Scalar computeArea() const {
const size_t n = this->m_points.size();
if (n < 3) {
return Scalar(0);
}
// Shoelace formula for polygon area
// See: https://en.wikipedia.org/wiki/Shoelace_formula
Scalar area = Scalar(0);
for (size_t i = 0; i < n; ++i) {
const size_t j = (i + 1) % n;
area += this->m_points[i](0) * this->m_points[j](1);
area -= this->m_points[j](0) * this->m_points[i](1);
}
return std::abs(area) / Scalar(2);
}
void addPoint(const Vec3s& point_3d) {
const Vec3s point = this->tf.inverseTransform(point_3d);
this->m_points.emplace_back(point.template head<2>());
}
Vec3s getPoint(const size_t i) const {
Vec3s point(0, 0, 0);
point.head<2>() = this->point(i);
point = tf.transform(point);
return point;
}
Vec3s getPointShape1(const size_t i) const {
Vec3s point = this->getPoint(i);
point -= (this->penetration_depth / 2) * this->getNormal();
return point;
}
Vec3s getPointShape2(const size_t i) const {
Vec3s point = this->getPoint(i);
point += (this->penetration_depth / 2) * this->getNormal();
return point;
}
std::vector<Vec2s>& points() { return this->m_points; }
const std::vector<Vec2s>& points() const { return this->m_points; }
Vec2s& point(const size_t i) {
COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
if (i < this->m_points.size()) {
return this->m_points[i];
}
return this->m_points.back();
}
const Vec2s& point(const size_t i) const {
COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
if (i < this->m_points.size()) {
return this->m_points[i];
}
return this->m_points.back();
}
void clear() {
this->m_points.clear();
this->tf.setIdentity();
this->penetration_depth = 0;
}
bool operator==(const ContactPatch& other) const {
if (this == &other) return true;
COAL_EQUAL_OPERATOR_CHECK(tf == other.tf);
COAL_EQUAL_OPERATOR_CHECK(direction == other.direction);
COAL_EQUAL_OPERATOR_CHECK(penetration_depth == other.penetration_depth);
COAL_EQUAL_OPERATOR_CHECK(points() == other.points());
return true;
}
bool operator!=(const ContactPatch& other) const { return !(*this == other); }
bool isSame(
const ContactPatch& other,
const Scalar tol = Eigen::NumTraits<Scalar>::dummy_precision()) const {
// The x and y axis of the set are arbitrary, but the z axis is
// always the normal. The position of the origin of the frame is also
// arbitrary. So we only check if the normals are the same.
if (!this->getNormal().isApprox(other.getNormal(), tol)) {
return false;
}
if (std::abs(this->penetration_depth - other.penetration_depth) > tol) {
return false;
}
if (this->direction != other.direction) {
return false;
}
if (this->size() != other.size()) {
return false;
}
// Check all points of the contact patch.
for (size_t i = 0; i < this->size(); ++i) {
bool found = false;
const Vec3s pi = this->getPoint(i);
for (size_t j = 0; j < other.size(); ++j) {
const Vec3s other_pj = other.getPoint(j);
if (pi.isApprox(other_pj, tol)) {
found = true;
}
}
if (!found) {
return false;
}
}
return true;
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
os << prefix << " tf: {" << "\n";
tf.disp(os, prefix + " ");
os << prefix << " },\n";
os << prefix << " direction: "
<< (direction == PatchDirection::DEFAULT ? "DEFAULT" : "INVERTED")
<< ",\n";
os << prefix << " penetration_depth: " << penetration_depth << ",\n";
os << prefix << " points: {" << "\n";
for (size_t i = 0; i < this->size(); ++i) {
os << prefix << " Point " << i << ": " << this->getPoint(i).transpose()
<< ",\n";
}
os << prefix << " }\n";
}
friend std::ostream& operator<<(std::ostream& os, const ContactPatch& patch) {
os << "ContactPatch: {" << "\n";
patch.disp(os);
os << "}" << "\n";
return os;
}
};
inline void constructContactPatchFrameFromContact(const Contact& contact,
ContactPatch& contact_patch) {
contact_patch.penetration_depth = contact.penetration_depth;
contact_patch.tf.translation() = contact.pos;
contact_patch.tf.rotation() =
constructOrthonormalBasisFromVector(contact.normal);
contact_patch.direction = ContactPatch::PatchDirection::DEFAULT;
}
using SupportSet = ContactPatch;
struct COAL_DLLAPI ContactPatchRequest {
size_t max_num_patch;
protected:
size_t m_num_samples_curved_shapes;
Scalar m_patch_tolerance;
public:
explicit ContactPatchRequest(size_t max_num_patch = 1,
size_t num_samples_curved_shapes =
ContactPatch::default_preallocated_size,
Scalar patch_tolerance = Scalar(1e-3))
: max_num_patch(max_num_patch) {
this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
this->setPatchTolerance(patch_tolerance);
}
explicit ContactPatchRequest(const CollisionRequest& collision_request,
size_t num_samples_curved_shapes =
ContactPatch::default_preallocated_size,
Scalar patch_tolerance = Scalar(1e-3))
: max_num_patch(collision_request.num_max_contacts) {
this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
this->setPatchTolerance(patch_tolerance);
}
void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) {
if (num_samples_curved_shapes < 3) {
COAL_LOG_WARNING(
"`num_samples_curved_shapes` cannot be lower than 3. Setting it to "
"3 to prevent bugs.");
this->m_num_samples_curved_shapes = 3;
} else {
this->m_num_samples_curved_shapes = num_samples_curved_shapes;
}
}
size_t getNumSamplesCurvedShapes() const {
return this->m_num_samples_curved_shapes;
}
void setPatchTolerance(const Scalar patch_tolerance) {
if (patch_tolerance < 0) {
COAL_LOG_WARNING(
"`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
"bugs.");
this->m_patch_tolerance = Eigen::NumTraits<Scalar>::dummy_precision();
} else {
this->m_patch_tolerance = patch_tolerance;
}
}
Scalar getPatchTolerance() const { return this->m_patch_tolerance; }
bool operator==(const ContactPatchRequest& other) const {
if (this == &other) return true;
COAL_EQUAL_OPERATOR_CHECK(max_num_patch == other.max_num_patch);
COAL_EQUAL_OPERATOR_CHECK(getNumSamplesCurvedShapes() ==
other.getNumSamplesCurvedShapes());
COAL_EQUAL_OPERATOR_CHECK(getPatchTolerance() == other.getPatchTolerance());
return true;
}
bool operator!=(const ContactPatchRequest& other) const {
return !(*this == other);
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
os << prefix << " max_num_patch: " << max_num_patch << ",\n";
os << prefix
<< " num_samples_curved_shapes: " << getNumSamplesCurvedShapes()
<< ",\n";
os << prefix << " patch_tolerance: " << getPatchTolerance() << ",\n";
}
friend std::ostream& operator<<(std::ostream& os,
const ContactPatchRequest& request) {
os << "ContactPatchRequest: {" << "\n";
request.disp(os);
os << "}" << "\n";
return os;
}
};
struct COAL_DLLAPI ContactPatchResult {
using ContactPatchVector = std::vector<ContactPatch>;
using ContactPatchRef = std::reference_wrapper<ContactPatch>;
using ContactPatchRefVector = std::vector<ContactPatchRef>;
protected:
ContactPatchVector m_contact_patches_data;
size_t m_id_available_patch;
ContactPatchRefVector m_contact_patches;
public:
ContactPatchResult() : m_id_available_patch(0) {
const size_t max_num_patch = 1;
const ContactPatchRequest request(max_num_patch);
this->set(request);
}
explicit ContactPatchResult(const ContactPatchRequest& request)
: m_id_available_patch(0) {
this->set(request);
};
ContactPatchResult(const ContactPatchResult& other) { *this = other; }
ContactPatchResult(ContactPatchResult&& other) noexcept {
*this = std::move(other);
}
ContactPatchResult& operator=(const ContactPatchResult& other) {
if (this != &other) {
this->m_contact_patches_data = other.m_contact_patches_data;
this->m_id_available_patch = other.m_id_available_patch;
this->m_contact_patches.clear();
for (size_t i = 0; i < this->m_id_available_patch; ++i) {
this->m_contact_patches.emplace_back(this->m_contact_patches_data[i]);
}
}
return *this;
}
ContactPatchResult& operator=(ContactPatchResult&& other) noexcept {
if (this != &other) {
this->m_contact_patches_data = std::move(other.m_contact_patches_data);
this->m_id_available_patch = other.m_id_available_patch;
this->m_contact_patches.clear();
for (size_t i = 0; i < this->m_id_available_patch; ++i) {
this->m_contact_patches.emplace_back(this->m_contact_patches_data[i]);
}
}
return *this;
}
size_t numContactPatches() const { return this->m_contact_patches.size(); }
ContactPatch& getUnusedContactPatch() {
if (this->m_id_available_patch >= this->m_contact_patches_data.size()) {
COAL_LOG_WARNING(
"Trying to get an unused contact patch but all contact patches are "
"used. Increasing size of contact patches vector, at the cost of a "
"copy. You should increase `max_num_patch` in the "
"`ContactPatchRequest` when setting up `ContactPatchResult`.");
bool need_to_reset_references = (this->m_contact_patches_data.size() ==
this->m_contact_patches_data.capacity());
this->m_contact_patches_data.emplace_back(ContactPatch());
if (need_to_reset_references) {
// If we have reached the capacity of the data vector, all references
// in `m_contact_patches` are invalidated. We need to reset them.
this->m_contact_patches.clear();
for (size_t i = 0; i < this->m_id_available_patch; ++i) {
this->m_contact_patches.emplace_back(this->m_contact_patches_data[i]);
}
}
}
ContactPatch& contact_patch =
this->m_contact_patches_data[this->m_id_available_patch];
contact_patch.clear();
this->m_contact_patches.emplace_back(contact_patch);
++(this->m_id_available_patch);
return this->m_contact_patches.back();
}
const ContactPatch& getContactPatch(const size_t i) const {
if (this->m_contact_patches.empty()) {
COAL_THROW_PRETTY(
"The number of contact patches is zero. No ContactPatch can be "
"returned.",
std::invalid_argument);
}
if (i < this->m_contact_patches.size()) {
return this->m_contact_patches[i];
}
return this->m_contact_patches.back();
}
ContactPatch& contactPatch(const size_t i) {
if (this->m_contact_patches.empty()) {
COAL_THROW_PRETTY(
"The number of contact patches is zero. No ContactPatch can be "
"returned.",
std::invalid_argument);
}
if (i < this->m_contact_patches.size()) {
return this->m_contact_patches[i];
}
return this->m_contact_patches.back();
}
void clear() {
this->m_contact_patches.clear();
this->m_id_available_patch = 0;
for (ContactPatch& patch : this->m_contact_patches_data) {
patch.clear();
}
}
void set(const ContactPatchRequest& request) {
if (this->m_contact_patches_data.size() < request.max_num_patch) {
this->m_contact_patches_data.resize(request.max_num_patch);
}
for (ContactPatch& patch : this->m_contact_patches_data) {
patch.points().reserve(request.getNumSamplesCurvedShapes());
}
this->clear();
}
bool check(const ContactPatchRequest& request) const {
assert(this->m_contact_patches_data.size() >= request.max_num_patch);
if (this->m_contact_patches_data.size() < request.max_num_patch) {
return false;
}
for (const ContactPatch& patch : this->m_contact_patches_data) {
if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) {
assert(patch.points().capacity() >=
request.getNumSamplesCurvedShapes());
return false;
}
}
return true;
}
bool operator==(const ContactPatchResult& other) const {
if (this == &other) return true;
COAL_EQUAL_OPERATOR_CHECK(numContactPatches() == other.numContactPatches());
for (size_t i = 0; i < this->numContactPatches(); ++i) {
const ContactPatch& patch = this->getContactPatch(i);
const ContactPatch& other_patch = other.getContactPatch(i);
COAL_EQUAL_OPERATOR_CHECK(patch == other_patch);
}
return true;
}
bool operator!=(const ContactPatchResult& other) const {
return !(*this == other);
}
void swapObjects() {
// Create new transform: it's the reflection of `tf` along the z-axis.
// This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
// becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
for (size_t i = 0; i < this->numContactPatches(); ++i) {
ContactPatch& patch = this->contactPatch(i);
patch.tf.rotation().col(0) *= -1.0;
patch.tf.rotation().col(2) *= -1.0;
for (size_t j = 0; j < patch.size(); ++j) {
patch.point(i)(0) *= Scalar(-1); // only invert the x-axis
}
}
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
os << prefix << " numContactPatches: " << this->numContactPatches()
<< ",\n";
os << prefix << " contact_patches: {" << "\n";
for (size_t i = 0; i < this->numContactPatches(); ++i) {
os << prefix << " ContactPatch " << i << ": {" << "\n";
const ContactPatch& patch = this->getContactPatch(i);
patch.disp(os, prefix + " ");
os << prefix << " }\n";
}
os << prefix << " }\n";
}
friend std::ostream& operator<<(std::ostream& os,
const ContactPatchResult& result) {
os << "ContactPatchResult: {" << "\n";
result.disp(os);
os << "}" << "\n";
return os;
}
};
struct DistanceResult;
struct COAL_DLLAPI DistanceRequest : QueryRequest {
COAL_DEPRECATED_MESSAGE(
Nearest points are always computed
: they are the points of the shapes that achieve a distance of
`DistanceResult::min_distance`
.\n Use `enable_signed_distance` if you want to compute a signed
minimum distance(and thus its corresponding nearest points)
.)
bool enable_nearest_points;
bool enable_signed_distance;
Scalar rel_err; // relative error, between 0 and 1
Scalar abs_err; // absolute error
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
DistanceRequest(bool enable_nearest_points_ = true,
bool enable_signed_distance_ = true, Scalar rel_err_ = 0.0,
Scalar abs_err_ = 0.0)
: enable_nearest_points(enable_nearest_points_),
enable_signed_distance(enable_signed_distance_),
rel_err(rel_err_),
abs_err(abs_err_) {}
COAL_COMPILER_DIAGNOSTIC_POP
bool isSatisfied(const DistanceResult& result) const;
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
DistanceRequest(const DistanceRequest& other) = default;
DistanceRequest& operator=(const DistanceRequest& other) = default;
COAL_COMPILER_DIAGNOSTIC_POP
inline bool operator==(const DistanceRequest& other) const {
if (this == &other) return true;
COAL_COMPILER_DIAGNOSTIC_PUSH
COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
COAL_EQUAL_OPERATOR_CHECK((QueryRequest::operator==(other)));
COAL_EQUAL_OPERATOR_CHECK(enable_nearest_points ==
other.enable_nearest_points);
COAL_EQUAL_OPERATOR_CHECK(enable_signed_distance ==
other.enable_signed_distance);
COAL_EQUAL_OPERATOR_CHECK(rel_err == other.rel_err);
COAL_EQUAL_OPERATOR_CHECK(abs_err == other.abs_err);
COAL_COMPILER_DIAGNOSTIC_POP
return true;
}
inline bool operator!=(const DistanceRequest& other) const {
return !(*this == other);
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
QueryRequest::disp(os, prefix);
os << prefix << " enable_signed_distance: " << enable_signed_distance
<< ",\n";
os << prefix << " rel_err: " << rel_err << ",\n";
os << prefix << " abs_err: " << abs_err << ",\n";
}
friend std::ostream& operator<<(std::ostream& os,
const DistanceRequest& request) {
os << "DistanceRequest: {" << "\n";
request.disp(os);
os << "}" << "\n";
return os;
}
};
struct COAL_DLLAPI DistanceResult : QueryResult {
public:
Scalar min_distance;
Vec3s normal;
std::array<Vec3s, 2> nearest_points;
const CollisionGeometry* o1;
const CollisionGeometry* o2;
int b1;
int b2;
static const int NONE = -1;
DistanceResult(Scalar min_distance_ = (std::numeric_limits<Scalar>::max)())
: min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
const Vec3s inf(Vec3s::Constant(std::numeric_limits<Scalar>::max()));
nearest_points[0] = nearest_points[1] = normal = inf;
}
DistanceResult(const DistanceResult& other) = default;
DistanceResult& operator=(const DistanceResult& other) = default;
DistanceResult(
const DistanceResult& other,
std::pair<const CollisionGeometry*, const CollisionGeometry*> new_o1_o2)
: DistanceResult(other) {
resolveReferences(new_o1_o2);
}
void resolveReferences(
std::pair<const CollisionGeometry*, const CollisionGeometry*> new_o1_o2) {
o1 = new_o1_o2.first;
o2 = new_o1_o2.second;
}
void update(Scalar 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(Scalar distance, const CollisionGeometry* o1_,
const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1,
const Vec3s& p2, const Vec3s& 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 Vec3s inf(Vec3s::Constant(std::numeric_limits<Scalar>::max()));
min_distance = (std::numeric_limits<Scalar>::max)();
o1 = NULL;
o2 = NULL;
b1 = NONE;
b2 = NONE;
nearest_points[0] = nearest_points[1] = normal = inf;
timings.clear();
}
inline bool operator==(const DistanceResult& other) const {
if (this == &other) return true;
COAL_EQUAL_OPERATOR_CHECK((QueryResult::operator==(other)));
COAL_EQUAL_OPERATOR_CHECK(min_distance == other.min_distance);
COAL_EQUAL_OPERATOR_CHECK(nearest_points[0] == other.nearest_points[0]);
COAL_EQUAL_OPERATOR_CHECK(nearest_points[1] == other.nearest_points[1]);
COAL_EQUAL_OPERATOR_CHECK(normal == other.normal);
COAL_EQUAL_OPERATOR_CHECK(((o1 == nullptr && other.o1 == nullptr) ||
(o1 != nullptr && other.o1 != nullptr)));
COAL_EQUAL_OPERATOR_CHECK(((o2 == nullptr && other.o2 == nullptr) ||
(o2 != nullptr && other.o2 != nullptr)));
if ((o1 && other.o1) && (o1 != other.o1))
COAL_EQUAL_OPERATOR_CHECK(*o1 == *other.o1);
if ((o2 && other.o2) && (o2 != other.o2))
COAL_EQUAL_OPERATOR_CHECK(*o2 == *other.o2);
COAL_EQUAL_OPERATOR_CHECK(b1 == other.b1);
COAL_EQUAL_OPERATOR_CHECK(b2 == other.b2);
return true;
}
inline bool operator!=(const DistanceResult& other) const {
return !(*this == other);
}
void disp(std::ostream& os, const std::string& prefix = "") const {
using namespace std;
QueryResult::disp(os, prefix);
os << prefix
<< " o1: " << (o1 ? get_node_type_name(o1->getNodeType()) : "null")
<< ",\n";
os << prefix
<< " o2: " << (o2 ? get_node_type_name(o2->getNodeType()) : "null")
<< ",\n";
os << prefix << " b1: " << b1 << ",\n";
os << prefix << " b2: " << b2 << ",\n";
os << prefix << " min_distance: " << min_distance << ",\n";
os << prefix << " normal: " << normal.transpose() << ",\n";
os << prefix << " nearest_points[0]: " << nearest_points[0].transpose()
<< ",\n";
os << prefix << " nearest_points[1]: " << nearest_points[1].transpose()
<< ",\n";
}
friend std::ostream& operator<<(std::ostream& os,
const DistanceResult& result) {
os << "DistanceResult: {" << "\n";
result.disp(os);
os << "}" << "\n";
return os;
}
};
namespace internal {
inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
CollisionResult& res,
const Scalar sqrDistLowerBound) {
// BV cannot find negative distance.
if (res.distance_lower_bound <= 0) return;
Scalar 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 Scalar& distance,
const Vec3s& p0, const Vec3s& p1,
const Vec3s& normal) {
if (distance < res.distance_lower_bound) {
res.distance_lower_bound = distance;
res.nearest_points[0] = p0;
res.nearest_points[1] = p1;
res.normal = normal;
}
}
} // 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 coal
#endif