| 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 INRIA 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_CONTACT_PATCH_SOLVER_HXX | ||
| 38 | #define COAL_CONTACT_PATCH_SOLVER_HXX | ||
| 39 | |||
| 40 | #include "coal/data_types.h" | ||
| 41 | #include "coal/shape/geometric_shapes_traits.h" | ||
| 42 | |||
| 43 | #include "coal/tracy.hh" | ||
| 44 | |||
| 45 | namespace coal { | ||
| 46 | |||
| 47 | // ============================================================================ | ||
| 48 | 24 | inline void ContactPatchSolver::set(const ContactPatchRequest& request) { | |
| 49 | // Note: it's important for the number of pre-allocated Vec3s in | ||
| 50 | // `m_clipping_sets` to be larger than `request.max_size_patch` | ||
| 51 | // because we don't know in advance how many supports will be discarded to | ||
| 52 | // form the convex-hulls of the shapes supports which will serve as the | ||
| 53 | // input of the Sutherland-Hodgman algorithm. | ||
| 54 | 24 | size_t num_preallocated_supports = default_num_preallocated_supports; | |
| 55 |
1/2✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
|
24 | if (num_preallocated_supports < 2 * request.getNumSamplesCurvedShapes()) { |
| 56 | 24 | num_preallocated_supports = 2 * request.getNumSamplesCurvedShapes(); | |
| 57 | } | ||
| 58 | |||
| 59 | // Used for support set computation of shape1 and for the first iterate of the | ||
| 60 | // Sutherland-Hodgman algo. | ||
| 61 | 24 | this->support_set_shape1.points().reserve(num_preallocated_supports); | |
| 62 | 24 | this->support_set_shape1.direction = SupportSetDirection::DEFAULT; | |
| 63 | |||
| 64 | // Used for computing the next iterate of the Sutherland-Hodgman algo. | ||
| 65 | 24 | this->support_set_buffer.points().reserve(num_preallocated_supports); | |
| 66 | |||
| 67 | // Used for support set computation of shape2 and acts as the "clipper" set in | ||
| 68 | // the Sutherland-Hodgman algo. | ||
| 69 | 24 | this->support_set_shape2.points().reserve(num_preallocated_supports); | |
| 70 | 24 | this->support_set_shape2.direction = SupportSetDirection::INVERTED; | |
| 71 | |||
| 72 | 24 | this->num_samples_curved_shapes = request.getNumSamplesCurvedShapes(); | |
| 73 | 24 | this->patch_tolerance = request.getPatchTolerance(); | |
| 74 | 24 | } | |
| 75 | |||
| 76 | // ============================================================================ | ||
| 77 | template <typename ShapeType1, typename ShapeType2> | ||
| 78 | 24 | void ContactPatchSolver::computePatch(const ShapeType1& s1, | |
| 79 | const Transform3s& tf1, | ||
| 80 | const ShapeType2& s2, | ||
| 81 | const Transform3s& tf2, | ||
| 82 | const Contact& contact, | ||
| 83 | ContactPatch& contact_patch) const { | ||
| 84 | COAL_TRACY_ZONE_SCOPED_N("coal::ContactPatchSolver::computePatch"); | ||
| 85 | // Note: `ContactPatch` is an alias for `SupportSet`. | ||
| 86 | // Step 1 | ||
| 87 | 24 | constructContactPatchFrameFromContact(contact, contact_patch); | |
| 88 | 24 | contact_patch.points().clear(); | |
| 89 | if ((bool)(shape_traits<ShapeType1>::IsStrictlyConvex) || | ||
| 90 | (bool)(shape_traits<ShapeType2>::IsStrictlyConvex)) { | ||
| 91 | // If a shape is strictly convex, the support set in any direction is | ||
| 92 | // reduced to a single point. Thus, the contact point `contact.pos` is the | ||
| 93 | // only point belonging to the contact patch, and it has already been | ||
| 94 | // computed. | ||
| 95 | // TODO(louis): even for strictly convex shapes, we can sample the support | ||
| 96 | // function around the normal and return a pseudo support set. This would | ||
| 97 | // allow spheres and ellipsoids to have a contact surface, which does make | ||
| 98 | // sense in certain physics simulation cases. | ||
| 99 | // Do the same for strictly convex regions of non-strictly convex shapes | ||
| 100 | // like the ends of capsules. | ||
| 101 | 2 | contact_patch.addPoint(contact.pos); | |
| 102 | 2 | return; | |
| 103 | } | ||
| 104 | |||
| 105 | // Step 2 - Compute support set of each shape, in the direction of | ||
| 106 | // the contact's normal. | ||
| 107 | // The first shape's support set is called "current"; it will be the first | ||
| 108 | // iterate of the Sutherland-Hodgman algorithm. The second shape's support set | ||
| 109 | // is called "clipper"; it will be used to clip "current". The support set | ||
| 110 | // computation step computes a convex polygon; its vertices are ordered | ||
| 111 | // counter-clockwise. This is important as the Sutherland-Hodgman algorithm | ||
| 112 | // expects points to be ranked counter-clockwise. | ||
| 113 | 22 | this->reset(s1, tf1, s2, tf2, contact_patch); | |
| 114 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 11 times.
|
22 | assert(this->num_samples_curved_shapes > 3); |
| 115 | |||
| 116 | 22 | this->supportFuncShape1(&s1, this->support_set_shape1, this->support_guess[0], | |
| 117 | this->supports_data[0], | ||
| 118 | 22 | this->num_samples_curved_shapes, | |
| 119 | 22 | this->patch_tolerance); | |
| 120 | |||
| 121 | 22 | this->supportFuncShape2(&s2, this->support_set_shape2, this->support_guess[1], | |
| 122 | this->supports_data[1], | ||
| 123 | 22 | this->num_samples_curved_shapes, | |
| 124 | 22 | this->patch_tolerance); | |
| 125 | |||
| 126 | // We can immediatly return if one of the support set has only | ||
| 127 | // one point. | ||
| 128 |
3/6✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 11 times.
|
44 | if (this->support_set_shape1.size() <= 1 || |
| 129 | 22 | this->support_set_shape2.size() <= 1) { | |
| 130 | ✗ | contact_patch.addPoint(contact.pos); | |
| 131 | ✗ | return; | |
| 132 | } | ||
| 133 | |||
| 134 | // `eps` is be used to check strict positivity of determinants. | ||
| 135 | 22 | const Scalar eps = Eigen::NumTraits<Scalar>::dummy_precision(); | |
| 136 | using Polygon = SupportSet::Polygon; | ||
| 137 | |||
| 138 |
6/6✓ Branch 1 taken 3 times.
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 10 times.
|
28 | if ((this->support_set_shape1.size() == 2) && |
| 139 | 6 | (this->support_set_shape2.size() == 2)) { | |
| 140 | // Segment-Segment case | ||
| 141 | // We compute the determinant; if it is non-zero, the intersection | ||
| 142 | // has already been computed: it's `Contact::pos`. | ||
| 143 | 2 | const Polygon& pts1 = this->support_set_shape1.points(); | |
| 144 | 2 | const Vec2s& a = pts1[0]; | |
| 145 | 2 | const Vec2s& b = pts1[1]; | |
| 146 | |||
| 147 | 2 | const Polygon& pts2 = this->support_set_shape2.points(); | |
| 148 | 2 | const Vec2s& c = pts2[0]; | |
| 149 | 2 | const Vec2s& d = pts2[1]; | |
| 150 | |||
| 151 | 2 | const Scalar det = | |
| 152 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
|
2 | (b(0) - a(0)) * (d(1) - c(1)) >= (b(1) - a(1)) * (d(0) - c(0)); |
| 153 |
1/8✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
2 | if ((std::abs(det) > eps) || ((c - d).squaredNorm() < eps) || |
| 154 |
1/8✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | ((b - a).squaredNorm() < eps)) { |
| 155 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_patch.addPoint(contact.pos); |
| 156 | 2 | return; | |
| 157 | } | ||
| 158 | |||
| 159 | ✗ | const Vec2s cd = (d - c); | |
| 160 | ✗ | const Scalar l = cd.squaredNorm(); | |
| 161 | ✗ | Polygon& patch = contact_patch.points(); | |
| 162 | |||
| 163 | // Project a onto [c, d] | ||
| 164 | ✗ | Scalar t1 = (a - c).dot(cd); | |
| 165 | ✗ | t1 = (t1 >= l) ? 1.0 : ((t1 <= 0) ? 0.0 : (t1 / l)); | |
| 166 | ✗ | const Vec2s p1 = c + t1 * cd; | |
| 167 | ✗ | patch.emplace_back(p1); | |
| 168 | |||
| 169 | // Project b onto [c, d] | ||
| 170 | ✗ | Scalar t2 = (b - c).dot(cd); | |
| 171 | ✗ | t2 = (t2 >= l) ? 1.0 : ((t2 <= 0) ? 0.0 : (t2 / l)); | |
| 172 | ✗ | const Vec2s p2 = c + t2 * cd; | |
| 173 | ✗ | if ((p1 - p2).squaredNorm() >= eps) { | |
| 174 | ✗ | patch.emplace_back(p2); | |
| 175 | } | ||
| 176 | ✗ | return; | |
| 177 | } | ||
| 178 | |||
| 179 | // | ||
| 180 | // Step 3 - Main loop of the algorithm: use the "clipper" polygon to clip the | ||
| 181 | // "current" polygon. The resulting intersection is the contact patch of the | ||
| 182 | // contact between s1 and s2. "clipper" and "current" are the support sets of | ||
| 183 | // shape1 and shape2 (they can be swapped, i.e. clipper can be assigned to | ||
| 184 | // shape1 and current to shape2, depending on which case we are). Currently, | ||
| 185 | // to clip one polygon with the other, we use the Sutherland-Hodgman | ||
| 186 | // algorithm: | ||
| 187 | // https://en.wikipedia.org/wiki/Sutherland%E2%80%93Hodgman_algorithm | ||
| 188 | // In the general case, Sutherland-Hodgman clips one polygon of size >=3 using | ||
| 189 | // another polygon of size >=3. However, it can be easily extended to handle | ||
| 190 | // the segment-polygon case. | ||
| 191 | // | ||
| 192 | // The maximum size of the output of the Sutherland-Hodgman algorithm is n1 + | ||
| 193 | // n2 where n1 and n2 are the sizes of the first and second polygon. | ||
| 194 | 20 | const size_t max_result_size = | |
| 195 | 20 | this->support_set_shape1.size() + this->support_set_shape2.size(); | |
| 196 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | if (this->added_to_patch.size() < max_result_size) { |
| 197 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | this->added_to_patch.assign(max_result_size, false); |
| 198 | } | ||
| 199 | |||
| 200 | 20 | const Polygon* clipper_ptr = nullptr; | |
| 201 | 20 | Polygon* current_ptr = nullptr; | |
| 202 | 20 | Polygon* previous_ptr = &(this->support_set_buffer.points()); | |
| 203 | |||
| 204 | // Let the clipper set be the one with the most vertices, to make sure it is | ||
| 205 | // at least a triangle. | ||
| 206 |
2/2✓ Branch 2 taken 2 times.
✓ Branch 3 taken 8 times.
|
20 | if (this->support_set_shape1.size() < this->support_set_shape2.size()) { |
| 207 | 4 | current_ptr = &(this->support_set_shape1.points()); | |
| 208 | 4 | clipper_ptr = &(this->support_set_shape2.points()); | |
| 209 | } else { | ||
| 210 | 16 | current_ptr = &(this->support_set_shape2.points()); | |
| 211 | 16 | clipper_ptr = &(this->support_set_shape1.points()); | |
| 212 | } | ||
| 213 | |||
| 214 | 20 | const Polygon& clipper = *(clipper_ptr); | |
| 215 | 20 | const size_t clipper_size = clipper.size(); | |
| 216 |
2/2✓ Branch 0 taken 33 times.
✓ Branch 1 taken 8 times.
|
82 | for (size_t i = 0; i < clipper_size; ++i) { |
| 217 | // Swap `current` and `previous`. | ||
| 218 | // `previous` tracks the last iteration of the algorithm; `current` is | ||
| 219 | // filled by clipping `current` using `clipper`. | ||
| 220 | 66 | Polygon* tmp_ptr = previous_ptr; | |
| 221 | 66 | previous_ptr = current_ptr; | |
| 222 | 66 | current_ptr = tmp_ptr; | |
| 223 | |||
| 224 | 66 | const Polygon& previous = *(previous_ptr); | |
| 225 | 66 | Polygon& current = *(current_ptr); | |
| 226 | 66 | current.clear(); | |
| 227 | |||
| 228 | 66 | const Vec2s& a = clipper[i]; | |
| 229 | 66 | const Vec2s& b = clipper[(i + 1) % clipper_size]; | |
| 230 |
2/4✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33 times.
✗ Branch 5 not taken.
|
66 | const Vec2s ab = b - a; |
| 231 | |||
| 232 |
2/2✓ Branch 1 taken 15 times.
✓ Branch 2 taken 18 times.
|
66 | if (previous.size() == 2) { |
| 233 | // | ||
| 234 | // Segment-Polygon case | ||
| 235 | // | ||
| 236 | 30 | const Vec2s& p1 = previous[0]; | |
| 237 | 30 | const Vec2s& p2 = previous[1]; | |
| 238 | |||
| 239 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | const Vec2s ap1 = p1 - a; |
| 240 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | const Vec2s ap2 = p2 - a; |
| 241 | |||
| 242 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
30 | const Scalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); |
| 243 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
30 | const Scalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); |
| 244 | |||
| 245 |
3/4✓ Branch 0 taken 3 times.
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
|
30 | if (det1 < 0 && det2 < 0) { |
| 246 | // Both p1 and p2 are outside the clipping polygon, i.e. there is no | ||
| 247 | // intersection. The algorithm can stop. | ||
| 248 | 4 | break; | |
| 249 | } | ||
| 250 | |||
| 251 |
4/4✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 3 times.
|
30 | if (det1 >= 0 && det2 >= 0) { |
| 252 | // Both p1 and p2 are inside the clipping polygon, there is nothing to | ||
| 253 | // do; move to the next iteration. | ||
| 254 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
18 | current = previous; |
| 255 | 26 | continue; | |
| 256 | } | ||
| 257 | |||
| 258 | // Compute the intersection between the line (a, b) and the segment | ||
| 259 | // [p1, p2]. | ||
| 260 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
|
12 | if (det1 >= 0) { |
| 261 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
|
6 | if (det1 > eps) { |
| 262 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); |
| 263 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | current.emplace_back(p1); |
| 264 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | current.emplace_back(p); |
| 265 | 4 | continue; | |
| 266 | 4 | } else { | |
| 267 | // p1 is the only point of current which is also a point of the | ||
| 268 | // clipper. We can exit. | ||
| 269 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | current.emplace_back(p1); |
| 270 | 2 | break; | |
| 271 | } | ||
| 272 | } else { | ||
| 273 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
|
6 | if (det2 > eps) { |
| 274 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); |
| 275 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | current.emplace_back(p2); |
| 276 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | current.emplace_back(p); |
| 277 | 4 | continue; | |
| 278 | 4 | } else { | |
| 279 | // p2 is the only point of current which is also a point of the | ||
| 280 | // clipper. We can exit. | ||
| 281 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | current.emplace_back(p2); |
| 282 | 2 | break; | |
| 283 | } | ||
| 284 | } | ||
| 285 | } else { | ||
| 286 | // | ||
| 287 | // Polygon-Polygon case. | ||
| 288 | // | ||
| 289 |
1/2✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
|
36 | std::fill(this->added_to_patch.begin(), // |
| 290 | this->added_to_patch.end(), // | ||
| 291 | 36 | false); | |
| 292 | |||
| 293 | 36 | const size_t previous_size = previous.size(); | |
| 294 |
2/2✓ Branch 0 taken 70 times.
✓ Branch 1 taken 18 times.
|
176 | for (size_t j = 0; j < previous_size; ++j) { |
| 295 | 140 | const Vec2s& p1 = previous[j]; | |
| 296 | 140 | const Vec2s& p2 = previous[(j + 1) % previous_size]; | |
| 297 | |||
| 298 |
2/4✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
|
140 | const Vec2s ap1 = p1 - a; |
| 299 |
2/4✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
|
140 | const Vec2s ap2 = p2 - a; |
| 300 | |||
| 301 |
4/8✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
|
140 | const Scalar det1 = ab(0) * ap1(1) - ab(1) * ap1(0); |
| 302 |
4/8✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
|
140 | const Scalar det2 = ab(0) * ap2(1) - ab(1) * ap2(0); |
| 303 | |||
| 304 |
3/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
|
140 | if (det1 < 0 && det2 < 0) { |
| 305 | // No intersection. Continue to next segment of previous. | ||
| 306 | 132 | continue; | |
| 307 | } | ||
| 308 | |||
| 309 |
4/4✓ Branch 0 taken 68 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 66 times.
✓ Branch 3 taken 2 times.
|
140 | if (det1 >= 0 && det2 >= 0) { |
| 310 | // Both p1 and p2 are inside the clipping polygon, add p1 to current | ||
| 311 | // (only if it has not already been added). | ||
| 312 |
3/4✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✓ Branch 5 taken 2 times.
|
132 | if (!this->added_to_patch[j]) { |
| 313 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
128 | current.emplace_back(p1); |
| 314 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
128 | this->added_to_patch[j] = true; |
| 315 | } | ||
| 316 | // Continue to next segment of previous. | ||
| 317 | 132 | continue; | |
| 318 | } | ||
| 319 | |||
| 320 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
8 | if (det1 >= 0) { |
| 321 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
|
4 | if (det1 > eps) { |
| 322 | ✗ | if (!this->added_to_patch[j]) { | |
| 323 | ✗ | current.emplace_back(p1); | |
| 324 | ✗ | this->added_to_patch[j] = true; | |
| 325 | } | ||
| 326 | ✗ | const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); | |
| 327 | ✗ | current.emplace_back(p); | |
| 328 | } else { | ||
| 329 | // a, b and p1 are colinear; we add only p1. | ||
| 330 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | if (!this->added_to_patch[j]) { |
| 331 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | current.emplace_back(p1); |
| 332 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | this->added_to_patch[j] = true; |
| 333 | } | ||
| 334 | } | ||
| 335 | } else { | ||
| 336 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
|
4 | if (det2 > eps) { |
| 337 | ✗ | const Vec2s p = computeLineSegmentIntersection(a, b, p1, p2); | |
| 338 | ✗ | current.emplace_back(p); | |
| 339 | } else { | ||
| 340 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
4 | if (!this->added_to_patch[(j + 1) % previous.size()]) { |
| 341 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | current.emplace_back(p2); |
| 342 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | this->added_to_patch[(j + 1) % previous.size()] = true; |
| 343 | } | ||
| 344 | } | ||
| 345 | } | ||
| 346 | } | ||
| 347 | } | ||
| 348 | // | ||
| 349 | // End of iteration i of Sutherland-Hodgman. | ||
| 350 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
|
36 | if (current.size() <= 1) { |
| 351 | // No intersection or one point found, the algo can early stop. | ||
| 352 | ✗ | break; | |
| 353 | } | ||
| 354 | } | ||
| 355 | |||
| 356 | // Transfer the result of the Sutherland-Hodgman algorithm to the contact | ||
| 357 | // patch. | ||
| 358 | 20 | this->getResult(contact, current_ptr, contact_patch); | |
| 359 | } | ||
| 360 | |||
| 361 | // ============================================================================ | ||
| 362 | 22 | inline void ContactPatchSolver::getResult( | |
| 363 | const Contact& contact, const ContactPatch::Polygon* result_ptr, | ||
| 364 | ContactPatch& contact_patch) const { | ||
| 365 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 16 times.
|
22 | if (result_ptr->size() <= 1) { |
| 366 | 6 | contact_patch.addPoint(contact.pos); | |
| 367 | 6 | return; | |
| 368 | } | ||
| 369 | |||
| 370 | 16 | const ContactPatch::Polygon& result = *(result_ptr); | |
| 371 | 16 | ContactPatch::Polygon& patch = contact_patch.points(); | |
| 372 | 16 | patch = result; | |
| 373 | } | ||
| 374 | |||
| 375 | // ============================================================================ | ||
| 376 | template <typename ShapeType1, typename ShapeType2> | ||
| 377 | 22 | inline void ContactPatchSolver::reset(const ShapeType1& shape1, | |
| 378 | const Transform3s& tf1, | ||
| 379 | const ShapeType2& shape2, | ||
| 380 | const Transform3s& tf2, | ||
| 381 | const ContactPatch& contact_patch) const { | ||
| 382 | // Reset internal quantities | ||
| 383 | 22 | this->support_set_shape1.clear(); | |
| 384 | 22 | this->support_set_shape2.clear(); | |
| 385 | 22 | this->support_set_buffer.clear(); | |
| 386 | |||
| 387 | // Get the support function of each shape | ||
| 388 | 22 | const Transform3s& tfc = contact_patch.tf; | |
| 389 | |||
| 390 | 22 | this->support_set_shape1.direction = SupportSetDirection::DEFAULT; | |
| 391 | // Set the reference frame of the support set of the first shape to be the | ||
| 392 | // local frame of shape 1. | ||
| 393 | 22 | Transform3s& tf1c = this->support_set_shape1.tf; | |
| 394 |
4/8✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 11 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 11 times.
✗ Branch 14 not taken.
|
22 | tf1c.rotation().noalias() = tf1.rotation().transpose() * tfc.rotation(); |
| 395 |
4/8✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
|
44 | tf1c.translation().noalias() = |
| 396 |
1/2✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
|
44 | tf1.rotation().transpose() * (tfc.translation() - tf1.translation()); |
| 397 | 22 | this->supportFuncShape1 = | |
| 398 | 22 | this->makeSupportSetFunction(&shape1, this->supports_data[0]); | |
| 399 | |||
| 400 | 22 | this->support_set_shape2.direction = SupportSetDirection::INVERTED; | |
| 401 | // Set the reference frame of the support set of the second shape to be the | ||
| 402 | // local frame of shape 2. | ||
| 403 | 22 | Transform3s& tf2c = this->support_set_shape2.tf; | |
| 404 |
4/8✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 11 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 11 times.
✗ Branch 14 not taken.
|
22 | tf2c.rotation().noalias() = tf2.rotation().transpose() * tfc.rotation(); |
| 405 |
4/8✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
|
44 | tf2c.translation().noalias() = |
| 406 |
1/2✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
|
44 | tf2.rotation().transpose() * (tfc.translation() - tf2.translation()); |
| 407 | 22 | this->supportFuncShape2 = | |
| 408 | 22 | this->makeSupportSetFunction(&shape2, this->supports_data[1]); | |
| 409 | 22 | } | |
| 410 | |||
| 411 | // ========================================================================== | ||
| 412 | 4 | inline Vec2s ContactPatchSolver::computeLineSegmentIntersection( | |
| 413 | const Vec2s& a, const Vec2s& b, const Vec2s& c, const Vec2s& d) { | ||
| 414 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | const Vec2s ab = b - a; |
| 415 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
4 | const Vec2s n(-ab(1), ab(0)); |
| 416 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | const Scalar denominator = n.dot(c - d); |
| 417 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
|
4 | if (std::abs(denominator) < std::numeric_limits<Scalar>::epsilon()) { |
| 418 | ✗ | return d; | |
| 419 | } | ||
| 420 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | const Scalar nominator = n.dot(a - d); |
| 421 | 4 | Scalar alpha = nominator / denominator; | |
| 422 | 4 | alpha = std::min<Scalar>(1.0, std::max<Scalar>(0.0, alpha)); | |
| 423 |
4/8✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
4 | return alpha * c + (1 - alpha) * d; |
| 424 | } | ||
| 425 | |||
| 426 | } // namespace coal | ||
| 427 | |||
| 428 | #endif // COAL_CONTACT_PATCH_SOLVER_HXX | ||
| 429 |