Directory: | ./ |
---|---|
File: | src/narrowphase/details.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 318 | 341 | 93.3% |
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 | 155994 | 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 | 155994 | Scalar r1(s1.radius); | |
111 | 155994 | Scalar r2(s2.radius); | |
112 | 155994 | Scalar lz2(s2.halfLength); | |
113 | // boundaries of the cylinder axis | ||
114 |
2/4✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155994 times.
✗ Branch 5 not taken.
|
155994 | Vec3s A(tf2.transform(Vec3s(0, 0, -lz2))); |
115 |
2/4✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155994 times.
✗ Branch 5 not taken.
|
155994 | Vec3s B(tf2.transform(Vec3s(0, 0, lz2))); |
116 | // Position of the center of the sphere | ||
117 |
1/2✓ Branch 2 taken 155994 times.
✗ Branch 3 not taken.
|
155994 | Vec3s S(tf1.getTranslation()); |
118 | // axis of the cylinder | ||
119 |
2/4✓ Branch 2 taken 155994 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 155994 times.
✗ Branch 6 not taken.
|
155994 | 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 155994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155994 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 155994 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 155994 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 155994 times.
|
155994 | assert((B - A - (s2.halfLength * 2) * u).norm() < eps); |
123 |
2/4✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155994 times.
✗ Branch 5 not taken.
|
155994 | Vec3s AS(S - A); |
124 | // abscissa of S on cylinder axis with A as the origin | ||
125 |
1/2✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
|
155994 | Scalar s(u.dot(AS)); |
126 |
3/6✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155994 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 155994 times.
✗ Branch 8 not taken.
|
155994 | Vec3s P(A + s * u); |
127 |
2/4✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155994 times.
✗ Branch 5 not taken.
|
155994 | Vec3s PS(S - P); |
128 |
1/2✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
|
155994 | 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 155994 times.
✗ Branch 2 not taken.
|
155994 | Vec3s v(0, 0, 0); |
132 | Scalar dist; | ||
133 |
1/2✓ Branch 0 taken 155994 times.
✗ Branch 1 not taken.
|
155994 | if (dPS > eps) { |
134 | // S is not on cylinder axis | ||
135 |
2/4✓ Branch 1 taken 155994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 155994 times.
✗ Branch 5 not taken.
|
155994 | v = (1 / dPS) * PS; |
136 | } | ||
137 |
2/2✓ Branch 0 taken 76269 times.
✓ Branch 1 taken 79725 times.
|
155994 | if (s <= 0) { |
138 |
2/2✓ Branch 0 taken 59 times.
✓ Branch 1 taken 76210 times.
|
76269 | 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 76210 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76210 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76210 times.
✗ Branch 8 not taken.
|
76210 | p2 = A + r2 * v; |
147 |
2/4✓ Branch 1 taken 76210 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76210 times.
✗ Branch 5 not taken.
|
76210 | Vec3s Sp2(p2 - S); |
148 |
1/2✓ Branch 1 taken 76210 times.
✗ Branch 2 not taken.
|
76210 | Scalar dSp2 = Sp2.norm(); |
149 |
1/2✓ Branch 0 taken 76210 times.
✗ Branch 1 not taken.
|
76210 | if (dSp2 > eps) { |
150 |
2/4✓ Branch 1 taken 76210 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76210 times.
✗ Branch 5 not taken.
|
76210 | normal = (1 / dSp2) * Sp2; |
151 |
3/6✓ Branch 1 taken 76210 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76210 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76210 times.
✗ Branch 8 not taken.
|
76210 | p1 = S + r1 * normal; |
152 | 76210 | dist = dSp2 - r1; | |
153 |
3/6✓ Branch 1 taken 76210 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76210 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 76210 times.
|
76210 | 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 76176 times.
|
79725 | } 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 76104 times.
|
76176 | 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 76104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76104 times.
✗ Branch 8 not taken.
|
76104 | p2 = B + r2 * v; |
180 |
2/4✓ Branch 1 taken 76104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76104 times.
✗ Branch 5 not taken.
|
76104 | Vec3s Sp2(p2 - S); |
181 |
1/2✓ Branch 1 taken 76104 times.
✗ Branch 2 not taken.
|
76104 | Scalar dSp2 = Sp2.norm(); |
182 |
1/2✓ Branch 0 taken 76104 times.
✗ Branch 1 not taken.
|
76104 | if (dSp2 > eps) { |
183 |
2/4✓ Branch 1 taken 76104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76104 times.
✗ Branch 5 not taken.
|
76104 | normal = (1 / dSp2) * Sp2; |
184 |
3/6✓ Branch 1 taken 76104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 76104 times.
✗ Branch 8 not taken.
|
76104 | p1 = S + r1 * normal; |
185 | 76104 | dist = dSp2 - r1; | |
186 |
3/6✓ Branch 1 taken 76104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76104 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 76104 times.
|
76104 | 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 | 155994 | const Scalar ssr1 = s1.getSweptSphereRadius(); | |
199 | 155994 | const Scalar ssr2 = s2.getSweptSphereRadius(); | |
200 |
4/4✓ Branch 0 taken 155970 times.
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 155964 times.
|
155994 | 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 | 155994 | 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 | 72992 | inline Scalar sphereSphereDistance(const Sphere& s1, const Transform3s& tf1, | |
214 | const Sphere& s2, const Transform3s& tf2, | ||
215 | Vec3s& p1, Vec3s& p2, Vec3s& normal) { | ||
216 | 72992 | const coal::Vec3s& center1 = tf1.getTranslation(); | |
217 | 72992 | const coal::Vec3s& center2 = tf2.getTranslation(); | |
218 | 72992 | Scalar r1 = (s1.radius + s1.getSweptSphereRadius()); | |
219 | 72992 | Scalar r2 = (s2.radius + s2.getSweptSphereRadius()); | |
220 | |||
221 |
2/4✓ Branch 1 taken 72992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72992 times.
✗ Branch 5 not taken.
|
72992 | Vec3s c1c2 = center2 - center1; |
222 |
1/2✓ Branch 1 taken 72992 times.
✗ Branch 2 not taken.
|
72992 | Scalar cdist = c1c2.norm(); |
223 |
1/2✓ Branch 1 taken 72992 times.
✗ Branch 2 not taken.
|
72992 | Vec3s unit(1, 0, 0); |
224 |
4/6✓ Branch 1 taken 72985 times.
✓ Branch 2 taken 7 times.
✓ Branch 4 taken 72985 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72985 times.
✗ Branch 8 not taken.
|
72992 | if (cdist > Eigen::NumTraits<Scalar>::epsilon()) unit = c1c2 / cdist; |
225 | 72992 | Scalar dist = cdist - r1 - r2; | |
226 |
1/2✓ Branch 1 taken 72992 times.
✗ Branch 2 not taken.
|
72992 | normal = unit; |
227 |
4/8✓ Branch 1 taken 72992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72992 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72992 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72992 times.
✗ Branch 11 not taken.
|
72992 | p1.noalias() = center1 + r1 * unit; |
228 |
4/8✓ Branch 1 taken 72992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72992 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72992 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 72992 times.
✗ Branch 11 not taken.
|
72992 | p2.noalias() = center2 - r2 * unit; |
229 | 72992 | 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 | 303542 | inline Scalar boxSphereDistance(const Box& b, const Transform3s& tfb, | |
434 | const Sphere& s, const Transform3s& tfs, | ||
435 | Vec3s& pb, Vec3s& ps, Vec3s& normal) { | ||
436 | 303542 | const Vec3s& os = tfs.getTranslation(); | |
437 | 303542 | const Vec3s& ob = tfb.getTranslation(); | |
438 | 303542 | const Matrix3s& Rb = tfb.getRotation(); | |
439 | |||
440 |
1/2✓ Branch 1 taken 303542 times.
✗ Branch 2 not taken.
|
303542 | pb = ob; |
441 | |||
442 | 303542 | bool outside = false; | |
443 |
4/8✓ Branch 1 taken 303542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 303542 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 303542 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 303542 times.
✗ Branch 11 not taken.
|
303542 | const Vec3s os_in_b_frame(Rb.transpose() * (os - ob)); |
444 | 303542 | int axis = -1; | |
445 | 303542 | Scalar min_d = (std::numeric_limits<Scalar>::max)(); | |
446 |
2/2✓ Branch 0 taken 910626 times.
✓ Branch 1 taken 303542 times.
|
1214168 | for (int i = 0; i < 3; ++i) { |
447 | Scalar facedist; | ||
448 |
4/6✓ Branch 1 taken 910626 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 910626 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 445169 times.
✓ Branch 7 taken 465457 times.
|
910626 | if (os_in_b_frame(i) < -b.halfSide(i)) { // outside |
449 |
5/10✓ Branch 1 taken 445169 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 445169 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 445169 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 445169 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 445169 times.
✗ Branch 14 not taken.
|
445169 | pb.noalias() -= b.halfSide(i) * Rb.col(i); |
450 | 445169 | outside = true; | |
451 |
4/6✓ Branch 1 taken 465457 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 465457 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 447604 times.
✓ Branch 7 taken 17853 times.
|
465457 | } else if (os_in_b_frame(i) > b.halfSide(i)) { // outside |
452 |
5/10✓ Branch 1 taken 447604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 447604 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 447604 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 447604 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 447604 times.
✗ Branch 14 not taken.
|
447604 | pb.noalias() += b.halfSide(i) * Rb.col(i); |
453 | 447604 | outside = true; | |
454 | } else { | ||
455 |
5/10✓ Branch 1 taken 17853 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17853 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17853 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17853 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17853 times.
✗ Branch 14 not taken.
|
17853 | pb.noalias() += os_in_b_frame(i) * Rb.col(i); |
456 |
4/4✓ Branch 0 taken 3542 times.
✓ Branch 1 taken 14311 times.
✓ Branch 2 taken 3255 times.
✓ Branch 3 taken 14598 times.
|
21395 | if (!outside && |
457 |
4/6✓ Branch 1 taken 3542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3542 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3255 times.
✓ Branch 7 taken 287 times.
|
3542 | (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) { |
458 | 3255 | axis = i; | |
459 | 3255 | min_d = facedist; | |
460 | } | ||
461 | } | ||
462 | } | ||
463 |
2/4✓ Branch 1 taken 303542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 303542 times.
✗ Branch 5 not taken.
|
303542 | normal = pb - os; |
464 |
1/2✓ Branch 1 taken 303542 times.
✗ Branch 2 not taken.
|
303542 | Scalar pdist = normal.norm(); |
465 | Scalar dist; // distance between sphere and box | ||
466 |
2/2✓ Branch 0 taken 303437 times.
✓ Branch 1 taken 105 times.
|
303542 | if (outside) { // pb is on the box |
467 | 303437 | dist = pdist - s.radius; | |
468 |
1/2✓ Branch 1 taken 303437 times.
✗ Branch 2 not taken.
|
303437 | 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 303542 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 303542 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 303542 times.
✗ Branch 8 not taken.
|
303542 | ps = os - s.radius * normal; |
478 |
4/4✓ Branch 0 taken 303437 times.
✓ Branch 1 taken 105 times.
✓ Branch 2 taken 1364 times.
✓ Branch 3 taken 302073 times.
|
303542 | 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 | 303542 | const Scalar ssrb = b.getSweptSphereRadius(); | |
485 | 303542 | const Scalar ssrs = s.getSweptSphereRadius(); | |
486 |
4/4✓ Branch 0 taken 303518 times.
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 303512 times.
|
303542 | 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 | 303542 | 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 | 21227 | 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 |