| Directory: | ./ |
|---|---|
| File: | include/coal/internal/shape_shape_contact_patch_func.h |
| Date: | 2025-04-01 09:23:31 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 28 | 41 | 68.3% |
| Branches: | 19 | 76 | 25.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Software License Agreement (BSD License) | ||
| 3 | * | ||
| 4 | * Copyright (c) 2024, INRIA | ||
| 5 | * All rights reserved. | ||
| 6 | * | ||
| 7 | * Redistribution and use in source and binary forms, with or without | ||
| 8 | * modification, are permitted provided that the following conditions | ||
| 9 | * are met: | ||
| 10 | * | ||
| 11 | * * Redistributions of source code must retain the above copyright | ||
| 12 | * notice, this list of conditions and the following disclaimer. | ||
| 13 | * * Redistributions in binary form must reproduce the above | ||
| 14 | * copyright notice, this list of conditions and the following | ||
| 15 | * disclaimer in the documentation and/or other materials provided | ||
| 16 | * with the distribution. | ||
| 17 | * * Neither the name of Willow Garage, Inc. nor the names of its | ||
| 18 | * contributors may be used to endorse or promote products derived | ||
| 19 | * from this software without specific prior written permission. | ||
| 20 | * | ||
| 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 32 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 33 | */ | ||
| 34 | |||
| 35 | /** \author Louis Montaut */ | ||
| 36 | |||
| 37 | #ifndef COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H | ||
| 38 | #define COAL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H | ||
| 39 | |||
| 40 | #include "coal/collision_data.h" | ||
| 41 | #include "coal/collision_utility.h" | ||
| 42 | #include "coal/narrowphase/narrowphase.h" | ||
| 43 | #include "coal/contact_patch/contact_patch_solver.h" | ||
| 44 | #include "coal/shape/geometric_shapes_traits.h" | ||
| 45 | |||
| 46 | #include "coal/tracy.hh" | ||
| 47 | |||
| 48 | namespace coal { | ||
| 49 | |||
| 50 | /// @brief Shape-shape contact patch computation. | ||
| 51 | /// Assumes that `csolver` and the `ContactPatchResult` have already been set up | ||
| 52 | /// by the `ContactPatchRequest`. | ||
| 53 | template <typename ShapeType1, typename ShapeType2> | ||
| 54 | struct ComputeShapeShapeContactPatch { | ||
| 55 | 24 | static void run(const CollisionGeometry* o1, const Transform3s& tf1, | |
| 56 | const CollisionGeometry* o2, const Transform3s& tf2, | ||
| 57 | const CollisionResult& collision_result, | ||
| 58 | const ContactPatchSolver* csolver, | ||
| 59 | const ContactPatchRequest& request, | ||
| 60 | ContactPatchResult& result) { | ||
| 61 | // Note: see specializations for Plane and Halfspace below. | ||
| 62 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
|
24 | if (!collision_result.isCollision()) { |
| 63 | ✗ | return; | |
| 64 | } | ||
| 65 |
1/2✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
24 | COAL_ASSERT( |
| 66 | result.check(request), | ||
| 67 | "The contact patch result and request are incompatible (issue of " | ||
| 68 | "contact patch size or maximum number of contact patches). Make sure " | ||
| 69 | "result is initialized with request.", | ||
| 70 | std::logic_error); | ||
| 71 | |||
| 72 | 24 | const ShapeType1& s1 = static_cast<const ShapeType1&>(*o1); | |
| 73 | 24 | const ShapeType2& s2 = static_cast<const ShapeType2&>(*o2); | |
| 74 |
2/2✓ Branch 1 taken 12 times.
✓ Branch 2 taken 12 times.
|
48 | for (size_t i = 0; i < collision_result.numContacts(); ++i) { |
| 75 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
24 | if (i >= request.max_num_patch) { |
| 76 | ✗ | break; | |
| 77 | } | ||
| 78 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
24 | csolver->setSupportGuess(collision_result.cached_support_func_guess); |
| 79 | 24 | const Contact& contact = collision_result.getContact(i); | |
| 80 | 24 | ContactPatch& contact_patch = result.getUnusedContactPatch(); | |
| 81 | 24 | csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch); | |
| 82 | } | ||
| 83 | } | ||
| 84 | }; | ||
| 85 | |||
| 86 | /// @brief Computes the contact patch between a Plane/Halfspace and another | ||
| 87 | /// shape. | ||
| 88 | /// @tparam InvertShapes set to true if the first shape of the collision pair | ||
| 89 | /// is s2 and not s1 (if you had to invert (s1, tf1) and (s2, tf2) when calling | ||
| 90 | /// this function). | ||
| 91 | template <bool InvertShapes, typename OtherShapeType, typename PlaneOrHalfspace> | ||
| 92 | 24 | void computePatchPlaneOrHalfspace(const OtherShapeType& s1, | |
| 93 | const Transform3s& tf1, | ||
| 94 | const PlaneOrHalfspace& s2, | ||
| 95 | const Transform3s& tf2, | ||
| 96 | const ContactPatchSolver* csolver, | ||
| 97 | const Contact& contact, | ||
| 98 | ContactPatch& contact_patch) { | ||
| 99 | COAL_UNUSED_VARIABLE(s2); | ||
| 100 | COAL_UNUSED_VARIABLE(tf2); | ||
| 101 | 24 | constructContactPatchFrameFromContact(contact, contact_patch); | |
| 102 | if ((bool)(shape_traits<OtherShapeType>::IsStrictlyConvex)) { | ||
| 103 | // Only one point of contact; it has already been computed. | ||
| 104 | ✗ | contact_patch.addPoint(contact.pos); | |
| 105 | ✗ | return; | |
| 106 | } | ||
| 107 | |||
| 108 | // We only need to compute the support set in the direction of the normal. | ||
| 109 | // We need to temporarily express the patch in the local frame of shape1. | ||
| 110 | 24 | SupportSet& support_set = csolver->support_set_shape1; | |
| 111 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
24 | support_set.tf.rotation().noalias() = |
| 112 |
1/2✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | tf1.rotation().transpose() * contact_patch.tf.rotation(); |
| 113 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
|
48 | support_set.tf.translation().noalias() = |
| 114 |
1/2✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
48 | tf1.rotation().transpose() * |
| 115 | 24 | (contact_patch.tf.translation() - tf1.translation()); | |
| 116 | |||
| 117 | // Note: for now, taking into account swept-sphere radius does not change | ||
| 118 | // anything to the support set computations. However it will be used in the | ||
| 119 | // future if we want to store the offsets to the support plane for each point | ||
| 120 | // in a support set. | ||
| 121 | using SupportOptions = details::SupportOptions; | ||
| 122 | if (InvertShapes) { | ||
| 123 | 24 | support_set.direction = ContactPatch::PatchDirection::INVERTED; | |
| 124 | 24 | details::getShapeSupportSet<SupportOptions::WithSweptSphere>( | |
| 125 | 24 | &s1, support_set, csolver->support_guess[1], csolver->supports_data[1], | |
| 126 | 24 | csolver->num_samples_curved_shapes, csolver->patch_tolerance); | |
| 127 | } else { | ||
| 128 | ✗ | support_set.direction = ContactPatch::PatchDirection::DEFAULT; | |
| 129 | ✗ | details::getShapeSupportSet<SupportOptions::WithSweptSphere>( | |
| 130 | ✗ | &s1, support_set, csolver->support_guess[0], csolver->supports_data[0], | |
| 131 | ✗ | csolver->num_samples_curved_shapes, csolver->patch_tolerance); | |
| 132 | } | ||
| 133 | 24 | csolver->getResult(contact, &(support_set.points()), contact_patch); | |
| 134 | } | ||
| 135 | |||
| 136 | #define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace) \ | ||
| 137 | template <typename OtherShapeType> \ | ||
| 138 | struct ComputeShapeShapeContactPatch<OtherShapeType, PlaneOrHspace> { \ | ||
| 139 | static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ | ||
| 140 | const CollisionGeometry* o2, const Transform3s& tf2, \ | ||
| 141 | const CollisionResult& collision_result, \ | ||
| 142 | const ContactPatchSolver* csolver, \ | ||
| 143 | const ContactPatchRequest& request, \ | ||
| 144 | ContactPatchResult& result) { \ | ||
| 145 | COAL_TRACY_ZONE_SCOPED_N( \ | ||
| 146 | "coal::ComputeShapeShapeContactPatch<OtherShapeType, " \ | ||
| 147 | "PlaneOrHspace>::run"); \ | ||
| 148 | if (!collision_result.isCollision()) { \ | ||
| 149 | return; \ | ||
| 150 | } \ | ||
| 151 | COAL_ASSERT( \ | ||
| 152 | result.check(request), \ | ||
| 153 | "The contact patch result and request are incompatible (issue of " \ | ||
| 154 | "contact patch size or maximum number of contact patches). Make " \ | ||
| 155 | "sure " \ | ||
| 156 | "result is initialized with request.", \ | ||
| 157 | std::logic_error); \ | ||
| 158 | \ | ||
| 159 | const OtherShapeType& s1 = static_cast<const OtherShapeType&>(*o1); \ | ||
| 160 | const PlaneOrHspace& s2 = static_cast<const PlaneOrHspace&>(*o2); \ | ||
| 161 | for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ | ||
| 162 | if (i >= request.max_num_patch) { \ | ||
| 163 | break; \ | ||
| 164 | } \ | ||
| 165 | csolver->setSupportGuess(collision_result.cached_support_func_guess); \ | ||
| 166 | const Contact& contact = collision_result.getContact(i); \ | ||
| 167 | ContactPatch& contact_patch = result.getUnusedContactPatch(); \ | ||
| 168 | computePatchPlaneOrHalfspace<false, OtherShapeType, PlaneOrHspace>( \ | ||
| 169 | s1, tf1, s2, tf2, csolver, contact, contact_patch); \ | ||
| 170 | } \ | ||
| 171 | } \ | ||
| 172 | }; \ | ||
| 173 | \ | ||
| 174 | template <typename OtherShapeType> \ | ||
| 175 | struct ComputeShapeShapeContactPatch<PlaneOrHspace, OtherShapeType> { \ | ||
| 176 | static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ | ||
| 177 | const CollisionGeometry* o2, const Transform3s& tf2, \ | ||
| 178 | const CollisionResult& collision_result, \ | ||
| 179 | const ContactPatchSolver* csolver, \ | ||
| 180 | const ContactPatchRequest& request, \ | ||
| 181 | ContactPatchResult& result) { \ | ||
| 182 | COAL_TRACY_ZONE_SCOPED_N( \ | ||
| 183 | "coal::ComputeShapeShapeContactPatch<PlaneOrHspace, " \ | ||
| 184 | "OtherShapeType>::run"); \ | ||
| 185 | if (!collision_result.isCollision()) { \ | ||
| 186 | return; \ | ||
| 187 | } \ | ||
| 188 | COAL_ASSERT( \ | ||
| 189 | result.check(request), \ | ||
| 190 | "The contact patch result and request are incompatible (issue of " \ | ||
| 191 | "contact patch size or maximum number of contact patches). Make " \ | ||
| 192 | "sure " \ | ||
| 193 | "result is initialized with request.", \ | ||
| 194 | std::logic_error); \ | ||
| 195 | \ | ||
| 196 | const PlaneOrHspace& s1 = static_cast<const PlaneOrHspace&>(*o1); \ | ||
| 197 | const OtherShapeType& s2 = static_cast<const OtherShapeType&>(*o2); \ | ||
| 198 | for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ | ||
| 199 | if (i >= request.max_num_patch) { \ | ||
| 200 | break; \ | ||
| 201 | } \ | ||
| 202 | csolver->setSupportGuess(collision_result.cached_support_func_guess); \ | ||
| 203 | const Contact& contact = collision_result.getContact(i); \ | ||
| 204 | ContactPatch& contact_patch = result.getUnusedContactPatch(); \ | ||
| 205 | computePatchPlaneOrHalfspace<true, OtherShapeType, PlaneOrHspace>( \ | ||
| 206 | s2, tf2, s1, tf1, csolver, contact, contact_patch); \ | ||
| 207 | } \ | ||
| 208 | } \ | ||
| 209 | }; | ||
| 210 | |||
| 211 | ✗ | PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane) | |
| 212 |
6/10✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 12 times.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 20 taken 12 times.
✓ Branch 21 taken 12 times.
|
24 | PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace) |
| 213 | |||
| 214 | #define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2) \ | ||
| 215 | template <> \ | ||
| 216 | struct ComputeShapeShapeContactPatch<PlaneOrHspace1, PlaneOrHspace2> { \ | ||
| 217 | static void run(const CollisionGeometry* o1, const Transform3s& tf1, \ | ||
| 218 | const CollisionGeometry* o2, const Transform3s& tf2, \ | ||
| 219 | const CollisionResult& collision_result, \ | ||
| 220 | const ContactPatchSolver* csolver, \ | ||
| 221 | const ContactPatchRequest& request, \ | ||
| 222 | ContactPatchResult& result) { \ | ||
| 223 | COAL_UNUSED_VARIABLE(o1); \ | ||
| 224 | COAL_UNUSED_VARIABLE(tf1); \ | ||
| 225 | COAL_UNUSED_VARIABLE(o2); \ | ||
| 226 | COAL_UNUSED_VARIABLE(tf2); \ | ||
| 227 | COAL_UNUSED_VARIABLE(csolver); \ | ||
| 228 | if (!collision_result.isCollision()) { \ | ||
| 229 | return; \ | ||
| 230 | } \ | ||
| 231 | COAL_ASSERT( \ | ||
| 232 | result.check(request), \ | ||
| 233 | "The contact patch result and request are incompatible (issue of " \ | ||
| 234 | "contact patch size or maximum number of contact patches). Make " \ | ||
| 235 | "sure " \ | ||
| 236 | "result is initialized with request.", \ | ||
| 237 | std::logic_error); \ | ||
| 238 | \ | ||
| 239 | for (size_t i = 0; i < collision_result.numContacts(); ++i) { \ | ||
| 240 | if (i >= request.max_num_patch) { \ | ||
| 241 | break; \ | ||
| 242 | } \ | ||
| 243 | const Contact& contact = collision_result.getContact(i); \ | ||
| 244 | ContactPatch& contact_patch = result.getUnusedContactPatch(); \ | ||
| 245 | constructContactPatchFrameFromContact(contact, contact_patch); \ | ||
| 246 | contact_patch.addPoint(contact.pos); \ | ||
| 247 | } \ | ||
| 248 | } \ | ||
| 249 | }; | ||
| 250 | |||
| 251 | ✗ | PLANE_HSPACE_CONTACT_PATCH(Plane, Plane) | |
| 252 | ✗ | PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace) | |
| 253 | ✗ | PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane) | |
| 254 | ✗ | PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace) | |
| 255 | |||
| 256 | #undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH | ||
| 257 | #undef PLANE_HSPACE_CONTACT_PATCH | ||
| 258 | |||
| 259 | template <typename ShapeType1, typename ShapeType2> | ||
| 260 | 24 | void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3s& tf1, | |
| 261 | const CollisionGeometry* o2, const Transform3s& tf2, | ||
| 262 | const CollisionResult& collision_result, | ||
| 263 | const ContactPatchSolver* csolver, | ||
| 264 | const ContactPatchRequest& request, | ||
| 265 | ContactPatchResult& result) { | ||
| 266 | COAL_TRACY_ZONE_SCOPED_N("coal::ShapeShapeContactPatch"); | ||
| 267 | 24 | return ComputeShapeShapeContactPatch<ShapeType1, ShapeType2>::run( | |
| 268 | 24 | o1, tf1, o2, tf2, collision_result, csolver, request, result); | |
| 269 | } | ||
| 270 | |||
| 271 | } // namespace coal | ||
| 272 | |||
| 273 | #endif | ||
| 274 |