39 #ifndef COAL_COLLISION_DATA_H
40 #define COAL_COLLISION_DATA_H
108 static const int NONE = -1;
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());
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());
132 nearest_points{pos_ - (depth_ * normal_ / 2),
133 pos_ + (depth_ * normal_ / 2)},
135 penetration_depth(depth_) {}
145 nearest_points{p1, p2},
147 penetration_depth(depth_) {}
150 if (b1 == other.
b1)
return b2 < other.
b2;
151 return b1 < other.
b1;
155 return o1 == other.
o1 && o2 == other.
o2 && b1 == other.
b1 &&
156 b2 == other.
b2 && normal == other.
normal && pos == other.
pos &&
177 bool enable_cached_gjk_guess;
186 size_t gjk_max_iterations;
204 size_t epa_max_iterations;
224 enable_cached_gjk_guess(false),
225 cached_gjk_guess(1, 0, 0),
234 enable_timings(false),
235 collision_distance_threshold(
236 Eigen::NumTraits<
Scalar>::dummy_precision()) {}
260 gjk_convergence_criterion_type ==
286 : cached_gjk_guess(
Vec3s::Zero()),
321 bool enable_distance_lower_bound;
350 : num_max_contacts(num_max_contacts_),
351 enable_contact(flag &
CONTACT),
354 break_distance(
Scalar(1e-3)),
355 distance_upper_bound((std::numeric_limits<
Scalar>::max)()) {}
359 : num_max_contacts(1),
360 enable_contact(true),
361 enable_distance_lower_bound(false),
363 break_distance(
Scalar(1e-3)),
364 distance_upper_bound((std::numeric_limits<
Scalar>::max)()) {}
393 std::vector<Contact> contacts;
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());
425 if (distance_lower_bound_ < distance_lower_bound)
426 distance_lower_bound = distance_lower_bound_;
434 return contacts == other.contacts &&
449 if (contacts.size() == 0)
451 "The number of contacts is zero. No Contact can be returned.",
452 std::invalid_argument);
454 if (i < contacts.size())
457 return contacts.back();
462 if (contacts.size() == 0)
464 "The number of contacts is zero. No Contact can be returned.",
465 std::invalid_argument);
467 if (i < contacts.size())
475 contacts_.resize(contacts.size());
476 std::copy(contacts.begin(), contacts.end(), contacts_.begin());
479 const std::vector<Contact>&
getContacts()
const {
return contacts; }
483 distance_lower_bound = (std::numeric_limits<Scalar>::max)();
486 nearest_points[0] = nearest_points[1] = normal =
487 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
548 static constexpr
size_t default_preallocated_size = 12;
560 explicit ContactPatch(
size_t preallocated_size = default_preallocated_size)
563 penetration_depth(0) {
564 this->m_points.reserve(preallocated_size);
569 if (this->direction == PatchDirection::INVERTED) {
576 size_t size()
const {
return this->m_points.size(); }
585 this->m_points.emplace_back(point.template head<2>());
590 Vec3s point(0, 0, 0);
591 point.head<2>() = this->point(i);
600 Vec3s point = this->getPoint(i);
601 point -= (this->penetration_depth / 2) * this->getNormal();
609 Vec3s point = this->getPoint(i);
610 point += (this->penetration_depth / 2) * this->getNormal();
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];
626 return this->m_points.back();
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];
635 return this->m_points.back();
640 this->m_points.clear();
642 this->penetration_depth = 0;
650 return this->tf == other.
tf && this->direction == other.
direction &&
652 this->points() == other.
points();
659 const Scalar tol = Eigen::NumTraits<Scalar>::dummy_precision())
const {
663 if (!this->getNormal().isApprox(other.
getNormal(), tol)) {
671 if (this->direction != other.
direction) {
675 if (this->size() != other.
size()) {
680 for (
size_t i = 0; i < this->size(); ++i) {
682 const Vec3s pi = this->getPoint(i);
683 for (
size_t j = 0; j < other.
size(); ++j) {
685 if (pi.isApprox(other_pj, tol)) {
709 contact_patch.
direction = ContactPatch::PatchDirection::DEFAULT;
761 size_t num_samples_curved_shapes =
764 : max_num_patch(max_num_patch) {
765 this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
766 this->setPatchTolerance(patch_tolerance);
771 size_t num_samples_curved_shapes =
774 : max_num_patch(collision_request.num_max_contacts) {
775 this->setNumSamplesCurvedShapes(num_samples_curved_shapes);
776 this->setPatchTolerance(patch_tolerance);
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;
787 this->m_num_samples_curved_shapes = num_samples_curved_shapes;
793 return this->m_num_samples_curved_shapes;
798 if (patch_tolerance < 0) {
800 "`patch_tolerance` cannot be negative. Setting it to 0 to prevent "
802 this->m_patch_tolerance = Eigen::NumTraits<Scalar>::dummy_precision();
804 this->m_patch_tolerance = patch_tolerance;
814 this->getNumSamplesCurvedShapes() ==
846 const size_t max_num_patch = 1;
853 : m_id_available_patch(0) {
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();
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();
882 if (this->m_contact_patches.empty()) {
884 "The number of contact patches is zero. No ContactPatch can be "
886 std::invalid_argument);
888 if (i < this->m_contact_patches.size()) {
889 return this->m_contact_patches[i];
891 return this->m_contact_patches.back();
896 if (this->m_contact_patches.empty()) {
898 "The number of contact patches is zero. No ContactPatch can be "
900 std::invalid_argument);
902 if (i < this->m_contact_patches.size()) {
903 return this->m_contact_patches[i];
905 return this->m_contact_patches.back();
910 this->m_contact_patches.clear();
911 this->m_id_available_patch = 0;
912 for (
ContactPatch& patch : this->m_contact_patches_data) {
919 if (this->m_contact_patches_data.size() < request.
max_num_patch) {
922 for (
ContactPatch& patch : this->m_contact_patches_data) {
931 assert(this->m_contact_patches_data.size() >= request.
max_num_patch);
932 if (this->m_contact_patches_data.size() < request.
max_num_patch) {
936 for (
const ContactPatch& patch : this->m_contact_patches_data) {
938 assert(patch.points().capacity() >=
952 for (
size_t i = 0; i < this->numContactPatches(); ++i) {
955 if (!(patch == other_patch)) {
969 for (
size_t i = 0; i < this->numContactPatches(); ++i) {
974 for (
size_t j = 0; j < patch.
size(); ++j) {
981 struct DistanceResult;
989 Nearest points are always computed : they are the points of the shapes
992 .\n Use `enable_signed_distance`
if you want to compute a
signed
993 minimum
distance(and thus its corresponding nearest points)
995 bool enable_nearest_points;
1009 bool enable_signed_distance;
1022 bool enable_signed_distance_ = true,
Scalar rel_err_ = 0.0,
1024 : enable_nearest_points(enable_nearest_points_),
1025 enable_signed_distance(enable_signed_distance_),
1027 abs_err(abs_err_) {}
1085 static const int NONE = -1;
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;
1115 nearest_points[0] = p1;
1116 nearest_points[1] = p2;
1125 o1 = other_result.
o1;
1126 o2 = other_result.
o2;
1127 b1 = other_result.
b1;
1128 b2 = other_result.
b2;
1131 normal = other_result.
normal;
1137 const Vec3s nan(Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
1138 min_distance = (std::numeric_limits<Scalar>::max)();
1143 nearest_points[0] = nearest_points[1] = normal = nan;
1152 normal == other.
normal && o1 == other.
o1 && o2 == other.
o2 &&
1153 b1 == other.
b1 && b2 == other.
b2;
1156 if ((o1 != NULL) ^ (other.
o1 != NULL))
return false;
1157 is_same &= (o1 == other.
o1);
1161 if ((o2 != NULL) ^ (other.
o2 != NULL))
return false;
1162 is_same &= (o2 == other.
o2);
1170 namespace internal {
1173 const Scalar sqrDistLowerBound) {
1176 Scalar new_dlb = std::sqrt(sqrDistLowerBound);
1184 const Vec3s& normal) {
1201 static_cast<int>(b));
1207 static_cast<int>(b));
1213 static_cast<int>(b));
The geometry for the object for collision or distance computation.
Definition: collision_object.h:94
#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
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 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