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 HPP_FCL_COLLISION_DATA_H
#define HPP_FCL_COLLISION_DATA_H
 
#include <vector>
#include <array>
#include <set>
#include <limits>
#include <numeric>
 
#include "hpp/fcl/collision_object.h"
#include "hpp/fcl/config.hh"
#include "hpp/fcl/data_types.h"
#include "hpp/fcl/timings.h"
#include "hpp/fcl/narrowphase/narrowphase_defaults.h"
#include "hpp/fcl/logging.h"
 
namespace hpp {
namespace fcl {
 
struct HPP_FCL_DLLAPI Contact {
  const CollisionGeometry* o1;
 
  const CollisionGeometry* o2;
 
  int b1;
 
  int b2;
 
  Vec3f normal;
 
  std::array<Vec3f, 2> nearest_points;
 
  Vec3f pos;
 
  FCL_REAL penetration_depth;
 
  static const int NONE = -1;
 
  Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
    penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
    nearest_points[0] = nearest_points[1] = normal = pos =
        Vec3f::Constant(std::numeric_limits<FCL_REAL>::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<FCL_REAL>::max)();
    nearest_points[0] = nearest_points[1] = normal = pos =
        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
  }
 
  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
          int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_)
      : o1(o1_),
        o2(o2_),
        b1(b1_),
        b2(b2_),
        normal(normal_),
        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 Vec3f& p1, const Vec3f& p2, const Vec3f& normal_,
          FCL_REAL 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); }
 
  FCL_REAL getDistanceToCollision(const CollisionRequest& request) const;
};
 
struct QueryResult;
 
struct HPP_FCL_DLLAPI QueryRequest {
  // @brief Initial guess to use for the GJK algorithm
  GJKInitialGuess gjk_initial_guess;
 
  HPP_FCL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
  bool enable_cached_gjk_guess;
 
  mutable Vec3f cached_gjk_guess;
 
  mutable support_func_guess_t cached_support_func_guess;
 
  size_t gjk_max_iterations;
 
  FCL_REAL gjk_tolerance;
 
  GJKVariant gjk_variant;
 
  GJKConvergenceCriterion gjk_convergence_criterion;
 
  GJKConvergenceCriterionType gjk_convergence_criterion_type;
 
  size_t epa_max_iterations;
 
  FCL_REAL epa_tolerance;
 
  bool enable_timings;
 
  FCL_REAL collision_distance_threshold;
 
  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
  QueryRequest()
      : gjk_initial_guess(GJKInitialGuess::DefaultGuess),
        enable_cached_gjk_guess(false),
        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<FCL_REAL>::dummy_precision()) {}
 
  QueryRequest(const QueryRequest& other) = default;
 
  QueryRequest& operator=(const QueryRequest& other) = default;
  HPP_FCL_COMPILER_DIAGNOSTIC_POP
 
  void updateGuess(const QueryResult& result) const;
 
  inline bool operator==(const QueryRequest& other) const {
    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    return gjk_initial_guess == other.gjk_initial_guess &&
           enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
           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;
    HPP_FCL_COMPILER_DIAGNOSTIC_POP
  }
};
 
struct HPP_FCL_DLLAPI QueryResult {
  Vec3f cached_gjk_guess;
 
  support_func_guess_t cached_support_func_guess;
 
  CPUTimes timings;
 
  QueryResult()
      : cached_gjk_guess(Vec3f::Zero()),
        cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
};
 
inline void QueryRequest::updateGuess(const QueryResult& result) const {
  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
  HPP_FCL_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;
  }
  HPP_FCL_COMPILER_DIAGNOSTIC_POP
}
 
struct CollisionResult;
 
enum CollisionRequestFlag {
  CONTACT = 0x00001,
  DISTANCE_LOWER_BOUND = 0x00002,
  NO_REQUEST = 0x01000
};
 
struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest {
  size_t num_max_contacts;
 
  bool enable_contact;
 
  HPP_FCL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.)
  bool enable_distance_lower_bound;
 
  FCL_REAL security_margin;
 
  FCL_REAL break_distance;
 
  FCL_REAL distance_upper_bound;
 
  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
  HPP_FCL_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<FCL_REAL>::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<FCL_REAL>::max)()) {}
  HPP_FCL_COMPILER_DIAGNOSTIC_POP
 
  bool isSatisfied(const CollisionResult& result) const;
 
  inline bool operator==(const CollisionRequest& other) const {
    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
    HPP_FCL_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;
    HPP_FCL_COMPILER_DIAGNOSTIC_POP
  }
};
 
inline FCL_REAL Contact::getDistanceToCollision(
    const CollisionRequest& request) const {
  return penetration_depth - request.security_margin;
}
 
struct HPP_FCL_DLLAPI CollisionResult : QueryResult {
 private:
  std::vector<Contact> contacts;
 
 public:
  FCL_REAL distance_lower_bound;
 
  Vec3f normal;
 
  std::array<Vec3f, 2> nearest_points;
 
 public:
  CollisionResult()
      : distance_lower_bound((std::numeric_limits<FCL_REAL>::max)()) {
    nearest_points[0] = nearest_points[1] = normal =
        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
  }
 
  inline void updateDistanceLowerBound(const FCL_REAL& distance_lower_bound_) {
    if (distance_lower_bound_ < distance_lower_bound)
      distance_lower_bound = distance_lower_bound_;
  }
 
  inline void addContact(const Contact& c) { contacts.push_back(c); }
 
  inline bool operator==(const CollisionResult& other) const {
    return contacts == other.contacts &&
           distance_lower_bound == other.distance_lower_bound &&
           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)
      HPP_FCL_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)
      HPP_FCL_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<FCL_REAL>::max)();
    contacts.clear();
    timings.clear();
    nearest_points[0] = nearest_points[1] = normal =
        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
  }
 
  void swapObjects();
};
 
struct HPP_FCL_DLLAPI ContactPatch {
 public:
  using Polygon = std::vector<Vec2f>;
 
  Transform3f tf;
 
  enum PatchDirection { DEFAULT = 0, INVERTED = 1 };
 
  PatchDirection direction;
 
  FCL_REAL 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(Transform3f::Identity()),
        direction(PatchDirection::DEFAULT),
        penetration_depth(0) {
    this->m_points.reserve(preallocated_size);
  }
 
  Vec3f 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 Vec3f& point_3d) {
    const Vec3f point = this->tf.inverseTransform(point_3d);
    this->m_points.emplace_back(point.template head<2>());
  }
 
  Vec3f getPoint(const size_t i) const {
    Vec3f point(0, 0, 0);
    point.head<2>() = this->point(i);
    point = tf.transform(point);
    return point;
  }
 
  Vec3f getPointShape1(const size_t i) const {
    Vec3f point = this->getPoint(i);
    point -= (this->penetration_depth / 2) * this->getNormal();
    return point;
  }
 
  Vec3f getPointShape2(const size_t i) const {
    Vec3f 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; }
 
  Vec2f& point(const size_t i) {
    HPP_FCL_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 Vec2f& point(const size_t i) const {
    HPP_FCL_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 FCL_REAL tol =
                  Eigen::NumTraits<FCL_REAL>::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 Vec3f pi = this->getPoint(i);
      for (size_t j = 0; j < other.size(); ++j) {
        const Vec3f 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 HPP_FCL_DLLAPI ContactPatchRequest {
  size_t max_num_patch;
 
 protected:
  size_t m_num_samples_curved_shapes;
 
  FCL_REAL m_patch_tolerance;
 
 public:
  explicit ContactPatchRequest(size_t max_num_patch = 1,
                               size_t num_samples_curved_shapes =
                                   ContactPatch::default_preallocated_size,
                               FCL_REAL 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,
                               FCL_REAL 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) {
      HPP_FCL_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 FCL_REAL patch_tolerance) {
    if (patch_tolerance < 0) {
      HPP_FCL_LOG_WARNING(
          "`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
          "bugs.");
      this->m_patch_tolerance = Eigen::NumTraits<FCL_REAL>::dummy_precision();
    } else {
      this->m_patch_tolerance = patch_tolerance;
    }
  }
 
  FCL_REAL 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 HPP_FCL_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()) {
      HPP_FCL_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()) {
      HPP_FCL_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()) {
      HPP_FCL_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 HPP_FCL_DLLAPI DistanceRequest : QueryRequest {
  HPP_FCL_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;
 
  FCL_REAL rel_err;  // relative error, between 0 and 1
  FCL_REAL abs_err;  // absolute error
 
  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
  DistanceRequest(bool enable_nearest_points_ = true,
                  bool enable_signed_distance_ = true, FCL_REAL rel_err_ = 0.0,
                  FCL_REAL abs_err_ = 0.0)
      : enable_nearest_points(enable_nearest_points_),
        enable_signed_distance(enable_signed_distance_),
        rel_err(rel_err_),
        abs_err(abs_err_) {}
  HPP_FCL_COMPILER_DIAGNOSTIC_POP
 
  bool isSatisfied(const DistanceResult& result) const;
 
  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
  DistanceRequest& operator=(const DistanceRequest& other) = default;
  HPP_FCL_COMPILER_DIAGNOSTIC_POP
 
  inline bool operator==(const DistanceRequest& other) const {
    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
    HPP_FCL_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;
    HPP_FCL_COMPILER_DIAGNOSTIC_POP
  }
};
 
struct HPP_FCL_DLLAPI DistanceResult : QueryResult {
 public:
  FCL_REAL min_distance;
 
  Vec3f normal;
 
  std::array<Vec3f, 2> nearest_points;
 
  const CollisionGeometry* o1;
 
  const CollisionGeometry* o2;
 
  int b1;
 
  int b2;
 
  static const int NONE = -1;
 
  DistanceResult(
      FCL_REAL min_distance_ = (std::numeric_limits<FCL_REAL>::max)())
      : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
    const Vec3f nan(
        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
    nearest_points[0] = nearest_points[1] = normal = nan;
  }
 
  void update(FCL_REAL distance, const CollisionGeometry* o1_,
              const CollisionGeometry* o2_, int b1_, int b2_) {
    if (min_distance > distance) {
      min_distance = distance;
      o1 = o1_;
      o2 = o2_;
      b1 = b1_;
      b2 = b2_;
    }
  }
 
  void update(FCL_REAL distance, const CollisionGeometry* o1_,
              const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1,
              const Vec3f& p2, const Vec3f& normal_) {
    if (min_distance > distance) {
      min_distance = distance;
      o1 = o1_;
      o2 = o2_;
      b1 = b1_;
      b2 = b2_;
      nearest_points[0] = p1;
      nearest_points[1] = p2;
      normal = normal_;
    }
  }
 
  void update(const DistanceResult& other_result) {
    if (min_distance > other_result.min_distance) {
      min_distance = other_result.min_distance;
      o1 = other_result.o1;
      o2 = other_result.o2;
      b1 = other_result.b1;
      b2 = other_result.b2;
      nearest_points[0] = other_result.nearest_points[0];
      nearest_points[1] = other_result.nearest_points[1];
      normal = other_result.normal;
    }
  }
 
  void clear() {
    const Vec3f nan(
        Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
    min_distance = (std::numeric_limits<FCL_REAL>::max)();
    o1 = NULL;
    o2 = NULL;
    b1 = NONE;
    b2 = NONE;
    nearest_points[0] = nearest_points[1] = normal = nan;
    timings.clear();
  }
 
  inline bool operator==(const DistanceResult& other) const {
    bool is_same = min_distance == other.min_distance &&
                   nearest_points[0] == other.nearest_points[0] &&
                   nearest_points[1] == other.nearest_points[1] &&
                   normal == other.normal && o1 == other.o1 && o2 == other.o2 &&
                   b1 == other.b1 && b2 == other.b2;
 
    // TODO: check also that two GeometryObject are indeed equal.
    if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
    is_same &= (o1 == other.o1);
    //    else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 ==
    //    *other.o1;
 
    if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
    is_same &= (o2 == other.o2);
    //    else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 ==
    //    *other.o2;
 
    return is_same;
  }
};
 
namespace internal {
inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/,
                                           CollisionResult& res,
                                           const FCL_REAL sqrDistLowerBound) {
  // BV cannot find negative distance.
  if (res.distance_lower_bound <= 0) return;
  FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound);  // - req.security_margin;
  if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
}
 
inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&,
                                             CollisionResult& res,
                                             const FCL_REAL& distance,
                                             const Vec3f& p0, const Vec3f& p1,
                                             const Vec3f& 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 fcl
 
}  // namespace hpp
 
#endif