coal  3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, fork of FCL -- The Flexible Collision Library
collision_data.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * Copyright (c) 2024, INRIA
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
39 #ifndef COAL_COLLISION_DATA_H
40 #define COAL_COLLISION_DATA_H
41 
42 #include <vector>
43 #include <array>
44 #include <set>
45 #include <limits>
46 #include <numeric>
47 
48 #include "coal/collision_object.h"
49 #include "coal/config.hh"
50 #include "coal/data_types.h"
51 #include "coal/timings.h"
53 #include "coal/logging.h"
54 
55 namespace coal {
56 
61 
64 
69  int b1;
70 
75  int b2;
76 
89 
99  std::array<Vec3s, 2> nearest_points;
100 
103 
106 
108  static const int NONE = -1;
109 
111  Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
112  penetration_depth = (std::numeric_limits<Scalar>::max)();
113  nearest_points[0] = nearest_points[1] = normal = pos =
114  Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
115  }
116 
117  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
118  int b2_)
119  : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {
120  penetration_depth = (std::numeric_limits<Scalar>::max)();
121  nearest_points[0] = nearest_points[1] = normal = pos =
122  Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
123  }
124 
125  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
126  int b2_, const Vec3s& pos_, const Vec3s& normal_, Scalar depth_)
127  : o1(o1_),
128  o2(o2_),
129  b1(b1_),
130  b2(b2_),
131  normal(normal_),
132  nearest_points{pos_ - (depth_ * normal_ / 2),
133  pos_ + (depth_ * normal_ / 2)},
134  pos(pos_),
135  penetration_depth(depth_) {}
136 
137  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_,
138  int b2_, const Vec3s& p1, const Vec3s& p2, const Vec3s& normal_,
139  Scalar depth_)
140  : o1(o1_),
141  o2(o2_),
142  b1(b1_),
143  b2(b2_),
144  normal(normal_),
145  nearest_points{p1, p2},
146  pos((p1 + p2) / 2),
147  penetration_depth(depth_) {}
148 
149  bool operator<(const Contact& other) const {
150  if (b1 == other.b1) return b2 < other.b2;
151  return b1 < other.b1;
152  }
153 
154  bool operator==(const Contact& other) const {
155  return o1 == other.o1 && o2 == other.o2 && b1 == other.b1 &&
156  b2 == other.b2 && normal == other.normal && pos == other.pos &&
157  nearest_points[0] == other.nearest_points[0] &&
158  nearest_points[1] == other.nearest_points[1] &&
159  penetration_depth == other.penetration_depth;
160  }
161 
162  bool operator!=(const Contact& other) const { return !(*this == other); }
163 
164  Scalar getDistanceToCollision(const CollisionRequest& request) const;
165 };
166 
167 struct QueryResult;
168 
171  // @brief Initial guess to use for the GJK algorithm
173 
176  COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
177  bool enable_cached_gjk_guess;
178 
180  mutable Vec3s cached_gjk_guess;
181 
183  mutable support_func_guess_t cached_support_func_guess;
184 
186  size_t gjk_max_iterations;
187 
192  Scalar gjk_tolerance;
193 
195  GJKVariant gjk_variant;
196 
198  GJKConvergenceCriterion gjk_convergence_criterion;
199 
201  GJKConvergenceCriterionType gjk_convergence_criterion_type;
202 
204  size_t epa_max_iterations;
205 
211  Scalar epa_tolerance;
212 
214  bool enable_timings;
215 
217  Scalar collision_distance_threshold;
218 
223  : gjk_initial_guess(GJKInitialGuess::DefaultGuess),
224  enable_cached_gjk_guess(false),
225  cached_gjk_guess(1, 0, 0),
226  cached_support_func_guess(support_func_guess_t::Zero()),
227  gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
228  gjk_tolerance(GJK_DEFAULT_TOLERANCE),
229  gjk_variant(GJKVariant::DefaultGJK),
230  gjk_convergence_criterion(GJKConvergenceCriterion::Default),
231  gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative),
232  epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
233  epa_tolerance(EPA_DEFAULT_TOLERANCE),
234  enable_timings(false),
235  collision_distance_threshold(
236  Eigen::NumTraits<Scalar>::dummy_precision()) {}
237 
239  QueryRequest(const QueryRequest& other) = default;
240 
242  QueryRequest& operator=(const QueryRequest& other) = default;
244 
250  void updateGuess(const QueryResult& result) const;
251 
253  inline bool operator==(const QueryRequest& other) const {
256  return gjk_initial_guess == other.gjk_initial_guess &&
257  enable_cached_gjk_guess == other.enable_cached_gjk_guess &&
258  gjk_variant == other.gjk_variant &&
259  gjk_convergence_criterion == other.gjk_convergence_criterion &&
260  gjk_convergence_criterion_type ==
262  gjk_tolerance == other.gjk_tolerance &&
263  gjk_max_iterations == other.gjk_max_iterations &&
264  cached_gjk_guess == other.cached_gjk_guess &&
265  cached_support_func_guess == other.cached_support_func_guess &&
266  epa_max_iterations == other.epa_max_iterations &&
267  epa_tolerance == other.epa_tolerance &&
268  enable_timings == other.enable_timings &&
269  collision_distance_threshold == other.collision_distance_threshold;
271  }
272 };
273 
278 
281 
284 
286  : cached_gjk_guess(Vec3s::Zero()),
287  cached_support_func_guess(support_func_guess_t::Constant(-1)) {}
288 };
289 
290 inline void QueryRequest::updateGuess(const QueryResult& result) const {
297  }
299 }
300 
301 struct CollisionResult;
302 
305  CONTACT = 0x00001,
307  NO_REQUEST = 0x01000
308 };
309 
314 
318 
320  COAL_DEPRECATED_MESSAGE(A lower bound on distance is always computed.)
321  bool enable_distance_lower_bound;
322 
328  Scalar security_margin;
329 
332  Scalar break_distance;
333 
340  Scalar distance_upper_bound;
341 
349  CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_)
350  : num_max_contacts(num_max_contacts_),
351  enable_contact(flag & CONTACT),
352  enable_distance_lower_bound(flag & DISTANCE_LOWER_BOUND),
353  security_margin(0),
354  break_distance(Scalar(1e-3)),
355  distance_upper_bound((std::numeric_limits<Scalar>::max)()) {}
356 
359  : num_max_contacts(1),
360  enable_contact(true),
361  enable_distance_lower_bound(false),
362  security_margin(0),
363  break_distance(Scalar(1e-3)),
364  distance_upper_bound((std::numeric_limits<Scalar>::max)()) {}
366 
367  bool isSatisfied(const CollisionResult& result) const;
368 
370  inline bool operator==(const CollisionRequest& other) const {
373  return QueryRequest::operator==(other) &&
374  num_max_contacts == other.num_max_contacts &&
375  enable_contact == other.enable_contact &&
376  enable_distance_lower_bound == other.enable_distance_lower_bound &&
377  security_margin == other.security_margin &&
378  break_distance == other.break_distance &&
379  distance_upper_bound == other.distance_upper_bound;
381  }
382 };
383 
385  const CollisionRequest& request) const {
386  return penetration_depth - request.security_margin;
387 }
388 
391  private:
393  std::vector<Contact> contacts;
394 
395  public:
402 
406 
414  std::array<Vec3s, 2> nearest_points;
415 
416  public:
418  : distance_lower_bound((std::numeric_limits<Scalar>::max)()) {
419  nearest_points[0] = nearest_points[1] = normal =
420  Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
421  }
422 
424  inline void updateDistanceLowerBound(const Scalar& distance_lower_bound_) {
425  if (distance_lower_bound_ < distance_lower_bound)
426  distance_lower_bound = distance_lower_bound_;
427  }
428 
430  inline void addContact(const Contact& c) { contacts.push_back(c); }
431 
433  inline bool operator==(const CollisionResult& other) const {
434  return contacts == other.contacts &&
435  distance_lower_bound == other.distance_lower_bound &&
436  nearest_points[0] == other.nearest_points[0] &&
437  nearest_points[1] == other.nearest_points[1] &&
438  normal == other.normal;
439  }
440 
442  bool isCollision() const { return contacts.size() > 0; }
443 
445  size_t numContacts() const { return contacts.size(); }
446 
448  const Contact& getContact(size_t i) const {
449  if (contacts.size() == 0)
451  "The number of contacts is zero. No Contact can be returned.",
452  std::invalid_argument);
453 
454  if (i < contacts.size())
455  return contacts[i];
456  else
457  return contacts.back();
458  }
459 
461  void setContact(size_t i, const Contact& c) {
462  if (contacts.size() == 0)
464  "The number of contacts is zero. No Contact can be returned.",
465  std::invalid_argument);
466 
467  if (i < contacts.size())
468  contacts[i] = c;
469  else
470  contacts.back() = c;
471  }
472 
474  void getContacts(std::vector<Contact>& contacts_) const {
475  contacts_.resize(contacts.size());
476  std::copy(contacts.begin(), contacts.end(), contacts_.begin());
477  }
478 
479  const std::vector<Contact>& getContacts() const { return contacts; }
480 
482  void clear() {
483  distance_lower_bound = (std::numeric_limits<Scalar>::max)();
484  contacts.clear();
485  timings.clear();
486  nearest_points[0] = nearest_points[1] = normal =
487  Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
488  }
489 
492  void swapObjects();
493 };
494 
512  public:
513  using Polygon = std::vector<Vec2s>;
514 
518 
528  enum PatchDirection { DEFAULT = 0, INVERTED = 1 };
529 
532 
545 
548  static constexpr size_t default_preallocated_size = 12;
549 
550  protected:
553 
554  public:
560  explicit ContactPatch(size_t preallocated_size = default_preallocated_size)
561  : tf(Transform3s::Identity()),
562  direction(PatchDirection::DEFAULT),
563  penetration_depth(0) {
564  this->m_points.reserve(preallocated_size);
565  }
566 
568  Vec3s getNormal() const {
569  if (this->direction == PatchDirection::INVERTED) {
570  return -this->tf.rotation().col(2);
571  }
572  return this->tf.rotation().col(2);
573  }
574 
576  size_t size() const { return this->m_points.size(); }
577 
583  void addPoint(const Vec3s& point_3d) {
584  const Vec3s point = this->tf.inverseTransform(point_3d);
585  this->m_points.emplace_back(point.template head<2>());
586  }
587 
589  Vec3s getPoint(const size_t i) const {
590  Vec3s point(0, 0, 0);
591  point.head<2>() = this->point(i);
592  point = tf.transform(point);
593  return point;
594  }
595 
599  Vec3s getPointShape1(const size_t i) const {
600  Vec3s point = this->getPoint(i);
601  point -= (this->penetration_depth / 2) * this->getNormal();
602  return point;
603  }
604 
608  Vec3s getPointShape2(const size_t i) const {
609  Vec3s point = this->getPoint(i);
610  point += (this->penetration_depth / 2) * this->getNormal();
611  return point;
612  }
613 
615  Polygon& points() { return this->m_points; }
616 
618  const Polygon& points() const { return this->m_points; }
619 
621  Vec2s& point(const size_t i) {
622  COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
623  if (i < this->m_points.size()) {
624  return this->m_points[i];
625  }
626  return this->m_points.back();
627  }
628 
630  const Vec2s& point(const size_t i) const {
631  COAL_ASSERT(this->m_points.size() > 0, "Patch is empty.", std::logic_error);
632  if (i < this->m_points.size()) {
633  return this->m_points[i];
634  }
635  return this->m_points.back();
636  }
637 
639  void clear() {
640  this->m_points.clear();
641  this->tf.setIdentity();
642  this->penetration_depth = 0;
643  }
644 
649  bool operator==(const ContactPatch& other) const {
650  return this->tf == other.tf && this->direction == other.direction &&
651  this->penetration_depth == other.penetration_depth &&
652  this->points() == other.points();
653  }
654 
657  bool isSame(
658  const ContactPatch& other,
659  const Scalar tol = Eigen::NumTraits<Scalar>::dummy_precision()) const {
660  // The x and y axis of the set are arbitrary, but the z axis is
661  // always the normal. The position of the origin of the frame is also
662  // arbitrary. So we only check if the normals are the same.
663  if (!this->getNormal().isApprox(other.getNormal(), tol)) {
664  return false;
665  }
666 
667  if (std::abs(this->penetration_depth - other.penetration_depth) > tol) {
668  return false;
669  }
670 
671  if (this->direction != other.direction) {
672  return false;
673  }
674 
675  if (this->size() != other.size()) {
676  return false;
677  }
678 
679  // Check all points of the contact patch.
680  for (size_t i = 0; i < this->size(); ++i) {
681  bool found = false;
682  const Vec3s pi = this->getPoint(i);
683  for (size_t j = 0; j < other.size(); ++j) {
684  const Vec3s other_pj = other.getPoint(j);
685  if (pi.isApprox(other_pj, tol)) {
686  found = true;
687  }
688  }
689  if (!found) {
690  return false;
691  }
692  }
693 
694  return true;
695  }
696 };
697 
704  ContactPatch& contact_patch) {
705  contact_patch.penetration_depth = contact.penetration_depth;
706  contact_patch.tf.translation() = contact.pos;
707  contact_patch.tf.rotation() =
709  contact_patch.direction = ContactPatch::PatchDirection::DEFAULT;
710 }
711 
721 
726 
727  protected:
733 
744 
745  public:
760  explicit ContactPatchRequest(size_t max_num_patch = 1,
761  size_t num_samples_curved_shapes =
763  Scalar patch_tolerance = Scalar(1e-3))
764  : max_num_patch(max_num_patch) {
765  this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
766  this->setPatchTolerance(patch_tolerance);
767  }
768 
770  explicit ContactPatchRequest(const CollisionRequest& collision_request,
771  size_t num_samples_curved_shapes =
773  Scalar patch_tolerance = Scalar(1e-3))
774  : max_num_patch(collision_request.num_max_contacts) {
775  this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
776  this->setPatchTolerance(patch_tolerance);
777  }
778 
780  void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes) {
781  if (num_samples_curved_shapes < 3) {
783  "`num_samples_curved_shapes` cannot be lower than 3. Setting it to "
784  "3 to prevent bugs.");
785  this->m_num_samples_curved_shapes = 3;
786  } else {
787  this->m_num_samples_curved_shapes = num_samples_curved_shapes;
788  }
789  }
790 
792  size_t getNumSamplesCurvedShapes() const {
793  return this->m_num_samples_curved_shapes;
794  }
795 
797  void setPatchTolerance(const Scalar patch_tolerance) {
798  if (patch_tolerance < 0) {
800  "`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
801  "bugs.");
802  this->m_patch_tolerance = Eigen::NumTraits<Scalar>::dummy_precision();
803  } else {
804  this->m_patch_tolerance = patch_tolerance;
805  }
806  }
807 
809  Scalar getPatchTolerance() const { return this->m_patch_tolerance; }
810 
812  bool operator==(const ContactPatchRequest& other) const {
813  return this->max_num_patch == other.max_num_patch &&
814  this->getNumSamplesCurvedShapes() ==
815  other.getNumSamplesCurvedShapes() &&
816  this->getPatchTolerance() == other.getPatchTolerance();
817  }
818 };
819 
822  using ContactPatchVector = std::vector<ContactPatch>;
823  using ContactPatchRef = std::reference_wrapper<ContactPatch>;
824  using ContactPatchRefVector = std::vector<ContactPatchRef>;
825 
826  protected:
834 
839 
842 
843  public:
845  ContactPatchResult() : m_id_available_patch(0) {
846  const size_t max_num_patch = 1;
847  const ContactPatchRequest request(max_num_patch);
848  this->set(request);
849  }
850 
852  explicit ContactPatchResult(const ContactPatchRequest& request)
853  : m_id_available_patch(0) {
854  this->set(request);
855  };
856 
858  size_t numContactPatches() const { return this->m_contact_patches.size(); }
859 
862  if (this->m_id_available_patch >= this->m_contact_patches_data.size()) {
864  "Trying to get an unused contact patch but all contact patches are "
865  "used. Increasing size of contact patches vector, at the cost of a "
866  "copy. You should increase `max_num_patch` in the "
867  "`ContactPatchRequest`.");
868  this->m_contact_patches_data.emplace_back(
869  this->m_contact_patches_data.back());
870  this->m_contact_patches_data.back().clear();
871  }
872  ContactPatch& contact_patch =
873  this->m_contact_patches_data[this->m_id_available_patch];
874  contact_patch.clear();
875  this->m_contact_patches.emplace_back(contact_patch);
876  ++(this->m_id_available_patch);
877  return this->m_contact_patches.back();
878  }
879 
881  const ContactPatch& getContactPatch(const size_t i) const {
882  if (this->m_contact_patches.empty()) {
884  "The number of contact patches is zero. No ContactPatch can be "
885  "returned.",
886  std::invalid_argument);
887  }
888  if (i < this->m_contact_patches.size()) {
889  return this->m_contact_patches[i];
890  }
891  return this->m_contact_patches.back();
892  }
893 
895  ContactPatch& contactPatch(const size_t i) {
896  if (this->m_contact_patches.empty()) {
898  "The number of contact patches is zero. No ContactPatch can be "
899  "returned.",
900  std::invalid_argument);
901  }
902  if (i < this->m_contact_patches.size()) {
903  return this->m_contact_patches[i];
904  }
905  return this->m_contact_patches.back();
906  }
907 
909  void clear() {
910  this->m_contact_patches.clear();
911  this->m_id_available_patch = 0;
912  for (ContactPatch& patch : this->m_contact_patches_data) {
913  patch.clear();
914  }
915  }
916 
918  void set(const ContactPatchRequest& request) {
919  if (this->m_contact_patches_data.size() < request.max_num_patch) {
920  this->m_contact_patches_data.resize(request.max_num_patch);
921  }
922  for (ContactPatch& patch : this->m_contact_patches_data) {
923  patch.points().reserve(request.getNumSamplesCurvedShapes());
924  }
925  this->clear();
926  }
927 
930  bool check(const ContactPatchRequest& request) const {
931  assert(this->m_contact_patches_data.size() >= request.max_num_patch);
932  if (this->m_contact_patches_data.size() < request.max_num_patch) {
933  return false;
934  }
935 
936  for (const ContactPatch& patch : this->m_contact_patches_data) {
937  if (patch.points().capacity() < request.getNumSamplesCurvedShapes()) {
938  assert(patch.points().capacity() >=
939  request.getNumSamplesCurvedShapes());
940  return false;
941  }
942  }
943  return true;
944  }
945 
947  bool operator==(const ContactPatchResult& other) const {
948  if (this->numContactPatches() != other.numContactPatches()) {
949  return false;
950  }
951 
952  for (size_t i = 0; i < this->numContactPatches(); ++i) {
953  const ContactPatch& patch = this->getContactPatch(i);
954  const ContactPatch& other_patch = other.getContactPatch(i);
955  if (!(patch == other_patch)) {
956  return false;
957  }
958  }
959 
960  return true;
961  }
962 
965  void swapObjects() {
966  // Create new transform: it's the reflection of `tf` along the z-axis.
967  // This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
968  // becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
969  for (size_t i = 0; i < this->numContactPatches(); ++i) {
970  ContactPatch& patch = this->contactPatch(i);
971  patch.tf.rotation().col(0) *= -1.0;
972  patch.tf.rotation().col(2) *= -1.0;
973 
974  for (size_t j = 0; j < patch.size(); ++j) {
975  patch.point(i)(0) *= Scalar(-1); // only invert the x-axis
976  }
977  }
978  }
979 };
980 
981 struct DistanceResult;
982 
989  Nearest points are always computed : they are the points of the shapes
990  that achieve a distance of
992  .\n Use `enable_signed_distance` if you want to compute a signed
993  minimum distance(and thus its corresponding nearest points)
994  .)
995  bool enable_nearest_points;
996 
1009  bool enable_signed_distance;
1010 
1012  Scalar rel_err; // relative error, between 0 and 1
1013  Scalar abs_err; // absolute error
1014 
1021  DistanceRequest(bool enable_nearest_points_ = true,
1022  bool enable_signed_distance_ = true, Scalar rel_err_ = 0.0,
1023  Scalar abs_err_ = 0.0)
1024  : enable_nearest_points(enable_nearest_points_),
1025  enable_signed_distance(enable_signed_distance_),
1026  rel_err(rel_err_),
1027  abs_err(abs_err_) {}
1029 
1030  bool isSatisfied(const DistanceResult& result) const;
1031 
1034  DistanceRequest& operator=(const DistanceRequest& other) = default;
1036 
1038  inline bool operator==(const DistanceRequest& other) const {
1041  return QueryRequest::operator==(other) &&
1042  enable_nearest_points == other.enable_nearest_points &&
1043  enable_signed_distance == other.enable_signed_distance &&
1044  rel_err == other.rel_err && abs_err == other.abs_err;
1046  }
1047 };
1048 
1051  public:
1058 
1061 
1064  std::array<Vec3s, 2> nearest_points;
1065 
1068 
1071 
1076  int b1;
1077 
1082  int b2;
1083 
1085  static const int NONE = -1;
1086 
1087  DistanceResult(Scalar min_distance_ = (std::numeric_limits<Scalar>::max)())
1088  : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) {
1089  const Vec3s nan(Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
1090  nearest_points[0] = nearest_points[1] = normal = nan;
1091  }
1092 
1095  const CollisionGeometry* o2_, int b1_, int b2_) {
1096  if (min_distance > distance) {
1097  min_distance = distance;
1098  o1 = o1_;
1099  o2 = o2_;
1100  b1 = b1_;
1101  b2 = b2_;
1102  }
1103  }
1104 
1107  const CollisionGeometry* o2_, int b1_, int b2_, const Vec3s& p1,
1108  const Vec3s& p2, const Vec3s& normal_) {
1109  if (min_distance > distance) {
1110  min_distance = distance;
1111  o1 = o1_;
1112  o2 = o2_;
1113  b1 = b1_;
1114  b2 = b2_;
1115  nearest_points[0] = p1;
1116  nearest_points[1] = p2;
1117  normal = normal_;
1118  }
1119  }
1120 
1122  void update(const DistanceResult& other_result) {
1123  if (min_distance > other_result.min_distance) {
1124  min_distance = other_result.min_distance;
1125  o1 = other_result.o1;
1126  o2 = other_result.o2;
1127  b1 = other_result.b1;
1128  b2 = other_result.b2;
1129  nearest_points[0] = other_result.nearest_points[0];
1130  nearest_points[1] = other_result.nearest_points[1];
1131  normal = other_result.normal;
1132  }
1133  }
1134 
1136  void clear() {
1137  const Vec3s nan(Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
1138  min_distance = (std::numeric_limits<Scalar>::max)();
1139  o1 = NULL;
1140  o2 = NULL;
1141  b1 = NONE;
1142  b2 = NONE;
1143  nearest_points[0] = nearest_points[1] = normal = nan;
1144  timings.clear();
1145  }
1146 
1148  inline bool operator==(const DistanceResult& other) const {
1149  bool is_same = min_distance == other.min_distance &&
1150  nearest_points[0] == other.nearest_points[0] &&
1151  nearest_points[1] == other.nearest_points[1] &&
1152  normal == other.normal && o1 == other.o1 && o2 == other.o2 &&
1153  b1 == other.b1 && b2 == other.b2;
1154 
1155  // TODO: check also that two GeometryObject are indeed equal.
1156  if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
1157  is_same &= (o1 == other.o1);
1158  // else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 ==
1159  // *other.o1;
1160 
1161  if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
1162  is_same &= (o2 == other.o2);
1163  // else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 ==
1164  // *other.o2;
1165 
1166  return is_same;
1167  }
1168 };
1169 
1170 namespace internal {
1172  CollisionResult& res,
1173  const Scalar sqrDistLowerBound) {
1174  // BV cannot find negative distance.
1175  if (res.distance_lower_bound <= 0) return;
1176  Scalar new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin;
1177  if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb;
1178 }
1179 
1181  CollisionResult& res,
1182  const Scalar& distance,
1183  const Vec3s& p0, const Vec3s& p1,
1184  const Vec3s& normal) {
1185  if (distance < res.distance_lower_bound) {
1187  res.nearest_points[0] = p0;
1188  res.nearest_points[1] = p1;
1189  res.normal = normal;
1190  }
1191 }
1192 } // namespace internal
1193 
1195  return static_cast<CollisionRequestFlag>(~static_cast<int>(a));
1196 }
1197 
1200  return static_cast<CollisionRequestFlag>(static_cast<int>(a) |
1201  static_cast<int>(b));
1202 }
1203 
1206  return static_cast<CollisionRequestFlag>(static_cast<int>(a) &
1207  static_cast<int>(b));
1208 }
1209 
1212  return static_cast<CollisionRequestFlag>(static_cast<int>(a) ^
1213  static_cast<int>(b));
1214 }
1215 
1218  return (CollisionRequestFlag&)((int&)(a) |= static_cast<int>(b));
1219 }
1220 
1223  return (CollisionRequestFlag&)((int&)(a) &= static_cast<int>(b));
1224 }
1225 
1228  return (CollisionRequestFlag&)((int&)(a) ^= static_cast<int>(b));
1229 }
1230 
1231 } // namespace coal
1232 
1233 #endif
The geometry for the object for collision or distance computation.
Definition: collision_object.h:94
Simple transform class used locally by InterpMotion.
Definition: transform.h:55
const Vec3s & translation() const
get translation
Definition: transform.h:104
Vec3s transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:152
Vec3s inverseTransform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the inverse of the transform
Definition: transform.h:158
const Matrix3s & rotation() const
get rotation
Definition: transform.h:113
void setIdentity()
set the transform to be identity transform
Definition: transform.h:198
#define COAL_DLLAPI
Definition: config.hh:88
#define COAL_DEPRECATED_MESSAGE(message)
Definition: deprecated.hh:38
#define COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: fwd.hh:122
#define COAL_ASSERT(check, message, exception)
Definition: fwd.hh:82
#define COAL_COMPILER_DIAGNOSTIC_PUSH
Definition: fwd.hh:120
#define COAL_COMPILER_DIAGNOSTIC_POP
Definition: fwd.hh:121
#define COAL_THROW_PRETTY(message, exception)
Definition: fwd.hh:64
Scalar distance(const Matrix3s &R0, const Vec3s &T0, const kIOS &b1, const kIOS &b2, Vec3s *P=NULL, Vec3s *Q=NULL)
Approximate distance between two kIOS bounding volumes.
#define COAL_LOG_WARNING(message)
Definition: logging.h:50
void updateDistanceLowerBoundFromBV(const CollisionRequest &, CollisionResult &res, const Scalar sqrDistLowerBound)
Definition: collision_data.h:1171
void updateDistanceLowerBoundFromLeaf(const CollisionRequest &, CollisionResult &res, const Scalar &distance, const Vec3s &p0, const Vec3s &p1, const Vec3s &normal)
Definition: collision_data.h:1180
Main namespace.
Definition: broadphase_bruteforce.h:44
GJKConvergenceCriterionType
Wether the convergence criterion is scaled on the norm of the solution or not.
Definition: data_types.h:118
@ Relative
Definition: data_types.h:118
CollisionRequestFlag & operator|=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:1216
constexpr Scalar GJK_DEFAULT_TOLERANCE
Definition: narrowphase_defaults.h:47
constexpr size_t GJK_DEFAULT_MAX_ITERATIONS
GJK.
Definition: narrowphase_defaults.h:46
CollisionRequestFlag
flag declaration for specifying required params in CollisionResult
Definition: collision_data.h:304
@ CONTACT
Definition: collision_data.h:305
@ NO_REQUEST
Definition: collision_data.h:307
@ DISTANCE_LOWER_BOUND
Definition: collision_data.h:306
Matrix3s constructOrthonormalBasisFromVector(const Vec3s &vec)
Construct othonormal basis from vector. The z-axis is the normalized input vector.
Definition: transform.h:261
constexpr Scalar EPA_DEFAULT_TOLERANCE
Definition: narrowphase_defaults.h:60
constexpr size_t EPA_DEFAULT_MAX_ITERATIONS
Definition: narrowphase_defaults.h:59
CollisionRequestFlag operator&(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:1204
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition: data_types.h:70
double Scalar
Definition: data_types.h:68
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:80
CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:1198
GJKConvergenceCriterion
Which convergence criterion is used to stop the algorithm (when the shapes are not in collision)....
Definition: data_types.h:114
@ Default
Definition: data_types.h:114
GJKInitialGuess
Initial guess to use for the GJK algorithm DefaultGuess: Vec3s(1, 0, 0) CachedGuess: previous vector ...
Definition: data_types.h:105
@ DefaultGuess
Definition: data_types.h:105
@ CachedGuess
Definition: data_types.h:105
CollisionRequestFlag operator~(CollisionRequestFlag a)
Definition: collision_data.h:1194
CollisionRequestFlag & operator^=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:1226
CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b)
Definition: collision_data.h:1210
CollisionRequestFlag & operator&=(CollisionRequestFlag &a, CollisionRequestFlag b)
Definition: collision_data.h:1221
Eigen::Matrix< Scalar, 2, 1 > Vec2s
Definition: data_types.h:71
void constructContactPatchFrameFromContact(const Contact &contact, ContactPatch &contact_patch)
Construct a frame from a Contact's position and normal. Because both Contact's position and normal ar...
Definition: collision_data.h:703
GJKVariant
Variant to use for the GJK algorithm.
Definition: data_types.h:108
@ DefaultGJK
Definition: data_types.h:108
Definition: timings.h:16
request to the collision algorithm
Definition: collision_data.h:311
CollisionRequest()
Default constructor.
Definition: collision_data.h:358
bool enable_distance_lower_bound
Whether a lower bound on distance is returned when objects are disjoint.
Definition: collision_data.h:321
Scalar security_margin
Distance below which objects are considered in collision. See Collision.
Definition: collision_data.h:328
bool operator==(const CollisionRequest &other) const
whether two CollisionRequest are the same or not
Definition: collision_data.h:370
bool enable_contact
whether the contact information (normal, penetration depth and contact position) will return.
Definition: collision_data.h:317
bool isSatisfied(const CollisionResult &result) const
Scalar distance_upper_bound
Distance above which GJK solver makes an early stopping. GJK stops searching for the closest points w...
Definition: collision_data.h:340
Scalar break_distance
Distance below which bounding volumes are broken down. See Collision.
Definition: collision_data.h:332
size_t num_max_contacts
The maximum number of contacts that can be returned.
Definition: collision_data.h:313
collision result
Definition: collision_data.h:390
Vec3s normal
normal associated to nearest_points. Same as CollisionResult::nearest_points but for the normal.
Definition: collision_data.h:405
void updateDistanceLowerBound(const Scalar &distance_lower_bound_)
Update the lower bound only if the distance is inferior.
Definition: collision_data.h:424
Scalar distance_lower_bound
Definition: collision_data.h:401
void swapObjects()
reposition Contact objects when fcl inverts them during their construction.
CollisionResult()
Definition: collision_data.h:417
void addContact(const Contact &c)
add one contact into result structure
Definition: collision_data.h:430
const std::vector< Contact > & getContacts() const
Definition: collision_data.h:479
std::array< Vec3s, 2 > nearest_points
nearest points. A CollisionResult can have multiple contacts. The nearest points in CollisionResults ...
Definition: collision_data.h:414
const Contact & getContact(size_t i) const
get the i-th contact calculated
Definition: collision_data.h:448
void getContacts(std::vector< Contact > &contacts_) const
get all the contacts
Definition: collision_data.h:474
size_t numContacts() const
number of contacts found
Definition: collision_data.h:445
void clear()
clear the results obtained
Definition: collision_data.h:482
void setContact(size_t i, const Contact &c)
set the i-th contact calculated
Definition: collision_data.h:461
bool isCollision() const
return binary collision result
Definition: collision_data.h:442
bool operator==(const CollisionResult &other) const
whether two CollisionResult are the same or not
Definition: collision_data.h:433
Request for a contact patch computation.
Definition: collision_data.h:723
size_t getNumSamplesCurvedShapes() const
Maximum samples to compute the support sets of curved shapes, i.e. when the normal is perpendicular t...
Definition: collision_data.h:792
ContactPatchRequest(size_t max_num_patch=1, size_t num_samples_curved_shapes=ContactPatch::default_preallocated_size, Scalar patch_tolerance=Scalar(1e-3))
Default constructor.
Definition: collision_data.h:760
size_t m_num_samples_curved_shapes
Maximum samples to compute the support sets of curved shapes, i.e. when the normal is perpendicular t...
Definition: collision_data.h:732
size_t max_num_patch
Maximum number of contact patches that will be computed.
Definition: collision_data.h:725
Scalar m_patch_tolerance
Tolerance below which points are added to a contact patch. In details, given two shapes S1 and S2,...
Definition: collision_data.h:743
Scalar getPatchTolerance() const
Tolerance below which points are added to a contact patch. In details, given two shapes S1 and S2,...
Definition: collision_data.h:809
void setNumSamplesCurvedShapes(const size_t num_samples_curved_shapes)
Maximum samples to compute the support sets of curved shapes, i.e. when the normal is perpendicular t...
Definition: collision_data.h:780
void setPatchTolerance(const Scalar patch_tolerance)
Tolerance below which points are added to a contact patch. In details, given two shapes S1 and S2,...
Definition: collision_data.h:797
bool operator==(const ContactPatchRequest &other) const
Whether two ContactPatchRequest are identical or not.
Definition: collision_data.h:812
ContactPatchRequest(const CollisionRequest &collision_request, size_t num_samples_curved_shapes=ContactPatch::default_preallocated_size, Scalar patch_tolerance=Scalar(1e-3))
Construct a contact patch request from a collision request.
Definition: collision_data.h:770
Result for a contact patch computation.
Definition: collision_data.h:821
size_t numContactPatches() const
Number of contact patches in the result.
Definition: collision_data.h:858
ContactPatchRef getUnusedContactPatch()
Returns a new unused contact patch from the internal data vector.
Definition: collision_data.h:861
bool check(const ContactPatchRequest &request) const
Return true if this ContactPatchResult is aligned with the ContactPatchRequest given as input.
Definition: collision_data.h:930
ContactPatch & contactPatch(const size_t i)
Getter for the i-th contact patch of the result.
Definition: collision_data.h:895
ContactPatchVector m_contact_patches_data
Data container for the vector of contact patches.
Definition: collision_data.h:833
std::reference_wrapper< ContactPatch > ContactPatchRef
Definition: collision_data.h:823
std::vector< ContactPatchRef > ContactPatchRefVector
Definition: collision_data.h:824
void set(const ContactPatchRequest &request)
Set up a ContactPatchResult from a ContactPatchRequest
Definition: collision_data.h:918
size_t m_id_available_patch
Contact patches in m_contact_patches_data can have two statuses: used or unused. This index tracks th...
Definition: collision_data.h:838
void clear()
Clears the contact patch result.
Definition: collision_data.h:909
ContactPatchResult()
Default constructor.
Definition: collision_data.h:845
bool operator==(const ContactPatchResult &other) const
Whether two ContactPatchResult are identical or not.
Definition: collision_data.h:947
ContactPatchRefVector m_contact_patches
Vector of contact patches of the result.
Definition: collision_data.h:841
void swapObjects()
Repositions the ContactPatch when they get inverted during their construction.
Definition: collision_data.h:965
ContactPatchResult(const ContactPatchRequest &request)
Constructor using a ContactPatchRequest.
Definition: collision_data.h:852
const ContactPatch & getContactPatch(const size_t i) const
Const getter for the i-th contact patch of the result.
Definition: collision_data.h:881
std::vector< ContactPatch > ContactPatchVector
Definition: collision_data.h:822
This structure allows to encode contact patches. A contact patch is defined by a set of points belong...
Definition: collision_data.h:511
Vec3s getPointShape2(const size_t i) const
Get the i-th point of the contact patch, projected back onto the first shape of the collision pair....
Definition: collision_data.h:608
Vec3s getPointShape1(const size_t i) const
Get the i-th point of the contact patch, projected back onto the first shape of the collision pair....
Definition: collision_data.h:599
PatchDirection direction
Direction of this contact patch.
Definition: collision_data.h:531
PatchDirection
Direction of ContactPatch. When doing collision detection, the convention of Coal is that the normal ...
Definition: collision_data.h:528
static constexpr size_t default_preallocated_size
Default maximum size of the polygon representing the contact patch. Used to pre-allocate memory for t...
Definition: collision_data.h:548
const Polygon & points() const
Const getter for the 2D points in the set.
Definition: collision_data.h:618
size_t size() const
Returns the number of points in the contact patch.
Definition: collision_data.h:576
bool isSame(const ContactPatch &other, const Scalar tol=Eigen::NumTraits< Scalar >::dummy_precision()) const
Whether two contact patches are the same or not. Checks for different order of the points.
Definition: collision_data.h:657
Polygon m_points
Container for the vertices of the set.
Definition: collision_data.h:552
Vec3s getNormal() const
Normal of the contact patch, expressed in the WORLD frame.
Definition: collision_data.h:568
bool operator==(const ContactPatch &other) const
Whether two contact patches are the same or not.
Definition: collision_data.h:649
void addPoint(const Vec3s &point_3d)
Add a 3D point to the set, expressed in the world frame.
Definition: collision_data.h:583
Scalar penetration_depth
Penetration depth of the contact patch. This value corresponds to the signed distance d between the s...
Definition: collision_data.h:544
ContactPatch(size_t preallocated_size=default_preallocated_size)
Default constructor. Note: the preallocated size does not determine the maximum number of points in t...
Definition: collision_data.h:560
Transform3s tf
Frame of the set, expressed in the world coordinates. The z-axis of the frame's rotation is the conta...
Definition: collision_data.h:517
std::vector< Vec2s > Polygon
Definition: collision_data.h:513
Vec2s & point(const size_t i)
Getter for the i-th 2D point in the set.
Definition: collision_data.h:621
const Vec2s & point(const size_t i) const
Const getter for the i-th 2D point in the set.
Definition: collision_data.h:630
void clear()
Clear the set.
Definition: collision_data.h:639
Polygon & points()
Getter for the 2D points in the set.
Definition: collision_data.h:615
Vec3s getPoint(const size_t i) const
Get the i-th point of the set, expressed in the 3D world frame.
Definition: collision_data.h:589
Contact information returned by collision.
Definition: collision_data.h:58
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_, const Vec3s &p1, const Vec3s &p2, const Vec3s &normal_, Scalar depth_)
Definition: collision_data.h:137
int b1
contact primitive in object 1 if object 1 is mesh or point cloud, it is the triangle or point id if o...
Definition: collision_data.h:69
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_, const Vec3s &pos_, const Vec3s &normal_, Scalar depth_)
Definition: collision_data.h:125
std::array< Vec3s, 2 > nearest_points
nearest points associated to this contact.
Definition: collision_data.h:99
Scalar penetration_depth
penetration depth
Definition: collision_data.h:105
Scalar getDistanceToCollision(const CollisionRequest &request) const
Definition: collision_data.h:384
Vec3s pos
contact position, in world space
Definition: collision_data.h:102
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:60
bool operator<(const Contact &other) const
Definition: collision_data.h:149
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:63
int b2
contact primitive in object 2 if object 2 is mesh or point cloud, it is the triangle or point id if o...
Definition: collision_data.h:75
Contact(const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_)
Definition: collision_data.h:117
bool operator==(const Contact &other) const
Definition: collision_data.h:154
Contact()
Default constructor.
Definition: collision_data.h:111
bool operator!=(const Contact &other) const
Definition: collision_data.h:162
Vec3s normal
contact normal, pointing from o1 to o2. The normal defined as the normalized separation vector: norma...
Definition: collision_data.h:88
request to the distance computation
Definition: collision_data.h:984
Scalar rel_err
error threshold for approximate distance
Definition: collision_data.h:1012
bool enable_signed_distance
whether to compute the penetration depth when objects are in collision. Turning this off can save com...
Definition: collision_data.h:1009
bool operator==(const DistanceRequest &other) const
whether two DistanceRequest are the same or not
Definition: collision_data.h:1038
Scalar abs_err
Definition: collision_data.h:1013
DistanceRequest & operator=(const DistanceRequest &other)=default
bool isSatisfied(const DistanceResult &result) const
bool enable_nearest_points
whether to return the nearest points. Nearest points are always computed and are the points of the sh...
Definition: collision_data.h:995
distance result
Definition: collision_data.h:1050
int b1
information about the nearest point in object 1 if object 1 is mesh or point cloud,...
Definition: collision_data.h:1076
void update(Scalar distance, const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_, const Vec3s &p1, const Vec3s &p2, const Vec3s &normal_)
add distance information into the result
Definition: collision_data.h:1106
const CollisionGeometry * o2
collision object 2
Definition: collision_data.h:1070
void clear()
clear the result
Definition: collision_data.h:1136
const CollisionGeometry * o1
collision object 1
Definition: collision_data.h:1067
int b2
information about the nearest point in object 2 if object 2 is mesh or point cloud,...
Definition: collision_data.h:1082
void update(Scalar distance, const CollisionGeometry *o1_, const CollisionGeometry *o2_, int b1_, int b2_)
add distance information into the result
Definition: collision_data.h:1094
std::array< Vec3s, 2 > nearest_points
nearest points. See CollisionResult::nearest_points.
Definition: collision_data.h:1064
Scalar min_distance
minimum distance between two objects. If two objects are in collision and DistanceRequest::enable_sig...
Definition: collision_data.h:1057
Vec3s normal
normal.
Definition: collision_data.h:1060
bool operator==(const DistanceResult &other) const
whether two DistanceResult are the same or not
Definition: collision_data.h:1148
void update(const DistanceResult &other_result)
add distance information into the result
Definition: collision_data.h:1122
DistanceResult(Scalar min_distance_=(std::numeric_limits< Scalar >::max)())
Definition: collision_data.h:1087
base class for all query requests
Definition: collision_data.h:170
QueryRequest(const QueryRequest &other)=default
Copy constructor.
Scalar epa_tolerance
tolerance for EPA. Note: This tolerance determines the precision on the estimated distance between tw...
Definition: collision_data.h:211
Vec3s cached_gjk_guess
the gjk initial guess set by user
Definition: collision_data.h:180
size_t epa_max_iterations
max number of iterations for EPA
Definition: collision_data.h:204
Scalar collision_distance_threshold
threshold below which a collision is considered.
Definition: collision_data.h:217
GJKConvergenceCriterion gjk_convergence_criterion
convergence criterion used to stop GJK
Definition: collision_data.h:198
bool operator==(const QueryRequest &other) const
whether two QueryRequest are the same or not
Definition: collision_data.h:253
bool enable_timings
enable timings when performing collision/distance request
Definition: collision_data.h:214
QueryRequest & operator=(const QueryRequest &other)=default
Copy assignment operator.
support_func_guess_t cached_support_func_guess
the support function initial guess set by user
Definition: collision_data.h:183
bool enable_cached_gjk_guess
whether enable gjk initial guess @Deprecated Use gjk_initial_guess instead
Definition: collision_data.h:177
Scalar gjk_tolerance
tolerance for the GJK algorithm. Note: This tolerance determines the precision on the estimated dista...
Definition: collision_data.h:192
GJKVariant gjk_variant
whether to enable the Nesterov accleration of GJK
Definition: collision_data.h:195
GJKConvergenceCriterionType gjk_convergence_criterion_type
convergence criterion used to stop GJK
Definition: collision_data.h:201
GJKInitialGuess gjk_initial_guess
Definition: collision_data.h:172
void updateGuess(const QueryResult &result) const
Updates the guess for the internal GJK algorithm in order to warm-start it when reusing this collisio...
Definition: collision_data.h:290
size_t gjk_max_iterations
maximum iteration for the GJK algorithm
Definition: collision_data.h:186
base class for all query results
Definition: collision_data.h:275
Vec3s cached_gjk_guess
stores the last GJK ray when relevant.
Definition: collision_data.h:277
support_func_guess_t cached_support_func_guess
stores the last support function vertex index, when relevant.
Definition: collision_data.h:280
QueryResult()
Definition: collision_data.h:285
CPUTimes timings
timings for the given request
Definition: collision_data.h:283