Program Listing for File contact_patch_solver.hxx
↰ Return to documentation for file (include/coal/contact_patch/contact_patch_solver.hxx)
/*
* Software License Agreement (BSD License)
*
* 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 INRIA nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef COAL_CONTACT_PATCH_SOLVER_HXX
#define COAL_CONTACT_PATCH_SOLVER_HXX
#include "coal/data_types.h"
#include "coal/shape/geometric_shapes_traits.h"
namespace coal {
// ============================================================================
inline void ContactPatchSolver::set(const ContactPatchRequest& request) {
// Note: it's important for the number of pre-allocated Vec3s in
// `m_clipping_sets` to be larger than `request.max_size_patch`
// because we don't know in advance how many supports will be discarded to
// form the convex-hulls of the shapes supports which will serve as the
// input of the Sutherland-Hodgman algorithm.
size_t num_preallocated_supports = default_num_preallocated_supports;
if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) {
num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes();
}
// Used for support set computation of shape1 and for the first iterate of the
// Sutherland-Hodgman algo.
this->support_set_shape1.points().reserve(num_preallocated_supports);
this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
// Used for computing the next iterate of the Sutherland-Hodgman algo.
this->support_set_buffer.points().reserve(num_preallocated_supports);
// Used for support set computation of shape2 and acts as the "clipper" set in
// the Sutherland-Hodgman algo.
this->support_set_shape2.points().reserve(num_preallocated_supports);
this->support_set_shape2.direction = SupportSetDirection::INVERTED;
this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes();
this->patch_tolerance = request.getPatchTolerance();
}
// ============================================================================
template <typename ShapeType1, typename ShapeType2>
void ContactPatchSolver::computePatch(const ShapeType1& s1,
const Transform3s& tf1,
const ShapeType2& s2,
const Transform3s& tf2,
const Contact& contact,
ContactPatch& contact_patch) const {
// Note: `ContactPatch` is an alias for `SupportSet`.
// Step 1
constructContactPatchFrameFromContact(contact, contact_patch);
contact_patch.points().clear();
if ((bool)(shape_traits<ShapeType1>::IsStrictlyConvex) ||
(bool)(shape_traits<ShapeType2>::IsStrictlyConvex)) {
// If a shape is strictly convex, the support set in any direction is
// reduced to a single point. Thus, the contact point `contact.pos` is the
// only point belonging to the contact patch, and it has already been
// computed.
// TODO(louis): even for strictly convex shapes, we can sample the support
// function around the normal and return a pseudo support set. This would
// allow spheres and ellipsoids to have a contact surface, which does make
// sense in certain physics simulation cases.
// Do the same for strictly convex regions of non-strictly convex shapes
// like the ends of capsules.
contact_patch.addPoint(contact.pos);
return;
}
// Step 2 - Compute support set of each shape, in the direction of
// the contact's normal.
// The first shape's support set is called "current"; it will be the first
// iterate of the Sutherland-Hodgman algorithm. The second shape's support set
// is called "clipper"; it will be used to clip "current". The support set
// computation step computes a convex polygon; its vertices are ordered
// counter-clockwise. This is important as the Sutherland-Hodgman algorithm
// expects points to be ranked counter-clockwise.
this->reset(s1, tf1, s2, tf2, contact_patch);
assert(this->num_samples_curved_shapes > 3);
this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0],
this->supports_data[0],
this->num_samples_curved_shapes,
this->patch_tolerance);
this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1],
this->supports_data[1],
this->num_samples_curved_shapes,
this->patch_tolerance);
// We can immediatly return if one of the support set has only
// one point.
if (this->support_set_shape1.size() <= 1 ||
this->support_set_shape2.size() <= 1) {
contact_patch.addPoint(contact.pos);
return;
}
// `eps` is be used to check strict positivity of determinants.
const CoalScalar eps = Eigen::NumTraits<CoalScalar>::dummy_precision();
using Polygon = SupportSet::Polygon;
if ((this->support_set_shape1.size() == 2) &&
(this->support_set_shape2.size() == 2)) {
// Segment-Segment case
// We compute the determinant; if it is non-zero, the intersection
// has already been computed: it's `Contact::pos`.
const Polygon& pts1 = this->support_set_shape1.points();
const Vec2s& a = pts1[0];
const Vec2s& b = pts1[1];
const Polygon& pts2 = this->support_set_shape2.points();
const Vec2s& c = pts2[0];
const Vec2s& d = pts2[1];
const CoalScalar det =
(b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0));
if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) ||
((b - a).squaredNorm() < eps)) {
contact_patch.addPoint(contact.pos);
return;
}
const Vec2s cd = (d - c);
const CoalScalar l = cd.squaredNorm();
Polygon& patch = contact_patch.points();
// Project a onto [c, d]
CoalScalar t1 = (a - c).dot(cd);
t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l));
const Vec2s p1 = c + t1 * cd;
patch.emplace_back(p1);
// Project b onto [c, d]
CoalScalar t2 = (b - c).dot(cd);
t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l));
const Vec2s p2 = c + t2 * cd;
if ((p1 - p2).squaredNorm() >= eps) {
patch.emplace_back(p2);
}
return;
}
//
// Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the
// "current" polygon. The resulting intersection is the contact patch of the
// contact between s1 and s2. "clipper" and "current" are the support sets of
// shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to
// shape1 and current to shape2, depending on which case we are). Currently,
// to clip one polygon with the other, we use the Sutherland-Hodgman
// algorithm:
// https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm
// In the general case, Sutherland-Hodgman clips one polygon of size >=3 using
// another polygon of size >=3. However, it can be easily extended to handle
// the segment-polygon case.
//
// The maximum size of the output of the Sutherland-Hodgman algorithm is n1 +
// n2 where n1 and n2 are the sizes of the first and second polygon.
const size_t max_result_size =
this->support_set_shape1.size() + this->support_set_shape2.size();
if (this->added_to_patch.size() < max_result_size) {
this->added_to_patch.assign(max_result_size, false);
}
const Polygon* clipper_ptr = nullptr;
Polygon* current_ptr = nullptr;
Polygon* previous_ptr = &(this->support_set_buffer.points());
// Let the clipper set be the one with the most vertices, to make sure it is
// at least a triangle.
if (this->support_set_shape1.size() < this->support_set_shape2.size()) {
current_ptr = &(this->support_set_shape1.points());
clipper_ptr = &(this->support_set_shape2.points());
} else {
current_ptr = &(this->support_set_shape2.points());
clipper_ptr = &(this->support_set_shape1.points());
}
const Polygon& clipper = *(clipper_ptr);
const size_t clipper_size = clipper.size();
for (size_t i = 0; i < clipper_size; ++i) {
// Swap `current` and `previous`.
// `previous` tracks the last iteration of the algorithm; `current` is
// filled by clipping `current` using `clipper`.
Polygon* tmp_ptr = previous_ptr;
previous_ptr = current_ptr;
current_ptr = tmp_ptr;
const Polygon& previous = *(previous_ptr);
Polygon& current = *(current_ptr);
current.clear();
const Vec2s& a = clipper[i];
const Vec2s& b = clipper[(i + 1) % clipper_size];
const Vec2s ab = b - a;
if (previous.size() == 2) {
//
// Segment-Polygon case
//
const Vec2s& p1 = previous[0];
const Vec2s& p2 = previous[1];
const Vec2s ap1 = p1 - a;
const Vec2s ap2 = p2 - a;
const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
if (det1 < 0 && det2 < 0) {
// Both p1 and p2 are outside the clipping polygon, i.e. there is no
// intersection. The algorithm can stop.
break;
}
if (det1 >= 0 && det2 >= 0) {
// Both p1 and p2 are inside the clipping polygon, there is nothing to
// do; move to the next iteration.
current = previous;
continue;
}
// Compute the intersection between the line (a, b) and the segment
// [p1, p2].
if (det1 >= 0) {
if (det1 > eps) {
const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p1);
current.emplace_back(p);
continue;
} else {
// p1 is the only point of current which is also a point of the
// clipper. We can exit.
current.emplace_back(p1);
break;
}
} else {
if (det2 > eps) {
const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p2);
current.emplace_back(p);
continue;
} else {
// p2 is the only point of current which is also a point of the
// clipper. We can exit.
current.emplace_back(p2);
break;
}
}
} else {
//
// Polygon-Polygon case.
//
std::fill(this->added_to_patch.begin(), //
this->added_to_patch.end(), //
false);
const size_t previous_size = previous.size();
for (size_t j = 0; j < previous_size; ++j) {
const Vec2s& p1 = previous[j];
const Vec2s& p2 = previous[(j + 1) % previous_size];
const Vec2s ap1 = p1 - a;
const Vec2s ap2 = p2 - a;
const CoalScalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0);
const CoalScalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0);
if (det1 < 0 && det2 < 0) {
// No intersection. Continue to next segment of previous.
continue;
}
if (det1 >= 0 && det2 >= 0) {
// Both p1 and p2 are inside the clipping polygon, add p1 to current
// (only if it has not already been added).
if (!this->added_to_patch[j]) {
current.emplace_back(p1);
this->added_to_patch[j] = true;
}
// Continue to next segment of previous.
continue;
}
if (det1 >= 0) {
if (det1 > eps) {
if (!this->added_to_patch[j]) {
current.emplace_back(p1);
this->added_to_patch[j] = true;
}
const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p);
} else {
// a, b and p1 are colinear; we add only p1.
if (!this->added_to_patch[j]) {
current.emplace_back(p1);
this->added_to_patch[j] = true;
}
}
} else {
if (det2 > eps) {
const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2);
current.emplace_back(p);
} else {
if (!this->added_to_patch[(j + 1) % previous.size()]) {
current.emplace_back(p2);
this->added_to_patch[(j + 1) % previous.size()] = true;
}
}
}
}
}
//
// End of iteration i of Sutherland-Hodgman.
if (current.size() <= 1) {
// No intersection or one point found, the algo can early stop.
break;
}
}
// Transfer the result of the Sutherland-Hodgman algorithm to the contact
// patch.
this->getResult(contact, current_ptr, contact_patch);
}
// ============================================================================
inline void ContactPatchSolver::getResult(
const Contact& contact, const ContactPatch::Polygon* result_ptr,
ContactPatch& contact_patch) const {
if (result_ptr->size() <= 1) {
contact_patch.addPoint(contact.pos);
return;
}
const ContactPatch::Polygon& result = *(result_ptr);
ContactPatch::Polygon& patch = contact_patch.points();
patch = result;
}
// ============================================================================
template <typename ShapeType1, typename ShapeType2>
inline void ContactPatchSolver::reset(const ShapeType1& shape1,
const Transform3s& tf1,
const ShapeType2& shape2,
const Transform3s& tf2,
const ContactPatch& contact_patch) const {
// Reset internal quantities
this->support_set_shape1.clear();
this->support_set_shape2.clear();
this->support_set_buffer.clear();
// Get the support function of each shape
const Transform3s& tfc = contact_patch.tf;
this->support_set_shape1.direction = SupportSetDirection::DEFAULT;
// Set the reference frame of the support set of the first shape to be the
// local frame of shape 1.
Transform3s& tf1c = this->support_set_shape1.tf;
tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation();
tf1c.translation().noalias() =
tf1.rotation().transpose() * (tfc.translation() - tf1.translation());
this->supportFuncShape1 =
this->makeSupportSetFunction(&shape1, this->supports_data[0]);
this->support_set_shape2.direction = SupportSetDirection::INVERTED;
// Set the reference frame of the support set of the second shape to be the
// local frame of shape 2.
Transform3s& tf2c = this->support_set_shape2.tf;
tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation();
tf2c.translation().noalias() =
tf2.rotation().transpose() * (tfc.translation() - tf2.translation());
this->supportFuncShape2 =
this->makeSupportSetFunction(&shape2, this->supports_data[1]);
}
// ==========================================================================
inline Vec2s ContactPatchSolver::computeLineSegmentIntersection(
const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) {
const Vec2s ab = b - a;
const Vec2s n(-ab(1), ab(0));
const CoalScalar denominator = n.dot(c - d);
if (std::abs(denominator) < std::numeric_limits<double>::epsilon()) {
return d;
}
const CoalScalar nominator = n.dot(a - d);
CoalScalar alpha = nominator / denominator;
alpha = std::min<double>(1.0, std::max<CoalScalar>(0.0, alpha));
return alpha * c + (1 - alpha) * d;
}
} // namespace coal
#endif // COAL_CONTACT_PATCH_SOLVER_HXX