Program Listing for File shape_shape_contact_patch_func.h
↰ Return to documentation for file (include/coal/internal/shape_shape_contact_patch_func.h)
/*
* 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 Willow Garage, Inc. 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_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
#define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
#include "coal/collision_data.h"
#include "coal/collision_utility.h"
#include "coal/narrowphase/narrowphase.h"
#include "coal/contact_patch/contact_patch_solver.h"
#include "coal/shape/geometric_shapes_traits.h"
namespace coal {
template <typename ShapeType1, typename ShapeType2>
struct ComputeShapeShapeContactPatch {
static void run(const CollisionGeometry* o1, const Transform3s& tf1,
const CollisionGeometry* o2, const Transform3s& tf2,
const CollisionResult& collision_result,
const ContactPatchSolver* csolver,
const ContactPatchRequest& request,
ContactPatchResult& result) {
// Note: see specializations for Plane and Halfspace below.
if (!collision_result.isCollision()) {
return;
}
COAL_ASSERT(
result.check(request),
"The contact patch result and request are incompatible (issue of "
"contact patch size or maximum number of contact patches). Make sure "
"result is initialized with request.",
std::logic_error);
const ShapeType1& s1 = static_cast<const ShapeType1&>(*o1);
const ShapeType2& s2 = static_cast<const ShapeType2&>(*o2);
for (size_t i = 0; i < collision_result.numContacts(); ++i) {
if (i >= request.max_num_patch) {
break;
}
csolver->setSupportGuess(collision_result.cached_support_func_guess);
const Contact& contact = collision_result.getContact(i);
ContactPatch& contact_patch = result.getUnusedContactPatch();
csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch);
}
}
};
template <bool InvertShapes, typename OtherShapeType, typename PlaneOrHalfspace>
void computePatchPlaneOrHalfspace(const OtherShapeType& s1,
const Transform3s& tf1,
const PlaneOrHalfspace& s2,
const Transform3s& tf2,
const ContactPatchSolver* csolver,
const Contact& contact,
ContactPatch& contact_patch) {
COAL_UNUSED_VARIABLE(s2);
COAL_UNUSED_VARIABLE(tf2);
constructContactPatchFrameFromContact(contact, contact_patch);
if ((bool)(shape_traits<OtherShapeType>::IsStrictlyConvex)) {
// Only one point of contact; it has already been computed.
contact_patch.addPoint(contact.pos);
return;
}
// We only need to compute the support set in the direction of the normal.
// We need to temporarily express the patch in the local frame of shape1.
SupportSet& support_set = csolver->support_set_shape1;
support_set.tf.rotation().noalias() =
tf1.rotation().transpose() * contact_patch.tf.rotation();
support_set.tf.translation().noalias() =
tf1.rotation().transpose() *
(contact_patch.tf.translation() - tf1.translation());
// Note: for now, taking into account swept-sphere radius does not change
// anything to the support set computations. However it will be used in the
// future if we want to store the offsets to the support plane for each point
// in a support set.
using SupportOptions = details::SupportOptions;
if (InvertShapes) {
support_set.direction = ContactPatch::PatchDirection::INVERTED;
details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
&s1, support_set, csolver->support_guess[1], csolver->supports_data[1],
csolver->num_samples_curved_shapes, csolver->patch_tolerance);
} else {
support_set.direction = ContactPatch::PatchDirection::DEFAULT;
details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
&s1, support_set, csolver->support_guess[0], csolver->supports_data[0],
csolver->num_samples_curved_shapes, csolver->patch_tolerance);
}
csolver->getResult(contact, &(support_set.points()), contact_patch);
}
#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace) \
template <typename OtherShapeType> \
struct ComputeShapeShapeContactPatch<OtherShapeType, PlaneOrHspace> { \
static void run(const CollisionGeometry* o1, const Transform3s& tf1, \
const CollisionGeometry* o2, const Transform3s& tf2, \
const CollisionResult& collision_result, \
const ContactPatchSolver* csolver, \
const ContactPatchRequest& request, \
ContactPatchResult& result) { \
if (!collision_result.isCollision()) { \
return; \
} \
COAL_ASSERT( \
result.check(request), \
"The contact patch result and request are incompatible (issue of " \
"contact patch size or maximum number of contact patches). Make " \
"sure " \
"result is initialized with request.", \
std::logic_error); \
\
const OtherShapeType& s1 = static_cast<const OtherShapeType&>(*o1); \
const PlaneOrHspace& s2 = static_cast<const PlaneOrHspace&>(*o2); \
for (size_t i = 0; i < collision_result.numContacts(); ++i) { \
if (i >= request.max_num_patch) { \
break; \
} \
csolver->setSupportGuess(collision_result.cached_support_func_guess); \
const Contact& contact = collision_result.getContact(i); \
ContactPatch& contact_patch = result.getUnusedContactPatch(); \
computePatchPlaneOrHalfspace<false, OtherShapeType, PlaneOrHspace>( \
s1, tf1, s2, tf2, csolver, contact, contact_patch); \
} \
} \
}; \
\
template <typename OtherShapeType> \
struct ComputeShapeShapeContactPatch<PlaneOrHspace, OtherShapeType> { \
static void run(const CollisionGeometry* o1, const Transform3s& tf1, \
const CollisionGeometry* o2, const Transform3s& tf2, \
const CollisionResult& collision_result, \
const ContactPatchSolver* csolver, \
const ContactPatchRequest& request, \
ContactPatchResult& result) { \
if (!collision_result.isCollision()) { \
return; \
} \
COAL_ASSERT( \
result.check(request), \
"The contact patch result and request are incompatible (issue of " \
"contact patch size or maximum number of contact patches). Make " \
"sure " \
"result is initialized with request.", \
std::logic_error); \
\
const PlaneOrHspace& s1 = static_cast<const PlaneOrHspace&>(*o1); \
const OtherShapeType& s2 = static_cast<const OtherShapeType&>(*o2); \
for (size_t i = 0; i < collision_result.numContacts(); ++i) { \
if (i >= request.max_num_patch) { \
break; \
} \
csolver->setSupportGuess(collision_result.cached_support_func_guess); \
const Contact& contact = collision_result.getContact(i); \
ContactPatch& contact_patch = result.getUnusedContactPatch(); \
computePatchPlaneOrHalfspace<true, OtherShapeType, PlaneOrHspace>( \
s2, tf2, s1, tf1, csolver, contact, contact_patch); \
} \
} \
};
PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane)
PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace)
#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2) \
template <> \
struct ComputeShapeShapeContactPatch<PlaneOrHspace1, PlaneOrHspace2> { \
static void run(const CollisionGeometry* o1, const Transform3s& tf1, \
const CollisionGeometry* o2, const Transform3s& tf2, \
const CollisionResult& collision_result, \
const ContactPatchSolver* csolver, \
const ContactPatchRequest& request, \
ContactPatchResult& result) { \
COAL_UNUSED_VARIABLE(o1); \
COAL_UNUSED_VARIABLE(tf1); \
COAL_UNUSED_VARIABLE(o2); \
COAL_UNUSED_VARIABLE(tf2); \
COAL_UNUSED_VARIABLE(csolver); \
if (!collision_result.isCollision()) { \
return; \
} \
COAL_ASSERT( \
result.check(request), \
"The contact patch result and request are incompatible (issue of " \
"contact patch size or maximum number of contact patches). Make " \
"sure " \
"result is initialized with request.", \
std::logic_error); \
\
for (size_t i = 0; i < collision_result.numContacts(); ++i) { \
if (i >= request.max_num_patch) { \
break; \
} \
const Contact& contact = collision_result.getContact(i); \
ContactPatch& contact_patch = result.getUnusedContactPatch(); \
constructContactPatchFrameFromContact(contact, contact_patch); \
contact_patch.addPoint(contact.pos); \
} \
} \
};
PLANE_HSPACE_CONTACT_PATCH(Plane, Plane)
PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace)
PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane)
PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace)
#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH
#undef PLANE_HSPACE_CONTACT_PATCH
template <typename ShapeType1, typename ShapeType2>
void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1,
const CollisionGeometry* o2, const Transform3s& tf2,
const CollisionResult& collision_result,
const ContactPatchSolver* csolver,
const ContactPatchRequest& request,
ContactPatchResult& result) {
return ComputeShapeShapeContactPatch<ShapeType1, ShapeType2>::run(
o1, tf1, o2, tf2, collision_result, csolver, request, result);
}
} // namespace coal
#endif