.. _program_listing_file_include_coal_collision_data.h:

Program Listing for File collision_data.h
=========================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_coal_collision_data.h>` (``include/coal/collision_data.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * 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;
   
     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>::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<Scalar>::max)();
       nearest_points[0] = nearest_points[1] = normal = pos =
           Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
     }
   
     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 {
       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); }
   
     Scalar 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;
   
     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 {
       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;
   
     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 {
       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 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>::quiet_NaN());
     }
   
     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 {
       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<Scalar>::max)();
       contacts.clear();
       timings.clear();
       nearest_points[0] = nearest_points[1] = normal =
           Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
     }
   
     void swapObjects();
   };
   
   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:
     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 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;
     }
   };
   
   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 {
       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) *= Scalar(-1);  // 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;
   
     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& 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:
     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 nan(Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
       nearest_points[0] = nearest_points[1] = normal = nan;
     }
   
     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 nan(Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
       min_distance = (std::numeric_limits<Scalar>::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 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