collision_data.h

Go to the documentation of this file.

/*
 * 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"

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;

 CoalScalar penetration_depth;

 static const int NONE = -1;

 Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
 penetration_depth = (std::numeric_limits<CoalScalar>::max)();
 nearest_points[0] = nearest_points[1] = normal = pos =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 }

 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
 int b2_)
 : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
 penetration_depth = (std::numeric_limits<CoalScalar>::max)();
 nearest_points[0] = nearest_points[1] = normal = pos =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 }

 Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
 int b2_, const Vec3s& pos_, const Vec3s& normal_, CoalScalar 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_,
 CoalScalar 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 {
 return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
 b2 == other.b2 && normal == other.normal && pos == other.pos &&
 nearest_points[0] == other.nearest_points[0] &&
 nearest_points[1] == other.nearest_points[1] &&
 penetration_depth == other.penetration_depth;
 }

 bool operator!=(const Contact& other) const { return !(*this == other); }

 CoalScalar getDistanceToCollision(const CollisionRequest& request) const;
};

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;

 CoalScalar gjk_tolerance;

 GJKVariant gjk_variant;

 GJKConvergenceCriterion gjk_convergence_criterion;

 GJKConvergenceCriterionType gjk_convergence_criterion_type;

 size_t epa_max_iterations;

 CoalScalar epa_tolerance;

 bool enable_timings;

 CoalScalar 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<CoalScalar>::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 {
 COAL_COMPILER_DIAGNOSTIC_PUSH
 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 return gjk_initial_guess == other.gjk_initial_guess &&
 enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
 gjk_variant == other.gjk_variant &&
 gjk_convergence_criterion == other.gjk_convergence_criterion &&
 gjk_convergence_criterion_type ==
 other.gjk_convergence_criterion_type &&
 gjk_tolerance == other.gjk_tolerance &&
 gjk_max_iterations == other.gjk_max_iterations &&
 cached_gjk_guess == other.cached_gjk_guess &&
 cached_support_func_guess == other.cached_support_func_guess &&
 epa_max_iterations == other.epa_max_iterations &&
 epa_tolerance == other.epa_tolerance &&
 enable_timings == other.enable_timings &&
 collision_distance_threshold == other.collision_distance_threshold;
 COAL_COMPILER_DIAGNOSTIC_POP
 }
};

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 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;

 CoalScalar security_margin;

 CoalScalar break_distance;

 CoalScalar 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(1e-3),
 distance_upper_bound((std::numeric_limits<CoalScalar>::max)()) {}

 CollisionRequest()
 : num_max_contacts(1),
 enable_contact(true),
 enable_distance_lower_bound(false),
 security_margin(0),
 break_distance(1e-3),
 distance_upper_bound((std::numeric_limits<CoalScalar>::max)()) {}
 COAL_COMPILER_DIAGNOSTIC_POP

 bool isSatisfied(const CollisionResult& result) const;

 inline bool operator==(const CollisionRequest& other) const {
 COAL_COMPILER_DIAGNOSTIC_PUSH
 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 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;
 COAL_COMPILER_DIAGNOSTIC_POP
 }
};

inline CoalScalar Contact::getDistanceToCollision(
 const CollisionRequest& request) const {
 return penetration_depth - request.security_margin;
}

struct COAL_DLLAPI CollisionResult : QueryResult {
 private:
 std::vector<Contact> contacts;

 public:
 CoalScalar distance_lower_bound;

 Vec3s normal;

 std::array<Vec3s, 2> nearest_points;

 public:
 CollisionResult()
 : distance_lower_bound((std::numeric_limits<CoalScalar>::max)()) {
 nearest_points[0] = nearest_points[1] = normal =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 }

 inline void updateDistanceLowerBound(
 const CoalScalar& 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 &&
 nearest_points[0] == other.nearest_points[0] &&
 nearest_points[1] == other.nearest_points[1] &&
 normal == other.normal;
 }

 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<CoalScalar>::max)();
 contacts.clear();
 timings.clear();
 nearest_points[0] = nearest_points[1] = normal =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 }

 void swapObjects();
};

struct COAL_DLLAPI ContactPatch {
 public:
 using Polygon = std::vector<Vec2s>;

 Transform3s tf;

 enum PatchDirection { DEFAULT = 0, INVERTED = 1 };

 PatchDirection direction;

 CoalScalar penetration_depth;

 static constexpr size_t default_preallocated_size = 12;

 protected:
 Polygon 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(); }

 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;
 }

 Polygon& points() { return this->m_points; }

 const Polygon& 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 {
 return this->tf == other.tf && this->direction == other.direction &&
 this->penetration_depth == other.penetration_depth &&
 this->points() == other.points();
 }

 bool isSame(const ContactPatch& other,
 const CoalScalar tol =
 Eigen::NumTraits<CoalScalar>::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;
 }
};

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;

 CoalScalar m_patch_tolerance;

 public:
 explicit ContactPatchRequest(size_t max_num_patch = 1,
 size_t num_samples_curved_shapes =
 ContactPatch::default_preallocated_size,
 CoalScalar patch_tolerance = 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,
 CoalScalar patch_tolerance = 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 CoalScalar 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<CoalScalar>::dummy_precision();
 } else {
 this->m_patch_tolerance = patch_tolerance;
 }
 }

 CoalScalar getPatchTolerance() const { return this->m_patch_tolerance; }

 bool operator==(const ContactPatchRequest& other) const {
 return this->max_num_patch == other.max_num_patch &&
 this->getNumSamplesCurvedShapes() ==
 other.getNumSamplesCurvedShapes() &&
 this->getPatchTolerance() == other.getPatchTolerance();
 }
};

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);
 };

 size_t numContactPatches() const { return this->m_contact_patches.size(); }

 ContactPatchRef 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`.");
 this->m_contact_patches_data.emplace_back(
 this->m_contact_patches_data.back());
 this->m_contact_patches_data.back().clear();
 }
 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->numContactPatches() != other.numContactPatches()) {
 return false;
 }

 for (size_t i = 0; i < this->numContactPatches(); ++i) {
 const ContactPatch& patch = this->getContactPatch(i);
 const ContactPatch& other_patch = other.getContactPatch(i);
 if (!(patch == other_patch)) {
 return false;
 }
 }

 return true;
 }

 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) *= -1.0; // only invert the x-axis
 }
 }
 }
};

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;

 CoalScalar rel_err; // relative error, between 0 and 1
 CoalScalar 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,
 CoalScalar rel_err_ = 0.0, CoalScalar 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& operator=(const DistanceRequest& other) = default;
 COAL_COMPILER_DIAGNOSTIC_POP

 inline bool operator==(const DistanceRequest& other) const {
 COAL_COMPILER_DIAGNOSTIC_PUSH
 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 return QueryRequest::operator==(other) &&
 enable_nearest_points == other.enable_nearest_points &&
 enable_signed_distance == other.enable_signed_distance &&
 rel_err == other.rel_err && abs_err == other.abs_err;
 COAL_COMPILER_DIAGNOSTIC_POP
 }
};

struct COAL_DLLAPI DistanceResult : QueryResult {
 public:
 CoalScalar 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(
 CoalScalar min_distance_ = (std::numeric_limits<CoalScalar>::max)())
 : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
 const Vec3s nan(
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
 nearest_points[0] = nearest_points[1] = normal = nan;
 }

 void update(CoalScalar 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(CoalScalar 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 nan(
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
 min_distance = (std::numeric_limits<CoalScalar>::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 CoalScalar sqrDistLowerBound) {
 // BV cannot find negative distance.
 if (res.distance_lower_bound <= 0) return;
 CoalScalar 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 CoalScalar& 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