GCC Code Coverage Report

Directory: ./
File: src/narrowphase/details.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 318 341 93.3%
Functions: 14 14 100.0%
Branches: 469 936 50.1%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
7 * Copyright (c) 2021-2024, INRIA
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of Open Source Robotics Foundation nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 */
37 /** \author Jia Pan, Florent Lamiraux */
38
39 #ifndef COAL_SRC_NARROWPHASE_DETAILS_H
40 #define COAL_SRC_NARROWPHASE_DETAILS_H
41
42 #include "coal/internal/traversal_node_setup.h"
43 #include "coal/narrowphase/narrowphase.h"
44
45 namespace coal {
46 namespace details {
47 // Compute the point on a line segment that is the closest point on the
48 // segment to to another point. The code is inspired by the explanation
49 // given by Dan Sunday's page:
50 // http://geomalgorithms.com/a02-_lines.html
51 881 static inline void lineSegmentPointClosestToPoint(const Vec3s& p,
52 const Vec3s& s1,
53 const Vec3s& s2, Vec3s& sp) {
54
2/4
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 881 times.
✗ Branch 5 not taken.
 881 Vec3s v = s2 - s1;
55
2/4
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 881 times.
✗ Branch 5 not taken.
 881 Vec3s w = p - s1;
56
57
1/2
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
 881 Scalar c1 = w.dot(v);
58
1/2
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
 881 Scalar c2 = v.dot(v);
59
60
2/2
✓ Branch 0 taken 289 times.
✓ Branch 1 taken 592 times.
 881 if (c1 <= 0) {
61
1/2
✓ Branch 1 taken 289 times.
✗ Branch 2 not taken.
 289 sp = s1;
62
2/2
✓ Branch 0 taken 384 times.
✓ Branch 1 taken 208 times.
 592 } else if (c2 <= c1) {
63
1/2
✓ Branch 1 taken 384 times.
✗ Branch 2 not taken.
 384 sp = s2;
64 } else {
65 208 Scalar b = c1 / c2;
66
3/6
✓ Branch 1 taken 208 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 208 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 208 times.
✗ Branch 8 not taken.
 208 Vec3s Pb = s1 + v * b;
67
1/2
✓ Branch 1 taken 208 times.
✗ Branch 2 not taken.
 208 sp = Pb;
68 }
69 881 }
70
71 /// @param p1 witness point on the Sphere.
72 /// @param p2 witness point on the Capsule.
73 /// @param normal pointing from shape 1 to shape 2 (sphere to capsule).
74 /// @return the distance between the two shapes (negative if penetration).
75 881 inline Scalar sphereCapsuleDistance(const Sphere& s1, const Transform3s& tf1,
76 const Capsule& s2, const Transform3s& tf2,
77 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
78
2/4
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 881 times.
✗ Branch 5 not taken.
 881 Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength)));
79
2/4
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 881 times.
✗ Branch 5 not taken.
 881 Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength)));
80
1/2
✓ Branch 2 taken 881 times.
✗ Branch 3 not taken.
 881 Vec3s s_c = tf1.getTranslation();
81
82
1/2
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
 881 Vec3s segment_point;
83
84
1/2
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
 881 lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point);
85
2/4
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 881 times.
✗ Branch 5 not taken.
 881 normal = segment_point - s_c;
86
1/2
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
 881 Scalar norm(normal.norm());
87 881 Scalar r1 = s1.radius + s1.getSweptSphereRadius();
88 881 Scalar r2 = s2.radius + s2.getSweptSphereRadius();
89 881 Scalar dist = norm - r1 - r2;
90
91 static const Scalar eps(std::numeric_limits<Scalar>::epsilon());
92
2/2
✓ Branch 0 taken 877 times.
✓ Branch 1 taken 4 times.
 881 if (norm > eps) {
93
1/2
✓ Branch 1 taken 877 times.
✗ Branch 2 not taken.
 877 normal.normalize();
94 } else {
95
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 normal << 1, 0, 0;
96 }
97
3/6
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 881 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 881 times.
✗ Branch 8 not taken.
 881 p1 = s_c + normal * r1;
98
3/6
✓ Branch 1 taken 881 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 881 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 881 times.
✗ Branch 8 not taken.
 881 p2 = segment_point - normal * r2;
99 881 return dist;
100 }
101
102 /// @param p1 witness point on the Sphere.
103 /// @param p2 witness point on the Cylinder.
104 /// @param normal pointing from shape 1 to shape 2 (sphere to cylinder).
105 /// @return the distance between the two shapes (negative if penetration).
106 155999 inline Scalar sphereCylinderDistance(const Sphere& s1, const Transform3s& tf1,
107 const Cylinder& s2, const Transform3s& tf2,
108 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
109 static const Scalar eps(sqrt(std::numeric_limits<Scalar>::epsilon()));
110 155999 Scalar r1(s1.radius);
111 155999 Scalar r2(s2.radius);
112 155999 Scalar lz2(s2.halfLength);
113 // boundaries of the cylinder axis
114
2/4
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155999 times.
✗ Branch 5 not taken.
 155999 Vec3s A(tf2.transform(Vec3s(0, 0, -lz2)));
115
2/4
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155999 times.
✗ Branch 5 not taken.
 155999 Vec3s B(tf2.transform(Vec3s(0, 0, lz2)));
116 // Position of the center of the sphere
117
1/2
✓ Branch 2 taken 155999 times.
✗ Branch 3 not taken.
 155999 Vec3s S(tf1.getTranslation());
118 // axis of the cylinder
119
2/4
✓ Branch 2 taken 155999 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 155999 times.
✗ Branch 6 not taken.
 155999 Vec3s u(tf2.getRotation().col(2));
120 /// @todo a tiny performance improvement could be achieved using the abscissa
121 /// with S as the origin
122
5/10
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155999 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 155999 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 155999 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 155999 times.
 155999 assert((B - A - (s2.halfLength * 2) * u).norm() < eps);
123
2/4
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155999 times.
✗ Branch 5 not taken.
 155999 Vec3s AS(S - A);
124 // abscissa of S on cylinder axis with A as the origin
125
1/2
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
 155999 Scalar s(u.dot(AS));
126
3/6
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155999 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 155999 times.
✗ Branch 8 not taken.
 155999 Vec3s P(A + s * u);
127
2/4
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155999 times.
✗ Branch 5 not taken.
 155999 Vec3s PS(S - P);
128
1/2
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
 155999 Scalar dPS = PS.norm();
129 // Normal to cylinder axis such that plane (A, u, v) contains sphere
130 // center
131
1/2
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
 155999 Vec3s v(0, 0, 0);
132 Scalar dist;
133
1/2
✓ Branch 0 taken 155999 times.
✗ Branch 1 not taken.
 155999 if (dPS > eps) {
134 // S is not on cylinder axis
135
2/4
✓ Branch 1 taken 155999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155999 times.
✗ Branch 5 not taken.
 155999 v = (1 / dPS) * PS;
136 }
137
2/2
✓ Branch 0 taken 76271 times.
✓ Branch 1 taken 79728 times.
 155999 if (s <= 0) {
138
2/2
✓ Branch 0 taken 59 times.
✓ Branch 1 taken 76212 times.
 76271 if (dPS <= r2) {
139 // closest point on cylinder is on cylinder disc basis
140 59 dist = -s - r1;
141
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 59 p1 = S + r1 * u;
142
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 59 p2 = A + dPS * v;
143
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 59 normal = u;
144 } else {
145 // closest point on cylinder is on cylinder circle basis
146
3/6
✓ Branch 1 taken 76212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76212 times.
✗ Branch 8 not taken.
 76212 p2 = A + r2 * v;
147
2/4
✓ Branch 1 taken 76212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76212 times.
✗ Branch 5 not taken.
 76212 Vec3s Sp2(p2 - S);
148
1/2
✓ Branch 1 taken 76212 times.
✗ Branch 2 not taken.
 76212 Scalar dSp2 = Sp2.norm();
149
1/2
✓ Branch 0 taken 76212 times.
✗ Branch 1 not taken.
 76212 if (dSp2 > eps) {
150
2/4
✓ Branch 1 taken 76212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76212 times.
✗ Branch 5 not taken.
 76212 normal = (1 / dSp2) * Sp2;
151
3/6
✓ Branch 1 taken 76212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76212 times.
✗ Branch 8 not taken.
 76212 p1 = S + r1 * normal;
152 76212 dist = dSp2 - r1;
153
3/6
✓ Branch 1 taken 76212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76212 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 76212 times.
 76212 assert(fabs(dist) - (p1 - p2).norm() < eps);
154 } else {
155 // Center of sphere is on cylinder boundary
156 ✗ normal = p2 - .5 * (A + B);
157 ✗ assert(u.dot(normal) >= 0);
158 ✗ normal.normalize();
159 ✗ dist = -r1;
160 ✗ p1 = S + r1 * normal;
161 }
162 }
163
2/2
✓ Branch 0 taken 3549 times.
✓ Branch 1 taken 76179 times.
 79728 } else if (s <= (s2.halfLength * 2)) {
164 // 0 < s <= s2.lz
165
2/4
✓ Branch 1 taken 3549 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3549 times.
✗ Branch 5 not taken.
 3549 normal = -v;
166 3549 dist = dPS - r1 - r2;
167
3/6
✓ Branch 1 taken 3549 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3549 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3549 times.
✗ Branch 8 not taken.
 3549 p2 = P + r2 * v;
168
3/6
✓ Branch 1 taken 3549 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3549 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3549 times.
✗ Branch 8 not taken.
 3549 p1 = S - r1 * v;
169 } else {
170 // lz < s
171
2/2
✓ Branch 0 taken 72 times.
✓ Branch 1 taken 76107 times.
 76179 if (dPS <= r2) {
172 // closest point on cylinder is on cylinder disc basis
173 72 dist = s - (s2.halfLength * 2) - r1;
174
3/6
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
 72 p1 = S - r1 * u;
175
3/6
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72 times.
✗ Branch 8 not taken.
 72 p2 = B + dPS * v;
176
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
 72 normal = -u;
177 } else {
178 // closest point on cylinder is on cylinder circle basis
179
3/6
✓ Branch 1 taken 76107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76107 times.
✗ Branch 8 not taken.
 76107 p2 = B + r2 * v;
180
2/4
✓ Branch 1 taken 76107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76107 times.
✗ Branch 5 not taken.
 76107 Vec3s Sp2(p2 - S);
181
1/2
✓ Branch 1 taken 76107 times.
✗ Branch 2 not taken.
 76107 Scalar dSp2 = Sp2.norm();
182
1/2
✓ Branch 0 taken 76107 times.
✗ Branch 1 not taken.
 76107 if (dSp2 > eps) {
183
2/4
✓ Branch 1 taken 76107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76107 times.
✗ Branch 5 not taken.
 76107 normal = (1 / dSp2) * Sp2;
184
3/6
✓ Branch 1 taken 76107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76107 times.
✗ Branch 8 not taken.
 76107 p1 = S + r1 * normal;
185 76107 dist = dSp2 - r1;
186
3/6
✓ Branch 1 taken 76107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76107 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 76107 times.
 76107 assert(fabs(dist) - (p1 - p2).norm() < eps);
187 } else {
188 // Center of sphere is on cylinder boundary
189 ✗ normal = p2 - .5 * (A + B);
190 ✗ normal.normalize();
191 ✗ p1 = S + r1 * normal;
192 ✗ dist = -r1;
193 }
194 }
195 }
196
197 // Take swept-sphere radius into account
198 155999 const Scalar ssr1 = s1.getSweptSphereRadius();
199 155999 const Scalar ssr2 = s2.getSweptSphereRadius();
200
4/4
✓ Branch 0 taken 155975 times.
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 155969 times.
 155999 if (ssr1 > 0 || ssr2 > 0) {
201
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 p1 += ssr1 * normal;
202
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 p2 -= ssr2 * normal;
203 30 dist -= (ssr1 + ssr2);
204 }
205
206 155999 return dist;
207 }
208
209 /// @param p1 witness point on the first Sphere.
210 /// @param p2 witness point on the second Sphere.
211 /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2).
212 /// @return the distance between the two spheres (negative if penetration).
213 72995 inline Scalar sphereSphereDistance(const Sphere& s1, const Transform3s& tf1,
214 const Sphere& s2, const Transform3s& tf2,
215 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
216 72995 const coal::Vec3s& center1 = tf1.getTranslation();
217 72995 const coal::Vec3s& center2 = tf2.getTranslation();
218 72995 Scalar r1 = (s1.radius + s1.getSweptSphereRadius());
219 72995 Scalar r2 = (s2.radius + s2.getSweptSphereRadius());
220
221
2/4
✓ Branch 1 taken 72995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72995 times.
✗ Branch 5 not taken.
 72995 Vec3s c1c2 = center2 - center1;
222
1/2
✓ Branch 1 taken 72995 times.
✗ Branch 2 not taken.
 72995 Scalar cdist = c1c2.norm();
223
1/2
✓ Branch 1 taken 72995 times.
✗ Branch 2 not taken.
 72995 Vec3s unit(1, 0, 0);
224
4/6
✓ Branch 1 taken 72988 times.
✓ Branch 2 taken 7 times.
✓ Branch 4 taken 72988 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72988 times.
✗ Branch 8 not taken.
 72995 if (cdist > Eigen::NumTraits<Scalar>::epsilon()) unit = c1c2 / cdist;
225 72995 Scalar dist = cdist - r1 - r2;
226
1/2
✓ Branch 1 taken 72995 times.
✗ Branch 2 not taken.
 72995 normal = unit;
227
4/8
✓ Branch 1 taken 72995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72995 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72995 times.
✗ Branch 11 not taken.
 72995 p1.noalias() = center1 + r1 * unit;
228
4/8
✓ Branch 1 taken 72995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72995 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72995 times.
✗ Branch 11 not taken.
 72995 p2.noalias() = center2 - r2 * unit;
229 72995 return dist;
230 }
231
232 /** @brief the minimum distance from a point to a line */
233 138 inline Scalar segmentSqrDistance(const Vec3s& from, const Vec3s& to,
234 const Vec3s& p, Vec3s& nearest) {
235
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 Vec3s diff = p - from;
236
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 Vec3s v = to - from;
237
1/2
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 138 Scalar t = v.dot(diff);
238
239
2/2
✓ Branch 0 taken 126 times.
✓ Branch 1 taken 12 times.
 138 if (t > 0) {
240
1/2
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
 126 Scalar dotVV = v.squaredNorm();
241
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 6 times.
 126 if (t < dotVV) {
242 120 t /= dotVV;
243
2/4
✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
 120 diff -= v * t;
244 } else {
245 6 t = 1;
246
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 diff -= v;
247 }
248 } else
249 12 t = 0;
250
251
4/8
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 138 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 138 times.
✗ Branch 11 not taken.
 138 nearest.noalias() = from + v * t;
252
1/2
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 276 return diff.squaredNorm();
253 }
254
255 /// @brief Whether a point's projection is in a triangle
256 78 inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3,
257 const Vec3s& normal, const Vec3s& p) {
258
2/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
 78 Vec3s edge1(p2 - p1);
259
2/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
 78 Vec3s edge2(p3 - p2);
260
2/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
 78 Vec3s edge3(p1 - p3);
261
262
2/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
 78 Vec3s p1_to_p(p - p1);
263
2/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
 78 Vec3s p2_to_p(p - p2);
264
2/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
 78 Vec3s p3_to_p(p - p3);
265
266
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 Vec3s edge1_normal(edge1.cross(normal));
267
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 Vec3s edge2_normal(edge2.cross(normal));
268
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 Vec3s edge3_normal(edge3.cross(normal));
269
270 Scalar r1, r2, r3;
271
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 r1 = edge1_normal.dot(p1_to_p);
272
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 r2 = edge2_normal.dot(p2_to_p);
273
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 r3 = edge3_normal.dot(p3_to_p);
274
12/12
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 54 times.
✓ Branch 2 taken 14 times.
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 10 times.
✓ Branch 6 taken 54 times.
✓ Branch 7 taken 14 times.
✓ Branch 8 taken 26 times.
✓ Branch 9 taken 28 times.
✓ Branch 10 taken 22 times.
✓ Branch 11 taken 4 times.
 78 if ((r1 > 0 && r2 > 0 && r3 > 0) || (r1 <= 0 && r2 <= 0 && r3 <= 0)) {
275 32 return true;
276 }
277 46 return false;
278 }
279
280 /// @param p1 witness point on the first Sphere.
281 /// @param p2 witness point on the second Sphere.
282 /// @param normal pointing from shape 1 to shape 2 (sphere1 to sphere2).
283 /// @return the distance between the two shapes (negative if penetration).
284 78 inline Scalar sphereTriangleDistance(const Sphere& s, const Transform3s& tf1,
285 const TriangleP& tri,
286 const Transform3s& tf2, Vec3s& p1,
287 Vec3s& p2, Vec3s& normal) {
288
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 const Vec3s& P1 = tf2.transform(tri.a);
289
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 const Vec3s& P2 = tf2.transform(tri.b);
290
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 const Vec3s& P3 = tf2.transform(tri.c);
291
292
3/6
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 78 times.
✗ Branch 8 not taken.
 78 Vec3s tri_normal = (P2 - P1).cross(P3 - P1);
293
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 tri_normal.normalize();
294 78 const Vec3s& center = tf1.getTranslation();
295 // Note: comparing an object with a swept-sphere radius of r1 against another
296 // object with a swept-sphere radius of r2 is equivalent to comparing the
297 // first object with a swept-sphere radius of r1 + r2 against the second
298 // object with a swept-sphere radius of 0.
299 const Scalar& radius =
300 78 s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius();
301
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 78 times.
 78 assert(radius >= 0);
302
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 78 times.
 78 assert(s.radius >= 0);
303
2/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
 78 Vec3s p1_to_center = center - P1;
304
1/2
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
 78 Scalar distance_from_plane = p1_to_center.dot(tri_normal);
305 Vec3s closest_point(
306
2/4
✓ Branch 2 taken 78 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 78 times.
✗ Branch 6 not taken.
 78 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN()));
307 Scalar min_distance_sqr, distance_sqr;
308
309
2/2
✓ Branch 0 taken 38 times.
✓ Branch 1 taken 40 times.
 78 if (distance_from_plane < 0) {
310 38 distance_from_plane *= -1;
311
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 tri_normal *= -1;
312 }
313
314
3/4
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 32 times.
✓ Branch 4 taken 46 times.
 78 if (projectInTriangle(P1, P2, P3, tri_normal, center)) {
315
3/6
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
 32 closest_point = center - tri_normal * distance_from_plane;
316 32 min_distance_sqr = distance_from_plane * distance_from_plane;
317 } else {
318 // Compute distance to each edge and take minimal distance
319
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 Vec3s nearest_on_edge;
320
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point);
321
322
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
323
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 26 times.
 46 if (distance_sqr < min_distance_sqr) {
324 20 min_distance_sqr = distance_sqr;
325
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 closest_point = nearest_on_edge;
326 }
327
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 46 distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
328
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 40 times.
 46 if (distance_sqr < min_distance_sqr) {
329 6 min_distance_sqr = distance_sqr;
330
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 closest_point = nearest_on_edge;
331 }
332 }
333
334
3/6
✓ Branch 1 taken 78 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 78 times.
✗ Branch 8 not taken.
 78 normal = (closest_point - center).normalized();
335
3/6
✓ Branch 2 taken 78 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 78 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 78 times.
✗ Branch 9 not taken.
 78 p1 = center + normal * (s.radius + s.getSweptSphereRadius());
336
3/6
✓ Branch 2 taken 78 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 78 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 78 times.
✗ Branch 9 not taken.
 78 p2 = closest_point - normal * tri.getSweptSphereRadius();
337 78 const Scalar distance = std::sqrt(min_distance_sqr) - radius;
338 78 return distance;
339 }
340
341 /// @param p1 closest (or most penetrating) point on the Halfspace,
342 /// @param p2 closest (or most penetrating) point on the shape,
343 /// @param normal the halfspace normal.
344 /// @return the distance between the two shapes (negative if penetration).
345 6359 inline Scalar halfspaceDistance(const Halfspace& h, const Transform3s& tf1,
346 const ShapeBase& s, const Transform3s& tf2,
347 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
348 // TODO(louis): handle multiple contact points when the halfspace normal is
349 // parallel to the shape's surface (every primitive except sphere and
350 // ellipsoid).
351
352 // Express halfspace in world frame
353
1/2
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
 6359 Halfspace new_h = transform(h, tf1);
354
355 // Express halfspace normal in shape frame
356
3/6
✓ Branch 2 taken 6359 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6359 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6359 times.
✗ Branch 9 not taken.
 6359 Vec3s n_2(tf2.getRotation().transpose() * new_h.n);
357
358 // Compute support of shape in direction of halfspace normal
359 6359 int hint = 0;
360
1/2
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
 6359 p2.noalias() =
361
4/8
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6359 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6359 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6359 times.
✗ Branch 11 not taken.
 12718 getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_2, hint);
362
1/2
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
 6359 p2 = tf2.transform(p2);
363
364
1/2
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
 6359 const Scalar dist = new_h.signedDistance(p2);
365
4/8
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6359 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6359 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6359 times.
✗ Branch 11 not taken.
 6359 p1.noalias() = p2 - dist * new_h.n;
366
2/4
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6359 times.
✗ Branch 5 not taken.
 6359 normal.noalias() = new_h.n;
367
368 6359 const Scalar dummy_precision =
369 std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());
370 COAL_UNUSED_VARIABLE(dummy_precision);
371
2/4
✓ Branch 1 taken 6359 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6359 times.
 6359 assert(new_h.distance(p1) <= dummy_precision);
372 6359 return dist;
373 6359 }
374
375 /// @param p1 closest (or most penetrating) point on the Plane,
376 /// @param p2 closest (or most penetrating) point on the shape,
377 /// @param normal the halfspace normal.
378 /// @return the distance between the two shapes (negative if penetration).
379 5546 inline Scalar planeDistance(const Plane& plane, const Transform3s& tf1,
380 const ShapeBase& s, const Transform3s& tf2,
381 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
382 // TODO(louis): handle multiple contact points when the plane normal is
383 // parallel to the shape's surface (every primitive except sphere and
384 // ellipsoid).
385
386 // Express plane as two halfspaces in world frame
387
1/2
✓ Branch 1 taken 5546 times.
✗ Branch 2 not taken.
 5546 std::array<Halfspace, 2> new_h = transformToHalfspaces(plane, tf1);
388
389 // Express halfspace normals in shape frame
390
3/6
✓ Branch 3 taken 5546 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5546 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5546 times.
✗ Branch 10 not taken.
 5546 Vec3s n_h1(tf2.getRotation().transpose() * new_h[0].n);
391
3/6
✓ Branch 3 taken 5546 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5546 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5546 times.
✗ Branch 10 not taken.
 5546 Vec3s n_h2(tf2.getRotation().transpose() * new_h[1].n);
392
393 // Compute support of shape in direction of halfspace normal and its opposite
394 5546 int hint = 0;
395 Vec3s p2h1 =
396
3/6
✓ Branch 1 taken 5546 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5546 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5546 times.
✗ Branch 8 not taken.
 5546 getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h1, hint);
397
1/2
✓ Branch 1 taken 5546 times.
✗ Branch 2 not taken.
 5546 p2h1 = tf2.transform(p2h1);
398
399 5546 hint = 0;
400 Vec3s p2h2 =
401
3/6
✓ Branch 1 taken 5546 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5546 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5546 times.
✗ Branch 8 not taken.
 5546 getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h2, hint);
402
1/2
✓ Branch 1 taken 5546 times.
✗ Branch 2 not taken.
 5546 p2h2 = tf2.transform(p2h2);
403
404
1/2
✓ Branch 2 taken 5546 times.
✗ Branch 3 not taken.
 5546 Scalar dist1 = new_h[0].signedDistance(p2h1);
405
1/2
✓ Branch 2 taken 5546 times.
✗ Branch 3 not taken.
 5546 Scalar dist2 = new_h[1].signedDistance(p2h2);
406
407 5546 const Scalar dummy_precision =
408 std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());
409 COAL_UNUSED_VARIABLE(dummy_precision);
410
411 Scalar dist;
412
2/2
✓ Branch 0 taken 2406 times.
✓ Branch 1 taken 3140 times.
 5546 if (dist1 >= dist2) {
413 2406 dist = dist1;
414
2/4
✓ Branch 1 taken 2406 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2406 times.
✗ Branch 5 not taken.
 2406 p2.noalias() = p2h1;
415
4/8
✓ Branch 2 taken 2406 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2406 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2406 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2406 times.
✗ Branch 12 not taken.
 2406 p1.noalias() = p2 - dist * new_h[0].n;
416
2/4
✓ Branch 2 taken 2406 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2406 times.
✗ Branch 6 not taken.
 2406 normal.noalias() = new_h[0].n;
417
2/4
✓ Branch 2 taken 2406 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2406 times.
 2406 assert(new_h[0].distance(p1) <= dummy_precision);
418 } else {
419 3140 dist = dist2;
420
2/4
✓ Branch 1 taken 3140 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3140 times.
✗ Branch 5 not taken.
 3140 p2.noalias() = p2h2;
421
4/8
✓ Branch 2 taken 3140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3140 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3140 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3140 times.
✗ Branch 12 not taken.
 3140 p1.noalias() = p2 - dist * new_h[1].n;
422
2/4
✓ Branch 2 taken 3140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3140 times.
✗ Branch 6 not taken.
 3140 normal.noalias() = new_h[1].n;
423
2/4
✓ Branch 2 taken 3140 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3140 times.
 3140 assert(new_h[1].distance(p1) <= dummy_precision);
424 }
425 5546 return dist;
426 5546 }
427
428 /// Taken from book Real Time Collision Detection, from Christer Ericson
429 /// @param pb the witness point on the box surface
430 /// @param ps the witness point on the sphere.
431 /// @param normal pointing from box to sphere
432 /// @return the distance between the two shapes (negative if penetration).
433 303549 inline Scalar boxSphereDistance(const Box& b, const Transform3s& tfb,
434 const Sphere& s, const Transform3s& tfs,
435 Vec3s& pb, Vec3s& ps, Vec3s& normal) {
436 303549 const Vec3s& os = tfs.getTranslation();
437 303549 const Vec3s& ob = tfb.getTranslation();
438 303549 const Matrix3s& Rb = tfb.getRotation();
439
440
1/2
✓ Branch 1 taken 303549 times.
✗ Branch 2 not taken.
 303549 pb = ob;
441
442 303549 bool outside = false;
443
4/8
✓ Branch 1 taken 303549 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 303549 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 303549 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 303549 times.
✗ Branch 11 not taken.
 303549 const Vec3s os_in_b_frame(Rb.transpose() * (os - ob));
444 303549 int axis = -1;
445 303549 Scalar min_d = (std::numeric_limits<Scalar>::max)();
446
2/2
✓ Branch 0 taken 910647 times.
✓ Branch 1 taken 303549 times.
 1214196 for (int i = 0; i < 3; ++i) {
447 Scalar facedist;
448
4/6
✓ Branch 1 taken 910647 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 910647 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 445184 times.
✓ Branch 7 taken 465463 times.
 910647 if (os_in_b_frame(i) < -b.halfSide(i)) { // outside
449
5/10
✓ Branch 1 taken 445184 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 445184 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 445184 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 445184 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 445184 times.
✗ Branch 14 not taken.
 445184 pb.noalias() -= b.halfSide(i) * Rb.col(i);
450 445184 outside = true;
451
4/6
✓ Branch 1 taken 465463 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 465463 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 447612 times.
✓ Branch 7 taken 17851 times.
 465463 } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside
452
5/10
✓ Branch 1 taken 447612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 447612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 447612 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 447612 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 447612 times.
✗ Branch 14 not taken.
 447612 pb.noalias() += b.halfSide(i) * Rb.col(i);
453 447612 outside = true;
454 } else {
455
5/10
✓ Branch 1 taken 17851 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17851 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17851 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17851 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17851 times.
✗ Branch 14 not taken.
 17851 pb.noalias() += os_in_b_frame(i) * Rb.col(i);
456
4/4
✓ Branch 0 taken 3541 times.
✓ Branch 1 taken 14310 times.
✓ Branch 2 taken 3254 times.
✓ Branch 3 taken 14597 times.
 21392 if (!outside &&
457
4/6
✓ Branch 1 taken 3541 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3541 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3254 times.
✓ Branch 7 taken 287 times.
 3541 (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) {
458 3254 axis = i;
459 3254 min_d = facedist;
460 }
461 }
462 }
463
2/4
✓ Branch 1 taken 303549 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 303549 times.
✗ Branch 5 not taken.
 303549 normal = pb - os;
464
1/2
✓ Branch 1 taken 303549 times.
✗ Branch 2 not taken.
 303549 Scalar pdist = normal.norm();
465 Scalar dist; // distance between sphere and box
466
2/2
✓ Branch 0 taken 303444 times.
✓ Branch 1 taken 105 times.
 303549 if (outside) { // pb is on the box
467 303444 dist = pdist - s.radius;
468
1/2
✓ Branch 1 taken 303444 times.
✗ Branch 2 not taken.
 303444 normal /= -pdist;
469 } else { // pb is inside the box
470
3/4
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60 times.
✓ Branch 4 taken 45 times.
 105 if (os_in_b_frame(axis) >= 0) {
471
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 60 normal = Rb.col(axis);
472 } else {
473
3/6
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
 45 normal = -Rb.col(axis);
474 }
475 105 dist = -min_d - s.radius;
476 }
477
3/6
✓ Branch 1 taken 303549 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 303549 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 303549 times.
✗ Branch 8 not taken.
 303549 ps = os - s.radius * normal;
478
4/4
✓ Branch 0 taken 303444 times.
✓ Branch 1 taken 105 times.
✓ Branch 2 taken 1364 times.
✓ Branch 3 taken 302080 times.
 303549 if (!outside || dist <= 0) {
479 // project point pb onto the box's surface
480
3/6
✓ Branch 1 taken 1469 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1469 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1469 times.
✗ Branch 8 not taken.
 1469 pb = ps - dist * normal;
481 }
482
483 // Take swept-sphere radius into account
484 303549 const Scalar ssrb = b.getSweptSphereRadius();
485 303549 const Scalar ssrs = s.getSweptSphereRadius();
486
4/4
✓ Branch 0 taken 303525 times.
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 303519 times.
 303549 if (ssrb > 0 || ssrs > 0) {
487
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 pb += ssrb * normal;
488
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 ps -= ssrs * normal;
489 30 dist -= (ssrb + ssrs);
490 }
491
492 303549 return dist;
493 }
494
495 /// @brief return distance between two halfspaces
496 /// @param p1 the witness point on the first halfspace.
497 /// @param p2 the witness point on the second halfspace.
498 /// @param normal pointing from first to second halfspace.
499 /// @return the distance between the two shapes (negative if penetration).
500 ///
501 /// @note If the two halfspaces don't have the same normal (or opposed
502 /// normals), they collide and their distance is set to -infinity as there is no
503 /// translation that can separate them; they have infinite penetration depth.
504 /// The points p1 and p2 are the same point and represent the origin of the
505 /// intersection line between the objects. The normal is the direction of this
506 /// line.
507 20 inline Scalar halfspaceHalfspaceDistance(const Halfspace& s1,
508 const Transform3s& tf1,
509 const Halfspace& s2,
510 const Transform3s& tf2, Vec3s& p1,
511 Vec3s& p2, Vec3s& normal) {
512
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Halfspace new_s1 = transform(s1, tf1);
513
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Halfspace new_s2 = transform(s2, tf2);
514
515 Scalar distance;
516
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Vec3s dir = (new_s1.n).cross(new_s2.n);
517
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Scalar dir_sq_norm = dir.squaredNorm();
518
519
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 8 times.
 20 if (dir_sq_norm < std::numeric_limits<Scalar>::epsilon()) // parallel
520 {
521
3/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 4 times.
 12 if (new_s1.n.dot(new_s2.n) > 0) {
522 // If the two halfspaces have the same normal, one is inside the other
523 // and they can't be separated. They have inifinte penetration depth.
524 8 distance = -(std::numeric_limits<Scalar>::max)();
525
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
 8 if (new_s1.d <= new_s2.d) {
526
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 normal = new_s1.n;
527
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 p1 = normal * distance;
528
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 p2 = new_s2.n * new_s2.d;
529
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 8 times.
 8 assert(new_s2.distance(p2) <=
530 Eigen::NumTraits<Scalar>::dummy_precision());
531 } else {
532 ✗ normal = -new_s1.n;
533 ✗ p1 << new_s1.n * new_s1.d;
534 ✗ p2 = -(normal * distance);
535 ✗ assert(new_s1.distance(p1) <=
536 Eigen::NumTraits<Scalar>::dummy_precision());
537 }
538 } else {
539 4 distance = -(new_s1.d + new_s2.d);
540
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 normal = new_s1.n;
541
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 p1 = new_s1.n * new_s1.d;
542
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 p2 = new_s2.n * new_s2.d;
543 }
544 } else {
545 // If the halfspaces are not parallel, they are in collision.
546 // Their distance, in the sens of the norm of separation vector, is infinite
547 // (it's impossible to find a translation which separates them)
548 8 distance = -(std::numeric_limits<Scalar>::max)();
549 // p1 and p2 are the same point, corresponding to a point on the
550 // intersection line between the two objects. Normal is the direction of
551 // that line.
552
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 normal = dir;
553 p1 = p2 =
554
7/14
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
 8 ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm;
555 // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection
556 // and https://en.wikipedia.org/wiki/Cross_product
557 }
558
559 // Take swept-sphere radius into account
560 20 const Scalar ssr1 = s1.getSweptSphereRadius();
561 20 const Scalar ssr2 = s2.getSweptSphereRadius();
562
2/4
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
 20 if (ssr1 > 0 || ssr2 > 0) {
563 ✗ p1 += ssr1 * normal;
564 ✗ p2 -= ssr2 * normal;
565 ✗ distance -= (ssr1 + ssr2);
566 }
567
568 20 return distance;
569 20 }
570
571 /// @brief return distance between plane and halfspace.
572 /// @param p1 the witness point on the halfspace.
573 /// @param p2 the witness point on the plane.
574 /// @param normal pointing from halfspace to plane.
575 /// @return the distance between the two shapes (negative if penetration).
576 ///
577 /// @note If plane and halfspace don't have the same normal (or opposed
578 /// normals), they collide and their distance is set to -infinity as there is no
579 /// translation that can separate them; they have infinite penetration depth.
580 /// The points p1 and p2 are the same point and represent the origin of the
581 /// intersection line between the objects. The normal is the direction of this
582 /// line.
583 20 inline Scalar halfspacePlaneDistance(const Halfspace& s1,
584 const Transform3s& tf1, const Plane& s2,
585 const Transform3s& tf2, Vec3s& p1,
586 Vec3s& p2, Vec3s& normal) {
587
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Halfspace new_s1 = transform(s1, tf1);
588
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Plane new_s2 = transform(s2, tf2);
589
590 Scalar distance;
591
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Vec3s dir = (new_s1.n).cross(new_s2.n);
592
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Scalar dir_sq_norm = dir.squaredNorm();
593
594
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 8 times.
 20 if (dir_sq_norm < std::numeric_limits<Scalar>::epsilon()) // parallel
595 {
596
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 normal = new_s1.n;
597
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 distance = new_s1.n.dot(new_s2.n) > 0 ? (new_s2.d - new_s1.d)
598 ✗ : -(new_s1.d + new_s2.d);
599
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 p1 = new_s1.n * new_s1.d;
600
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 p2 = new_s2.n * new_s2.d;
601
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
 12 assert(new_s1.distance(p1) <= Eigen::NumTraits<Scalar>::dummy_precision());
602
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
 12 assert(new_s2.distance(p2) <= Eigen::NumTraits<Scalar>::dummy_precision());
603 } else {
604 // If the halfspace and plane are not parallel, they are in collision.
605 // Their distance, in the sens of the norm of separation vector, is infinite
606 // (it's impossible to find a translation which separates them)
607 8 distance = -(std::numeric_limits<Scalar>::max)();
608 // p1 and p2 are the same point, corresponding to a point on the
609 // intersection line between the two objects. Normal is the direction of
610 // that line.
611
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 normal = dir;
612 p1 = p2 =
613
7/14
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
 8 ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm;
614 // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection
615 // and https://en.wikipedia.org/wiki/Cross_product
616 }
617
618 // Take swept-sphere radius into account
619 20 const Scalar ssr1 = s1.getSweptSphereRadius();
620 20 const Scalar ssr2 = s2.getSweptSphereRadius();
621
2/4
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
 20 if (ssr1 > 0 || ssr2 > 0) {
622 ✗ p1 += ssr1 * normal;
623 ✗ p2 -= ssr2 * normal;
624 ✗ distance -= (ssr1 + ssr2);
625 }
626
627 20 return distance;
628 20 }
629
630 /// @brief return distance between two planes
631 /// @param p1 the witness point on the first plane.
632 /// @param p2 the witness point on the second plane.
633 /// @param normal pointing from first to second plane.
634 /// @return the distance between the two shapes (negative if penetration).
635 ///
636 /// @note If the two planes don't have the same normal (or opposed
637 /// normals), they collide and their distance is set to -infinity as there is no
638 /// translation that can separate them; they have infinite penetration depth.
639 /// The points p1 and p2 are the same point and represent the origin of the
640 /// intersection line between the objects. The normal is the direction of this
641 /// line.
642 20 inline Scalar planePlaneDistance(const Plane& s1, const Transform3s& tf1,
643 const Plane& s2, const Transform3s& tf2,
644 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
645
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Plane new_s1 = transform(s1, tf1);
646
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Plane new_s2 = transform(s2, tf2);
647
648 Scalar distance;
649
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Vec3s dir = (new_s1.n).cross(new_s2.n);
650
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Scalar dir_sq_norm = dir.squaredNorm();
651
652
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 8 times.
 20 if (dir_sq_norm < std::numeric_limits<Scalar>::epsilon()) // parallel
653 {
654
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 p1 = new_s1.n * new_s1.d;
655
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 p2 = new_s2.n * new_s2.d;
656
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
 12 assert(new_s1.distance(p1) <= Eigen::NumTraits<Scalar>::dummy_precision());
657
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 12 times.
 12 assert(new_s2.distance(p2) <= Eigen::NumTraits<Scalar>::dummy_precision());
658
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 distance = (p1 - p2).norm();
659
660
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 4 times.
 12 if (distance > Eigen::NumTraits<Scalar>::dummy_precision()) {
661
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 normal = (p2 - p1).normalized();
662 } else {
663
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 normal = new_s1.n;
664 }
665 } else {
666 // If the planes are not parallel, they are in collision.
667 // Their distance, in the sens of the norm of separation vector, is infinite
668 // (it's impossible to find a translation which separates them)
669 8 distance = -(std::numeric_limits<Scalar>::max)();
670 // p1 and p2 are the same point, corresponding to a point on the
671 // intersection line between the two objects. Normal is the direction of
672 // that line.
673
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 normal = dir;
674 p1 = p2 =
675
7/14
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
 8 ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm;
676 // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection
677 // and https://en.wikipedia.org/wiki/Cross_product
678 }
679
680 // Take swept-sphere radius into account
681 20 const Scalar ssr1 = s1.getSweptSphereRadius();
682 20 const Scalar ssr2 = s2.getSweptSphereRadius();
683
2/4
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
 20 if (ssr1 > 0 || ssr2 > 0) {
684 ✗ p1 += ssr1 * normal;
685 ✗ p2 -= ssr2 * normal;
686 ✗ distance -= (ssr1 + ssr2);
687 }
688
689 20 return distance;
690 20 }
691
692 /// See the prototype below
693 21227 inline Scalar computePenetration(const Vec3s& P1, const Vec3s& P2,
694 const Vec3s& P3, const Vec3s& Q1,
695 const Vec3s& Q2, const Vec3s& Q3,
696 Vec3s& normal) {
697
3/6
✓ Branch 1 taken 21227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21227 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21227 times.
✗ Branch 8 not taken.
 21227 Vec3s u((P2 - P1).cross(P3 - P1));
698
2/4
✓ Branch 1 taken 21227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21227 times.
✗ Branch 5 not taken.
 21227 normal = u.normalized();
699
2/4
✓ Branch 1 taken 21227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21227 times.
✗ Branch 5 not taken.
 21227 Scalar depth1((P1 - Q1).dot(normal));
700
2/4
✓ Branch 1 taken 21227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21227 times.
✗ Branch 5 not taken.
 21227 Scalar depth2((P1 - Q2).dot(normal));
701
2/4
✓ Branch 1 taken 21227 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21227 times.
✗ Branch 5 not taken.
 21227 Scalar depth3((P1 - Q3).dot(normal));
702 42454 return std::max(depth1, std::max(depth2, depth3));
703 }
704
705 // Compute penetration distance and normal of two triangles in collision
706 // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
707 // minimal distance (Q1, Q2, Q3) should be translated along the normal so
708 // that the triangles are collision free.
709 //
710 // Note that we compute here an upper bound of the penetration distance,
711 // not the exact value.
712 inline Scalar computePenetration(const Vec3s& P1, const Vec3s& P2,
713 const Vec3s& P3, const Vec3s& Q1,
714 const Vec3s& Q2, const Vec3s& Q3,
715 const Transform3s& tf1, const Transform3s& tf2,
716 Vec3s& normal) {
717 Vec3s globalP1(tf1.transform(P1));
718 Vec3s globalP2(tf1.transform(P2));
719 Vec3s globalP3(tf1.transform(P3));
720 Vec3s globalQ1(tf2.transform(Q1));
721 Vec3s globalQ2(tf2.transform(Q2));
722 Vec3s globalQ3(tf2.transform(Q3));
723 return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2,
724 globalQ3, normal);
725 }
726
727 } // namespace details
728 } // namespace coal
729
730 #endif // COAL_SRC_NARROWPHASE_DETAILS_H
731