GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
* All rights reserved. |
||
8 |
* |
||
9 |
* Redistribution and use in source and binary forms, with or without |
||
10 |
* modification, are permitted provided that the following conditions |
||
11 |
* are met: |
||
12 |
* |
||
13 |
* * Redistributions of source code must retain the above copyright |
||
14 |
* notice, this list of conditions and the following disclaimer. |
||
15 |
* * Redistributions in binary form must reproduce the above |
||
16 |
* copyright notice, this list of conditions and the following |
||
17 |
* disclaimer in the documentation and/or other materials provided |
||
18 |
* with the distribution. |
||
19 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
20 |
* contributors may be used to endorse or promote products derived |
||
21 |
* from this software without specific prior written permission. |
||
22 |
* |
||
23 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
24 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
25 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
26 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
27 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
28 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
29 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
30 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
31 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
32 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
33 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
34 |
* POSSIBILITY OF SUCH DAMAGE. |
||
35 |
*/ |
||
36 |
/** \author Jia Pan, Florent Lamiraux */ |
||
37 |
|||
38 |
#ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H |
||
39 |
#define HPP_FCL_SRC_NARROWPHASE_DETAILS_H |
||
40 |
|||
41 |
#include <hpp/fcl/internal/traversal_node_setup.h> |
||
42 |
#include <hpp/fcl/narrowphase/narrowphase.h> |
||
43 |
|||
44 |
namespace hpp { |
||
45 |
namespace fcl { |
||
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 |
33 |
static inline void lineSegmentPointClosestToPoint(const Vec3f& p, |
|
52 |
const Vec3f& s1, |
||
53 |
const Vec3f& s2, Vec3f& sp) { |
||
54 |
✓✗✓✗ |
33 |
Vec3f v = s2 - s1; |
55 |
✓✗✓✗ |
33 |
Vec3f w = p - s1; |
56 |
|||
57 |
✓✗ | 33 |
FCL_REAL c1 = w.dot(v); |
58 |
✓✗ | 33 |
FCL_REAL c2 = v.dot(v); |
59 |
|||
60 |
✓✓ | 33 |
if (c1 <= 0) { |
61 |
✓✗ | 1 |
sp = s1; |
62 |
✗✓ | 32 |
} else if (c2 <= c1) { |
63 |
sp = s2; |
||
64 |
} else { |
||
65 |
32 |
FCL_REAL b = c1 / c2; |
|
66 |
✓✗✓✗ ✓✗ |
32 |
Vec3f Pb = s1 + v * b; |
67 |
✓✗ | 32 |
sp = Pb; |
68 |
} |
||
69 |
33 |
} |
|
70 |
|||
71 |
inline bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1, |
||
72 |
const Capsule& s2, const Transform3f& tf2, |
||
73 |
FCL_REAL& distance, Vec3f* contact_points, |
||
74 |
Vec3f* normal_) { |
||
75 |
Vec3f pos1( |
||
76 |
tf2.transform(Vec3f(0., 0., s2.halfLength))); // from distance function |
||
77 |
Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength))); |
||
78 |
Vec3f s_c = tf1.getTranslation(); |
||
79 |
|||
80 |
Vec3f segment_point; |
||
81 |
|||
82 |
lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); |
||
83 |
Vec3f diff = s_c - segment_point; |
||
84 |
|||
85 |
FCL_REAL diffN = diff.norm(); |
||
86 |
distance = diffN - s1.radius - s2.radius; |
||
87 |
|||
88 |
if (distance > 0) return false; |
||
89 |
|||
90 |
// Vec3f normal (-diff.normalized()); |
||
91 |
|||
92 |
if (normal_) *normal_ = -diff / diffN; |
||
93 |
|||
94 |
if (contact_points) { |
||
95 |
*contact_points = segment_point + diff * s2.radius; |
||
96 |
} |
||
97 |
|||
98 |
return true; |
||
99 |
} |
||
100 |
|||
101 |
33 |
inline bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, |
|
102 |
const Capsule& s2, const Transform3f& tf2, |
||
103 |
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, |
||
104 |
Vec3f& normal) { |
||
105 |
✓✗✓✗ |
33 |
Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength))); |
106 |
✓✗✓✗ |
33 |
Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength))); |
107 |
✓✗ | 33 |
Vec3f s_c = tf1.getTranslation(); |
108 |
|||
109 |
✓✗ | 33 |
Vec3f segment_point; |
110 |
|||
111 |
✓✗ | 33 |
lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point); |
112 |
✓✗✓✗ |
33 |
normal = segment_point - s_c; |
113 |
✓✗ | 33 |
FCL_REAL norm(normal.norm()); |
114 |
33 |
dist = norm - s1.radius - s2.radius; |
|
115 |
|||
116 |
static const FCL_REAL eps(std::numeric_limits<FCL_REAL>::epsilon()); |
||
117 |
✓✓ | 33 |
if (norm > eps) { |
118 |
✓✗ | 25 |
normal.normalize(); |
119 |
} else { |
||
120 |
✓✗✓✗ ✓✗ |
8 |
normal << 1, 0, 0; |
121 |
} |
||
122 |
✓✗✓✗ ✓✗ |
33 |
p1 = s_c + normal * s1.radius; |
123 |
✓✗✓✗ ✓✗ |
33 |
p2 = segment_point - normal * s2.radius; |
124 |
|||
125 |
✓✓ | 33 |
if (dist <= 0) { |
126 |
✓✗✓✗ ✓✗✓✗ |
22 |
p1 = p2 = .5 * (p1 + p2); |
127 |
22 |
return false; |
|
128 |
} |
||
129 |
11 |
return true; |
|
130 |
} |
||
131 |
|||
132 |
153751 |
inline bool sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1, |
|
133 |
const Cylinder& s2, const Transform3f& tf2, |
||
134 |
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, |
||
135 |
Vec3f& normal) { |
||
136 |
static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon())); |
||
137 |
153751 |
FCL_REAL r1(s1.radius); |
|
138 |
153751 |
FCL_REAL r2(s2.radius); |
|
139 |
153751 |
FCL_REAL lz2(s2.halfLength); |
|
140 |
// boundaries of the cylinder axis |
||
141 |
✓✗✓✗ |
153751 |
Vec3f A(tf2.transform(Vec3f(0, 0, -lz2))); |
142 |
✓✗✓✗ |
153751 |
Vec3f B(tf2.transform(Vec3f(0, 0, lz2))); |
143 |
// Position of the center of the sphere |
||
144 |
✓✗ | 153751 |
Vec3f S(tf1.getTranslation()); |
145 |
// axis of the cylinder |
||
146 |
✓✗✓✗ |
153751 |
Vec3f u(tf2.getRotation().col(2)); |
147 |
/// @todo a tiny performance improvement could be achieved using the abscissa |
||
148 |
/// with S as the origin |
||
149 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
153751 |
assert((B - A - (s2.halfLength * 2) * u).norm() < eps); |
150 |
✓✗✓✗ |
153751 |
Vec3f AS(S - A); |
151 |
// abscissa of S on cylinder axis with A as the origin |
||
152 |
✓✗ | 153751 |
FCL_REAL s(u.dot(AS)); |
153 |
✓✗✓✗ ✓✗ |
153751 |
Vec3f P(A + s * u); |
154 |
✓✗✓✗ |
153751 |
Vec3f PS(S - P); |
155 |
✓✗ | 153751 |
FCL_REAL dPS = PS.norm(); |
156 |
// Normal to cylinder axis such that plane (A, u, v) contains sphere |
||
157 |
// center |
||
158 |
✓✗ | 153751 |
Vec3f v(0, 0, 0); |
159 |
✓✗ | 153751 |
if (dPS > eps) { |
160 |
// S is not on cylinder axis |
||
161 |
✓✗✓✗ |
153751 |
v = (1 / dPS) * PS; |
162 |
} |
||
163 |
✓✓ | 153751 |
if (s <= 0) { |
164 |
✓✓ | 74462 |
if (dPS <= r2) { |
165 |
// closest point on cylinder is on cylinder disc basis |
||
166 |
32 |
dist = -s - r1; |
|
167 |
✓✗✓✗ ✓✗ |
32 |
p1 = S + r1 * u; |
168 |
✓✗✓✗ ✓✗ |
32 |
p2 = A + dPS * v; |
169 |
✓✗ | 32 |
normal = u; |
170 |
} else { |
||
171 |
// closest point on cylinder is on cylinder circle basis |
||
172 |
✓✗✓✗ ✓✗ |
74430 |
p2 = A + r2 * v; |
173 |
✓✗✓✗ |
74430 |
Vec3f Sp2(p2 - S); |
174 |
✓✗ | 74430 |
FCL_REAL dSp2 = Sp2.norm(); |
175 |
✓✗ | 74430 |
if (dSp2 > eps) { |
176 |
✓✗✓✗ |
74430 |
normal = (1 / dSp2) * Sp2; |
177 |
✓✗✓✗ ✓✗ |
74430 |
p1 = S + r1 * normal; |
178 |
74430 |
dist = dSp2 - r1; |
|
179 |
✓✗✓✗ ✗✓ |
74430 |
assert(fabs(dist) - (p1 - p2).norm() < eps); |
180 |
} else { |
||
181 |
// Center of sphere is on cylinder boundary |
||
182 |
normal = .5 * (A + B) - p2; |
||
183 |
normal.normalize(); |
||
184 |
p1 = p2; |
||
185 |
dist = -r1; |
||
186 |
} |
||
187 |
} |
||
188 |
✓✓ | 79289 |
} else if (s <= (s2.halfLength * 2)) { |
189 |
// 0 < s <= s2.lz |
||
190 |
✓✗✓✗ |
3335 |
normal = -v; |
191 |
3335 |
dist = dPS - r1 - r2; |
|
192 |
✓✓ | 3335 |
if (dPS <= r2) { |
193 |
// Sphere center is inside cylinder |
||
194 |
✓✗✓✗ |
3 |
p1 = p2 = S; |
195 |
} else { |
||
196 |
✓✗✓✗ ✓✗ |
3332 |
p2 = P + r2 * v; |
197 |
✓✗✓✗ ✓✗ |
3332 |
p1 = S - r1 * v; |
198 |
} |
||
199 |
} else { |
||
200 |
// lz < s |
||
201 |
✓✓ | 75954 |
if (dPS <= r2) { |
202 |
// closest point on cylinder is on cylinder disc basis |
||
203 |
21 |
dist = s - (s2.halfLength * 2) - r1; |
|
204 |
✓✗✓✗ ✓✗ |
21 |
p1 = S - r1 * u; |
205 |
✓✗✓✗ ✓✗ |
21 |
p2 = B + dPS * v; |
206 |
✓✗✓✗ |
21 |
normal = -u; |
207 |
} else { |
||
208 |
// closest point on cylinder is on cylinder circle basis |
||
209 |
✓✗✓✗ ✓✗ |
75933 |
p2 = B + r2 * v; |
210 |
✓✗✓✗ |
75933 |
Vec3f Sp2(p2 - S); |
211 |
✓✗ | 75933 |
FCL_REAL dSp2 = Sp2.norm(); |
212 |
✓✗ | 75933 |
if (dSp2 > eps) { |
213 |
✓✗✓✗ |
75933 |
normal = (1 / dSp2) * Sp2; |
214 |
✓✗✓✗ ✓✗ |
75933 |
p1 = S + r1 * normal; |
215 |
75933 |
dist = dSp2 - r1; |
|
216 |
✓✗✓✗ ✗✓ |
75933 |
assert(fabs(dist) - (p1 - p2).norm() < eps); |
217 |
} else { |
||
218 |
// Center of sphere is on cylinder boundary |
||
219 |
normal = .5 * (A + B) - p2; |
||
220 |
normal.normalize(); |
||
221 |
p1 = p2; |
||
222 |
dist = -r1; |
||
223 |
} |
||
224 |
} |
||
225 |
} |
||
226 |
✓✓ | 153751 |
if (dist < 0) { |
227 |
✓✗✓✗ ✓✗✓✗ |
141 |
p1 = p2 = .5 * (p1 + p2); |
228 |
} |
||
229 |
153751 |
return (dist > 0); |
|
230 |
} |
||
231 |
|||
232 |
inline bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, |
||
233 |
const Sphere& s2, const Transform3f& tf2, |
||
234 |
FCL_REAL& distance, Vec3f* contact_points, |
||
235 |
Vec3f* normal) { |
||
236 |
const Vec3f diff = tf2.getTranslation() - tf1.getTranslation(); |
||
237 |
FCL_REAL len = diff.norm(); |
||
238 |
distance = len - s1.radius - s2.radius; |
||
239 |
if (distance > 0) return false; |
||
240 |
|||
241 |
// If the centers of two sphere are at the same position, the normal is (0, 0, |
||
242 |
// 0). Otherwise, normal is pointing from center of object 1 to center of |
||
243 |
// object 2 |
||
244 |
if (normal) { |
||
245 |
if (len > 0) |
||
246 |
*normal = diff / len; |
||
247 |
else |
||
248 |
*normal = diff; |
||
249 |
} |
||
250 |
|||
251 |
if (contact_points) |
||
252 |
*contact_points = |
||
253 |
tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius); |
||
254 |
|||
255 |
return true; |
||
256 |
} |
||
257 |
|||
258 |
24 |
inline bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, |
|
259 |
const Sphere& s2, const Transform3f& tf2, |
||
260 |
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, |
||
261 |
Vec3f& normal) { |
||
262 |
24 |
const Vec3f& o1 = tf1.getTranslation(); |
|
263 |
24 |
const Vec3f& o2 = tf2.getTranslation(); |
|
264 |
✓✗✓✗ |
24 |
Vec3f diff = o1 - o2; |
265 |
✓✗ | 24 |
FCL_REAL len = diff.norm(); |
266 |
✓✗✓✗ ✓✗ |
24 |
normal = -diff / len; |
267 |
24 |
dist = len - s1.radius - s2.radius; |
|
268 |
|||
269 |
✓✗✓✗ ✓✗✓✗ |
24 |
p1.noalias() = o1 + normal * s1.radius; |
270 |
✓✗✓✗ ✓✗✓✗ |
24 |
p2.noalias() = o2 - normal * s2.radius; |
271 |
|||
272 |
24 |
return (dist >= 0); |
|
273 |
} |
||
274 |
|||
275 |
/** @brief the minimum distance from a point to a line */ |
||
276 |
81 |
inline FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to, |
|
277 |
const Vec3f& p, Vec3f& nearest) { |
||
278 |
✓✗✓✗ |
81 |
Vec3f diff = p - from; |
279 |
✓✗✓✗ |
81 |
Vec3f v = to - from; |
280 |
✓✗ | 81 |
FCL_REAL t = v.dot(diff); |
281 |
|||
282 |
✓✓ | 81 |
if (t > 0) { |
283 |
✓✗ | 69 |
FCL_REAL dotVV = v.squaredNorm(); |
284 |
✓✓ | 69 |
if (t < dotVV) { |
285 |
63 |
t /= dotVV; |
|
286 |
✓✗✓✗ |
63 |
diff -= v * t; |
287 |
} else { |
||
288 |
6 |
t = 1; |
|
289 |
✓✗ | 6 |
diff -= v; |
290 |
} |
||
291 |
} else |
||
292 |
12 |
t = 0; |
|
293 |
|||
294 |
✓✗✓✗ ✓✗✓✗ |
81 |
nearest.noalias() = from + v * t; |
295 |
✓✗ | 162 |
return diff.squaredNorm(); |
296 |
} |
||
297 |
|||
298 |
/// @brief Whether a point's projection is in a triangle |
||
299 |
34 |
inline bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, |
|
300 |
const Vec3f& normal, const Vec3f& p) { |
||
301 |
✓✗✓✗ |
34 |
Vec3f edge1(p2 - p1); |
302 |
✓✗✓✗ |
34 |
Vec3f edge2(p3 - p2); |
303 |
✓✗✓✗ |
34 |
Vec3f edge3(p1 - p3); |
304 |
|||
305 |
✓✗✓✗ |
34 |
Vec3f p1_to_p(p - p1); |
306 |
✓✗✓✗ |
34 |
Vec3f p2_to_p(p - p2); |
307 |
✓✗✓✗ |
34 |
Vec3f p3_to_p(p - p3); |
308 |
|||
309 |
✓✗ | 34 |
Vec3f edge1_normal(edge1.cross(normal)); |
310 |
✓✗ | 34 |
Vec3f edge2_normal(edge2.cross(normal)); |
311 |
✓✗ | 34 |
Vec3f edge3_normal(edge3.cross(normal)); |
312 |
|||
313 |
FCL_REAL r1, r2, r3; |
||
314 |
✓✗ | 34 |
r1 = edge1_normal.dot(p1_to_p); |
315 |
✓✗ | 34 |
r2 = edge2_normal.dot(p2_to_p); |
316 |
✓✗ | 34 |
r3 = edge3_normal.dot(p3_to_p); |
317 |
✓✓✓✓ ✓✓✓✓ ✓✓✓✓ |
34 |
if ((r1 > 0 && r2 > 0 && r3 > 0) || (r1 <= 0 && r2 <= 0 && r3 <= 0)) |
318 |
7 |
return true; |
|
319 |
27 |
return false; |
|
320 |
} |
||
321 |
|||
322 |
// Intersection between sphere and triangle |
||
323 |
// Sphere is in position tf1, Triangle is expressed in global frame |
||
324 |
34 |
inline bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf1, |
|
325 |
const Vec3f& P1, const Vec3f& P2, |
||
326 |
const Vec3f& P3, FCL_REAL& distance, |
||
327 |
Vec3f& p1, Vec3f& p2, Vec3f& normal_) { |
||
328 |
✓✗✓✗ ✓✗ |
34 |
Vec3f normal = (P2 - P1).cross(P3 - P1); |
329 |
✓✗ | 34 |
normal.normalize(); |
330 |
34 |
const Vec3f& center = tf1.getTranslation(); |
|
331 |
34 |
const FCL_REAL& radius = s.radius; |
|
332 |
✗✓ | 34 |
assert(radius >= 0); |
333 |
✓✗✓✗ |
34 |
Vec3f p1_to_center = center - P1; |
334 |
✓✗ | 34 |
FCL_REAL distance_from_plane = p1_to_center.dot(normal); |
335 |
Vec3f closest_point( |
||
336 |
✓✗✓✗ |
34 |
Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN())); |
337 |
FCL_REAL min_distance_sqr, distance_sqr; |
||
338 |
|||
339 |
✓✓ | 34 |
if (distance_from_plane < 0) { |
340 |
11 |
distance_from_plane *= -1; |
|
341 |
✓✗ | 11 |
normal *= -1; |
342 |
} |
||
343 |
|||
344 |
✓✗✓✓ |
34 |
if (projectInTriangle(P1, P2, P3, normal, center)) { |
345 |
✓✗✓✗ ✓✗ |
7 |
closest_point = center - normal * distance_from_plane; |
346 |
7 |
min_distance_sqr = distance_from_plane; |
|
347 |
} else { |
||
348 |
// Compute distance to each each and take minimal distance |
||
349 |
✓✗ | 27 |
Vec3f nearest_on_edge; |
350 |
✓✗ | 27 |
min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point); |
351 |
|||
352 |
✓✗ | 27 |
distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); |
353 |
✓✓ | 27 |
if (distance_sqr < min_distance_sqr) { |
354 |
20 |
min_distance_sqr = distance_sqr; |
|
355 |
✓✗ | 20 |
closest_point = nearest_on_edge; |
356 |
} |
||
357 |
✓✗ | 27 |
distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge); |
358 |
✓✓ | 27 |
if (distance_sqr < min_distance_sqr) { |
359 |
6 |
min_distance_sqr = distance_sqr; |
|
360 |
✓✗ | 6 |
closest_point = nearest_on_edge; |
361 |
} |
||
362 |
} |
||
363 |
|||
364 |
✓✓ | 34 |
if (min_distance_sqr < radius * radius) { |
365 |
// Collision |
||
366 |
✓✗✓✗ ✓✗ |
12 |
normal_ = (closest_point - center).normalized(); |
367 |
✓✗✓✗ |
12 |
p1 = p2 = closest_point; |
368 |
12 |
distance = sqrt(min_distance_sqr) - radius; |
|
369 |
✗✓ | 12 |
assert(distance < 0); |
370 |
12 |
return true; |
|
371 |
} else { |
||
372 |
✓✗✓✗ ✓✗ |
22 |
normal_ = (closest_point - center).normalized(); |
373 |
✓✗✓✗ ✓✗ |
22 |
p1 = center + normal_ * radius; |
374 |
✓✗ | 22 |
p2 = closest_point; |
375 |
22 |
distance = sqrt(min_distance_sqr) - radius; |
|
376 |
✗✓ | 22 |
assert(distance >= 0); |
377 |
22 |
return false; |
|
378 |
} |
||
379 |
} |
||
380 |
|||
381 |
inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf, |
||
382 |
const Vec3f& P1, const Vec3f& P2, |
||
383 |
const Vec3f& P3, FCL_REAL* dist) { |
||
384 |
// from geometric tools, very different from the collision code. |
||
385 |
|||
386 |
const Vec3f& center = tf.getTranslation(); |
||
387 |
FCL_REAL radius = sp.radius; |
||
388 |
Vec3f diff = P1 - center; |
||
389 |
Vec3f edge0 = P2 - P1; |
||
390 |
Vec3f edge1 = P3 - P1; |
||
391 |
FCL_REAL a00 = edge0.squaredNorm(); |
||
392 |
FCL_REAL a01 = edge0.dot(edge1); |
||
393 |
FCL_REAL a11 = edge1.squaredNorm(); |
||
394 |
FCL_REAL b0 = diff.dot(edge0); |
||
395 |
FCL_REAL b1 = diff.dot(edge1); |
||
396 |
FCL_REAL c = diff.squaredNorm(); |
||
397 |
FCL_REAL det = fabs(a00 * a11 - a01 * a01); |
||
398 |
FCL_REAL s = a01 * b1 - a11 * b0; |
||
399 |
FCL_REAL t = a01 * b0 - a00 * b1; |
||
400 |
|||
401 |
FCL_REAL sqr_dist; |
||
402 |
|||
403 |
if (s + t <= det) { |
||
404 |
if (s < 0) { |
||
405 |
if (t < 0) // region 4 |
||
406 |
{ |
||
407 |
if (b0 < 0) { |
||
408 |
t = 0; |
||
409 |
if (-b0 >= a00) { |
||
410 |
s = 1; |
||
411 |
sqr_dist = a00 + 2 * b0 + c; |
||
412 |
} else { |
||
413 |
s = -b0 / a00; |
||
414 |
sqr_dist = b0 * s + c; |
||
415 |
} |
||
416 |
} else { |
||
417 |
s = 0; |
||
418 |
if (b1 >= 0) { |
||
419 |
t = 0; |
||
420 |
sqr_dist = c; |
||
421 |
} else if (-b1 >= a11) { |
||
422 |
t = 1; |
||
423 |
sqr_dist = a11 + 2 * b1 + c; |
||
424 |
} else { |
||
425 |
t = -b1 / a11; |
||
426 |
sqr_dist = b1 * t + c; |
||
427 |
} |
||
428 |
} |
||
429 |
} else // region 3 |
||
430 |
{ |
||
431 |
s = 0; |
||
432 |
if (b1 >= 0) { |
||
433 |
t = 0; |
||
434 |
sqr_dist = c; |
||
435 |
} else if (-b1 >= a11) { |
||
436 |
t = 1; |
||
437 |
sqr_dist = a11 + 2 * b1 + c; |
||
438 |
} else { |
||
439 |
t = -b1 / a11; |
||
440 |
sqr_dist = b1 * t + c; |
||
441 |
} |
||
442 |
} |
||
443 |
} else if (t < 0) // region 5 |
||
444 |
{ |
||
445 |
t = 0; |
||
446 |
if (b0 >= 0) { |
||
447 |
s = 0; |
||
448 |
sqr_dist = c; |
||
449 |
} else if (-b0 >= a00) { |
||
450 |
s = 1; |
||
451 |
sqr_dist = a00 + 2 * b0 + c; |
||
452 |
} else { |
||
453 |
s = -b0 / a00; |
||
454 |
sqr_dist = b0 * s + c; |
||
455 |
} |
||
456 |
} else // region 0 |
||
457 |
{ |
||
458 |
// minimum at interior point |
||
459 |
FCL_REAL inv_det = (1) / det; |
||
460 |
s *= inv_det; |
||
461 |
t *= inv_det; |
||
462 |
sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + |
||
463 |
t * (a01 * s + a11 * t + 2 * b1) + c; |
||
464 |
} |
||
465 |
} else { |
||
466 |
FCL_REAL tmp0, tmp1, numer, denom; |
||
467 |
|||
468 |
if (s < 0) // region 2 |
||
469 |
{ |
||
470 |
tmp0 = a01 + b0; |
||
471 |
tmp1 = a11 + b1; |
||
472 |
if (tmp1 > tmp0) { |
||
473 |
numer = tmp1 - tmp0; |
||
474 |
denom = a00 - 2 * a01 + a11; |
||
475 |
if (numer >= denom) { |
||
476 |
s = 1; |
||
477 |
t = 0; |
||
478 |
sqr_dist = a00 + 2 * b0 + c; |
||
479 |
} else { |
||
480 |
s = numer / denom; |
||
481 |
t = 1 - s; |
||
482 |
sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + |
||
483 |
t * (a01 * s + a11 * t + 2 * b1) + c; |
||
484 |
} |
||
485 |
} else { |
||
486 |
s = 0; |
||
487 |
if (tmp1 <= 0) { |
||
488 |
t = 1; |
||
489 |
sqr_dist = a11 + 2 * b1 + c; |
||
490 |
} else if (b1 >= 0) { |
||
491 |
t = 0; |
||
492 |
sqr_dist = c; |
||
493 |
} else { |
||
494 |
t = -b1 / a11; |
||
495 |
sqr_dist = b1 * t + c; |
||
496 |
} |
||
497 |
} |
||
498 |
} else if (t < 0) // region 6 |
||
499 |
{ |
||
500 |
tmp0 = a01 + b1; |
||
501 |
tmp1 = a00 + b0; |
||
502 |
if (tmp1 > tmp0) { |
||
503 |
numer = tmp1 - tmp0; |
||
504 |
denom = a00 - 2 * a01 + a11; |
||
505 |
if (numer >= denom) { |
||
506 |
t = 1; |
||
507 |
s = 0; |
||
508 |
sqr_dist = a11 + 2 * b1 + c; |
||
509 |
} else { |
||
510 |
t = numer / denom; |
||
511 |
s = 1 - t; |
||
512 |
sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + |
||
513 |
t * (a01 * s + a11 * t + 2 * b1) + c; |
||
514 |
} |
||
515 |
} else { |
||
516 |
t = 0; |
||
517 |
if (tmp1 <= 0) { |
||
518 |
s = 1; |
||
519 |
sqr_dist = a00 + 2 * b0 + c; |
||
520 |
} else if (b0 >= 0) { |
||
521 |
s = 0; |
||
522 |
sqr_dist = c; |
||
523 |
} else { |
||
524 |
s = -b0 / a00; |
||
525 |
sqr_dist = b0 * s + c; |
||
526 |
} |
||
527 |
} |
||
528 |
} else // region 1 |
||
529 |
{ |
||
530 |
numer = a11 + b1 - a01 - b0; |
||
531 |
if (numer <= 0) { |
||
532 |
s = 0; |
||
533 |
t = 1; |
||
534 |
sqr_dist = a11 + 2 * b1 + c; |
||
535 |
} else { |
||
536 |
denom = a00 - 2 * a01 + a11; |
||
537 |
if (numer >= denom) { |
||
538 |
s = 1; |
||
539 |
t = 0; |
||
540 |
sqr_dist = a00 + 2 * b0 + c; |
||
541 |
} else { |
||
542 |
s = numer / denom; |
||
543 |
t = 1 - s; |
||
544 |
sqr_dist = s * (a00 * s + a01 * t + 2 * b0) + |
||
545 |
t * (a01 * s + a11 * t + 2 * b1) + c; |
||
546 |
} |
||
547 |
} |
||
548 |
} |
||
549 |
} |
||
550 |
|||
551 |
// Account for numerical round-off error. |
||
552 |
if (sqr_dist < 0) sqr_dist = 0; |
||
553 |
|||
554 |
if (sqr_dist > radius * radius) { |
||
555 |
if (dist) *dist = std::sqrt(sqr_dist) - radius; |
||
556 |
return true; |
||
557 |
} else { |
||
558 |
if (dist) *dist = -1; |
||
559 |
return false; |
||
560 |
} |
||
561 |
} |
||
562 |
|||
563 |
inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf, |
||
564 |
const Vec3f& P1, const Vec3f& P2, |
||
565 |
const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, |
||
566 |
Vec3f* p2) { |
||
567 |
if (p1 || p2) { |
||
568 |
const Vec3f& o = tf.getTranslation(); |
||
569 |
Project::ProjectResult result; |
||
570 |
result = Project::projectTriangle(P1, P2, P3, o); |
||
571 |
if (result.sqr_distance > sp.radius * sp.radius) { |
||
572 |
if (dist) *dist = std::sqrt(result.sqr_distance) - sp.radius; |
||
573 |
Vec3f project_p = P1 * result.parameterization[0] + |
||
574 |
P2 * result.parameterization[1] + |
||
575 |
P3 * result.parameterization[2]; |
||
576 |
Vec3f dir = o - project_p; |
||
577 |
dir.normalize(); |
||
578 |
if (p1) { |
||
579 |
*p1 = o - dir * sp.radius; |
||
580 |
} |
||
581 |
if (p2) *p2 = project_p; |
||
582 |
return true; |
||
583 |
} else |
||
584 |
return false; |
||
585 |
} else { |
||
586 |
return sphereTriangleDistance(sp, tf, P1, P2, P3, dist); |
||
587 |
} |
||
588 |
} |
||
589 |
|||
590 |
inline bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1, |
||
591 |
const Vec3f& P1, const Vec3f& P2, |
||
592 |
const Vec3f& P3, const Transform3f& tf2, |
||
593 |
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) { |
||
594 |
bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), |
||
595 |
tf2.transform(P2), |
||
596 |
tf2.transform(P3), dist, p1, p2); |
||
597 |
return res; |
||
598 |
} |
||
599 |
|||
600 |
struct HPP_FCL_LOCAL ContactPoint { |
||
601 |
Vec3f normal; |
||
602 |
Vec3f point; |
||
603 |
FCL_REAL depth; |
||
604 |
ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) |
||
605 |
: normal(n), point(p), depth(d) {} |
||
606 |
}; |
||
607 |
|||
608 |
static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua, |
||
609 |
const Vec3f& pb, const Vec3f& ub, |
||
610 |
FCL_REAL* alpha, FCL_REAL* beta) { |
||
611 |
Vec3f p = pb - pa; |
||
612 |
FCL_REAL uaub = ua.dot(ub); |
||
613 |
FCL_REAL q1 = ua.dot(p); |
||
614 |
FCL_REAL q2 = -ub.dot(p); |
||
615 |
FCL_REAL d = 1 - uaub * uaub; |
||
616 |
if (d <= (FCL_REAL)(0.0001f)) { |
||
617 |
*alpha = 0; |
||
618 |
*beta = 0; |
||
619 |
} else { |
||
620 |
d = 1 / d; |
||
621 |
*alpha = (q1 + uaub * q2) * d; |
||
622 |
*beta = (uaub * q1 + q2) * d; |
||
623 |
} |
||
624 |
} |
||
625 |
|||
626 |
// find all the intersection points between the 2D rectangle with vertices |
||
627 |
// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), |
||
628 |
// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). |
||
629 |
// |
||
630 |
// the intersection points are returned as x,y pairs in the 'ret' array. |
||
631 |
// the number of intersection points is returned by the function (this will |
||
632 |
// be in the range 0 to 8). |
||
633 |
static unsigned int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], |
||
634 |
FCL_REAL ret[16]) { |
||
635 |
// q (and r) contain nq (and nr) coordinate points for the current (and |
||
636 |
// chopped) polygons |
||
637 |
unsigned int nq = 4, nr = 0; |
||
638 |
FCL_REAL buffer[16]; |
||
639 |
FCL_REAL* q = p; |
||
640 |
FCL_REAL* r = ret; |
||
641 |
for (int dir = 0; dir <= 1; ++dir) { |
||
642 |
// direction notation: xy[0] = x axis, xy[1] = y axis |
||
643 |
for (int sign = -1; sign <= 1; sign += 2) { |
||
644 |
// chop q along the line xy[dir] = sign*h[dir] |
||
645 |
FCL_REAL* pq = q; |
||
646 |
FCL_REAL* pr = r; |
||
647 |
nr = 0; |
||
648 |
for (unsigned int i = nq; i > 0; --i) { |
||
649 |
// go through all points in q and all lines between adjacent points |
||
650 |
if (sign * pq[dir] < h[dir]) { |
||
651 |
// this point is inside the chopping line |
||
652 |
pr[0] = pq[0]; |
||
653 |
pr[1] = pq[1]; |
||
654 |
pr += 2; |
||
655 |
nr++; |
||
656 |
if (nr & 8) { |
||
657 |
q = r; |
||
658 |
goto done; |
||
659 |
} |
||
660 |
} |
||
661 |
FCL_REAL* nextq = (i > 1) ? pq + 2 : q; |
||
662 |
if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir])) { |
||
663 |
// this line crosses the chopping line |
||
664 |
pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) / |
||
665 |
(nextq[dir] - pq[dir]) * |
||
666 |
(sign * h[dir] - pq[dir]); |
||
667 |
pr[dir] = sign * h[dir]; |
||
668 |
pr += 2; |
||
669 |
nr++; |
||
670 |
if (nr & 8) { |
||
671 |
q = r; |
||
672 |
goto done; |
||
673 |
} |
||
674 |
} |
||
675 |
pq += 2; |
||
676 |
} |
||
677 |
q = r; |
||
678 |
r = (q == ret) ? buffer : ret; |
||
679 |
nq = nr; |
||
680 |
} |
||
681 |
} |
||
682 |
|||
683 |
done: |
||
684 |
if (q != ret) memcpy(ret, q, nr * 2 * sizeof(FCL_REAL)); |
||
685 |
return nr; |
||
686 |
} |
||
687 |
|||
688 |
// given n points in the plane (array p, of size 2*n), generate m points that |
||
689 |
// best represent the whole set. the definition of 'best' here is not |
||
690 |
// predetermined - the idea is to select points that give good box-box |
||
691 |
// collision detection behavior. the chosen point indexes are returned in the |
||
692 |
// array iret (of size m). 'i0' is always the first entry in the array. |
||
693 |
// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be |
||
694 |
// in the range [0..n-1]. |
||
695 |
static inline void cullPoints2(unsigned int n, FCL_REAL p[], unsigned int m, |
||
696 |
unsigned int i0, unsigned int iret[]) { |
||
697 |
// compute the centroid of the polygon in cx,cy |
||
698 |
FCL_REAL a, cx, cy, q; |
||
699 |
switch (n) { |
||
700 |
case 1: |
||
701 |
cx = p[0]; |
||
702 |
cy = p[1]; |
||
703 |
break; |
||
704 |
case 2: |
||
705 |
cx = 0.5 * (p[0] + p[2]); |
||
706 |
cy = 0.5 * (p[1] + p[3]); |
||
707 |
break; |
||
708 |
default: |
||
709 |
a = 0; |
||
710 |
cx = 0; |
||
711 |
cy = 0; |
||
712 |
assert(n > 0 && "n should be positive"); |
||
713 |
for (int i = 0; i < (int)n - 1; ++i) { |
||
714 |
q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1]; |
||
715 |
a += q; |
||
716 |
cx += q * (p[i * 2] + p[i * 2 + 2]); |
||
717 |
cy += q * (p[i * 2 + 1] + p[i * 2 + 3]); |
||
718 |
} |
||
719 |
q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1]; |
||
720 |
if (std::abs(a + q) > std::numeric_limits<FCL_REAL>::epsilon()) |
||
721 |
a = 1 / (3 * (a + q)); |
||
722 |
else |
||
723 |
a = 1e18f; |
||
724 |
|||
725 |
cx = a * (cx + q * (p[n * 2 - 2] + p[0])); |
||
726 |
cy = a * (cy + q * (p[n * 2 - 1] + p[1])); |
||
727 |
} |
||
728 |
|||
729 |
// compute the angle of each point w.r.t. the centroid |
||
730 |
FCL_REAL A[8]; |
||
731 |
for (unsigned int i = 0; i < n; ++i) |
||
732 |
A[i] = atan2(p[i * 2 + 1] - cy, p[i * 2] - cx); |
||
733 |
|||
734 |
// search for points that have angles closest to A[i0] + i*(2*pi/m). |
||
735 |
int avail[] = {1, 1, 1, 1, 1, 1, 1, 1}; |
||
736 |
avail[i0] = 0; |
||
737 |
iret[0] = i0; |
||
738 |
iret++; |
||
739 |
const double pi = boost::math::constants::pi<FCL_REAL>(); |
||
740 |
for (unsigned int j = 1; j < m; ++j) { |
||
741 |
a = j * (2 * pi / m) + A[i0]; |
||
742 |
if (a > pi) a -= 2 * pi; |
||
743 |
FCL_REAL maxdiff = 1e9, diff; |
||
744 |
|||
745 |
*iret = i0; // iret is not allowed to keep this value, but it sometimes |
||
746 |
// does, when diff=#QNAN0 |
||
747 |
for (unsigned int i = 0; i < n; ++i) { |
||
748 |
if (avail[i]) { |
||
749 |
diff = std::abs(A[i] - a); |
||
750 |
if (diff > pi) diff = 2 * pi - diff; |
||
751 |
if (diff < maxdiff) { |
||
752 |
maxdiff = diff; |
||
753 |
*iret = i; |
||
754 |
} |
||
755 |
} |
||
756 |
} |
||
757 |
avail[*iret] = 0; |
||
758 |
iret++; |
||
759 |
} |
||
760 |
} |
||
761 |
|||
762 |
inline unsigned int boxBox2(const Vec3f& halfSide1, const Matrix3f& R1, |
||
763 |
const Vec3f& T1, const Vec3f& halfSide2, |
||
764 |
const Matrix3f& R2, const Vec3f& T2, Vec3f& normal, |
||
765 |
FCL_REAL* depth, int* return_code, |
||
766 |
unsigned int maxc, |
||
767 |
std::vector<ContactPoint>& contacts) { |
||
768 |
const FCL_REAL fudge_factor = FCL_REAL(1.05); |
||
769 |
Vec3f normalC; |
||
770 |
FCL_REAL s, s2, l; |
||
771 |
int invert_normal, code; |
||
772 |
|||
773 |
Vec3f p(T2 - |
||
774 |
T1); // get vector from centers of box 1 to box 2, relative to box 1 |
||
775 |
Vec3f pp(R1.transpose() * p); // get pp = p relative to body 1 |
||
776 |
|||
777 |
// get side lengths / 2 |
||
778 |
const Vec3f& A(halfSide1); |
||
779 |
const Vec3f& B(halfSide2); |
||
780 |
|||
781 |
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2 |
||
782 |
Matrix3f R(R1.transpose() * R2); |
||
783 |
Matrix3f Q(R.cwiseAbs()); |
||
784 |
|||
785 |
// for all 15 possible separating axes: |
||
786 |
// * see if the axis separates the boxes. if so, return 0. |
||
787 |
// * find the depth of the penetration along the separating axis (s2) |
||
788 |
// * if this is the largest depth so far, record it. |
||
789 |
// the normal vector will be set to the separating axis with the smallest |
||
790 |
// depth. note: normalR is set to point to a column of R1 or R2 if that is |
||
791 |
// the smallest depth normal so far. otherwise normalR is 0 and normalC is |
||
792 |
// set to a vector relative to body 1. invert_normal is 1 if the sign of |
||
793 |
// the normal should be flipped. |
||
794 |
|||
795 |
int best_col_id = -1; |
||
796 |
const Matrix3f* normalR = 0; |
||
797 |
FCL_REAL tmp = 0; |
||
798 |
|||
799 |
s = -(std::numeric_limits<FCL_REAL>::max)(); |
||
800 |
invert_normal = 0; |
||
801 |
code = 0; |
||
802 |
|||
803 |
// separating axis = u1, u2, u3 |
||
804 |
tmp = pp[0]; |
||
805 |
s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]); |
||
806 |
if (s2 > 0) { |
||
807 |
*return_code = 0; |
||
808 |
return 0; |
||
809 |
} |
||
810 |
if (s2 > s) { |
||
811 |
s = s2; |
||
812 |
best_col_id = 0; |
||
813 |
normalR = &R1; |
||
814 |
invert_normal = (tmp < 0); |
||
815 |
code = 1; |
||
816 |
} |
||
817 |
|||
818 |
tmp = pp[1]; |
||
819 |
s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]); |
||
820 |
if (s2 > 0) { |
||
821 |
*return_code = 0; |
||
822 |
return 0; |
||
823 |
} |
||
824 |
if (s2 > s) { |
||
825 |
s = s2; |
||
826 |
best_col_id = 1; |
||
827 |
normalR = &R1; |
||
828 |
invert_normal = (tmp < 0); |
||
829 |
code = 2; |
||
830 |
} |
||
831 |
|||
832 |
tmp = pp[2]; |
||
833 |
s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]); |
||
834 |
if (s2 > 0) { |
||
835 |
*return_code = 0; |
||
836 |
return 0; |
||
837 |
} |
||
838 |
if (s2 > s) { |
||
839 |
s = s2; |
||
840 |
best_col_id = 2; |
||
841 |
normalR = &R1; |
||
842 |
invert_normal = (tmp < 0); |
||
843 |
code = 3; |
||
844 |
} |
||
845 |
|||
846 |
// separating axis = v1, v2, v3 |
||
847 |
tmp = R2.col(0).dot(p); |
||
848 |
s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]); |
||
849 |
if (s2 > 0) { |
||
850 |
*return_code = 0; |
||
851 |
return 0; |
||
852 |
} |
||
853 |
if (s2 > s) { |
||
854 |
s = s2; |
||
855 |
best_col_id = 0; |
||
856 |
normalR = &R2; |
||
857 |
invert_normal = (tmp < 0); |
||
858 |
code = 4; |
||
859 |
} |
||
860 |
|||
861 |
tmp = R2.col(1).dot(p); |
||
862 |
s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]); |
||
863 |
if (s2 > 0) { |
||
864 |
*return_code = 0; |
||
865 |
return 0; |
||
866 |
} |
||
867 |
if (s2 > s) { |
||
868 |
s = s2; |
||
869 |
best_col_id = 1; |
||
870 |
normalR = &R2; |
||
871 |
invert_normal = (tmp < 0); |
||
872 |
code = 5; |
||
873 |
} |
||
874 |
|||
875 |
tmp = R2.col(2).dot(p); |
||
876 |
s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]); |
||
877 |
if (s2 > 0) { |
||
878 |
*return_code = 0; |
||
879 |
return 0; |
||
880 |
} |
||
881 |
if (s2 > s) { |
||
882 |
s = s2; |
||
883 |
best_col_id = 2; |
||
884 |
normalR = &R2; |
||
885 |
invert_normal = (tmp < 0); |
||
886 |
code = 6; |
||
887 |
} |
||
888 |
|||
889 |
FCL_REAL fudge2(1.0e-6); |
||
890 |
Q.array() += fudge2; |
||
891 |
|||
892 |
Vec3f n; |
||
893 |
static const FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon(); |
||
894 |
|||
895 |
// separating axis = u1 x (v1,v2,v3) |
||
896 |
tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); |
||
897 |
s2 = std::abs(tmp) - |
||
898 |
(A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1)); |
||
899 |
if (s2 > 0) { |
||
900 |
*return_code = 0; |
||
901 |
return 0; |
||
902 |
} |
||
903 |
n = Vec3f(0, -R(2, 0), R(1, 0)); |
||
904 |
l = n.norm(); |
||
905 |
if (l > eps) { |
||
906 |
s2 /= l; |
||
907 |
if (s2 * fudge_factor > s) { |
||
908 |
s = s2; |
||
909 |
best_col_id = -1; |
||
910 |
normalC = n / l; |
||
911 |
invert_normal = (tmp < 0); |
||
912 |
code = 7; |
||
913 |
} |
||
914 |
} |
||
915 |
|||
916 |
tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1); |
||
917 |
s2 = std::abs(tmp) - |
||
918 |
(A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0)); |
||
919 |
if (s2 > 0) { |
||
920 |
*return_code = 0; |
||
921 |
return 0; |
||
922 |
} |
||
923 |
n = Vec3f(0, -R(2, 1), R(1, 1)); |
||
924 |
l = n.norm(); |
||
925 |
if (l > eps) { |
||
926 |
s2 /= l; |
||
927 |
if (s2 * fudge_factor > s) { |
||
928 |
s = s2; |
||
929 |
best_col_id = -1; |
||
930 |
normalC = n / l; |
||
931 |
invert_normal = (tmp < 0); |
||
932 |
code = 8; |
||
933 |
} |
||
934 |
} |
||
935 |
|||
936 |
tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); |
||
937 |
s2 = std::abs(tmp) - |
||
938 |
(A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); |
||
939 |
if (s2 > 0) { |
||
940 |
*return_code = 0; |
||
941 |
return 0; |
||
942 |
} |
||
943 |
n = Vec3f(0, -R(2, 2), R(1, 2)); |
||
944 |
l = n.norm(); |
||
945 |
if (l > eps) { |
||
946 |
s2 /= l; |
||
947 |
if (s2 * fudge_factor > s) { |
||
948 |
s = s2; |
||
949 |
best_col_id = -1; |
||
950 |
normalC = n / l; |
||
951 |
invert_normal = (tmp < 0); |
||
952 |
code = 9; |
||
953 |
} |
||
954 |
} |
||
955 |
|||
956 |
// separating axis = u2 x (v1,v2,v3) |
||
957 |
tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0); |
||
958 |
s2 = std::abs(tmp) - |
||
959 |
(A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1)); |
||
960 |
if (s2 > 0) { |
||
961 |
*return_code = 0; |
||
962 |
return 0; |
||
963 |
} |
||
964 |
n = Vec3f(R(2, 0), 0, -R(0, 0)); |
||
965 |
l = n.norm(); |
||
966 |
if (l > eps) { |
||
967 |
s2 /= l; |
||
968 |
if (s2 * fudge_factor > s) { |
||
969 |
s = s2; |
||
970 |
best_col_id = -1; |
||
971 |
normalC = n / l; |
||
972 |
invert_normal = (tmp < 0); |
||
973 |
code = 10; |
||
974 |
} |
||
975 |
} |
||
976 |
|||
977 |
tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1); |
||
978 |
s2 = std::abs(tmp) - |
||
979 |
(A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0)); |
||
980 |
if (s2 > 0) { |
||
981 |
*return_code = 0; |
||
982 |
return 0; |
||
983 |
} |
||
984 |
n = Vec3f(R(2, 1), 0, -R(0, 1)); |
||
985 |
l = n.norm(); |
||
986 |
if (l > eps) { |
||
987 |
s2 /= l; |
||
988 |
if (s2 * fudge_factor > s) { |
||
989 |
s = s2; |
||
990 |
best_col_id = -1; |
||
991 |
normalC = n / l; |
||
992 |
invert_normal = (tmp < 0); |
||
993 |
code = 11; |
||
994 |
} |
||
995 |
} |
||
996 |
|||
997 |
tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); |
||
998 |
s2 = std::abs(tmp) - |
||
999 |
(A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); |
||
1000 |
if (s2 > 0) { |
||
1001 |
*return_code = 0; |
||
1002 |
return 0; |
||
1003 |
} |
||
1004 |
n = Vec3f(R(2, 2), 0, -R(0, 2)); |
||
1005 |
l = n.norm(); |
||
1006 |
if (l > eps) { |
||
1007 |
s2 /= l; |
||
1008 |
if (s2 * fudge_factor > s) { |
||
1009 |
s = s2; |
||
1010 |
best_col_id = -1; |
||
1011 |
normalC = n / l; |
||
1012 |
invert_normal = (tmp < 0); |
||
1013 |
code = 12; |
||
1014 |
} |
||
1015 |
} |
||
1016 |
|||
1017 |
// separating axis = u3 x (v1,v2,v3) |
||
1018 |
tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0); |
||
1019 |
s2 = std::abs(tmp) - |
||
1020 |
(A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1)); |
||
1021 |
if (s2 > 0) { |
||
1022 |
*return_code = 0; |
||
1023 |
return 0; |
||
1024 |
} |
||
1025 |
n = Vec3f(-R(1, 0), R(0, 0), 0); |
||
1026 |
l = n.norm(); |
||
1027 |
if (l > eps) { |
||
1028 |
s2 /= l; |
||
1029 |
if (s2 * fudge_factor > s) { |
||
1030 |
s = s2; |
||
1031 |
best_col_id = -1; |
||
1032 |
normalC = n / l; |
||
1033 |
invert_normal = (tmp < 0); |
||
1034 |
code = 13; |
||
1035 |
} |
||
1036 |
} |
||
1037 |
|||
1038 |
tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1); |
||
1039 |
s2 = std::abs(tmp) - |
||
1040 |
(A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0)); |
||
1041 |
if (s2 > 0) { |
||
1042 |
*return_code = 0; |
||
1043 |
return 0; |
||
1044 |
} |
||
1045 |
n = Vec3f(-R(1, 1), R(0, 1), 0); |
||
1046 |
l = n.norm(); |
||
1047 |
if (l > eps) { |
||
1048 |
s2 /= l; |
||
1049 |
if (s2 * fudge_factor > s) { |
||
1050 |
s = s2; |
||
1051 |
best_col_id = -1; |
||
1052 |
normalC = n / l; |
||
1053 |
invert_normal = (tmp < 0); |
||
1054 |
code = 14; |
||
1055 |
} |
||
1056 |
} |
||
1057 |
|||
1058 |
tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); |
||
1059 |
s2 = std::abs(tmp) - |
||
1060 |
(A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); |
||
1061 |
if (s2 > 0) { |
||
1062 |
*return_code = 0; |
||
1063 |
return 0; |
||
1064 |
} |
||
1065 |
n = Vec3f(-R(1, 2), R(0, 2), 0); |
||
1066 |
l = n.norm(); |
||
1067 |
if (l > eps) { |
||
1068 |
s2 /= l; |
||
1069 |
if (s2 * fudge_factor > s) { |
||
1070 |
s = s2; |
||
1071 |
best_col_id = -1; |
||
1072 |
normalC = n / l; |
||
1073 |
invert_normal = (tmp < 0); |
||
1074 |
code = 15; |
||
1075 |
} |
||
1076 |
} |
||
1077 |
|||
1078 |
if (!code) { |
||
1079 |
*return_code = code; |
||
1080 |
return 0; |
||
1081 |
} |
||
1082 |
|||
1083 |
// if we get to this point, the boxes interpenetrate. compute the normal |
||
1084 |
// in global coordinates. |
||
1085 |
if (best_col_id != -1) |
||
1086 |
normal = normalR->col(best_col_id); |
||
1087 |
else |
||
1088 |
normal.noalias() = R1 * normalC; |
||
1089 |
|||
1090 |
if (invert_normal) normal = -normal; |
||
1091 |
|||
1092 |
*depth = -s; // s is negative when the boxes are in collision |
||
1093 |
|||
1094 |
// compute contact point(s) |
||
1095 |
|||
1096 |
if (code > 6) { |
||
1097 |
// an edge from box 1 touches an edge from box 2. |
||
1098 |
// find a point pa on the intersecting edge of box 1 |
||
1099 |
Vec3f pa(T1); |
||
1100 |
FCL_REAL sign; |
||
1101 |
|||
1102 |
for (int j = 0; j < 3; ++j) { |
||
1103 |
sign = (R1.col(j).dot(normal) > 0) ? 1 : -1; |
||
1104 |
pa += R1.col(j) * (A[j] * sign); |
||
1105 |
} |
||
1106 |
|||
1107 |
// find a point pb on the intersecting edge of box 2 |
||
1108 |
Vec3f pb(T2); |
||
1109 |
|||
1110 |
for (int j = 0; j < 3; ++j) { |
||
1111 |
sign = (R2.col(j).dot(normal) > 0) ? -1 : 1; |
||
1112 |
pb += R2.col(j) * (B[j] * sign); |
||
1113 |
} |
||
1114 |
|||
1115 |
FCL_REAL alpha, beta; |
||
1116 |
Vec3f ua(R1.col((code - 7) / 3)); |
||
1117 |
Vec3f ub(R2.col((code - 7) % 3)); |
||
1118 |
|||
1119 |
lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); |
||
1120 |
pa += ua * alpha; |
||
1121 |
pb += ub * beta; |
||
1122 |
|||
1123 |
// Vec3f pointInWorld((pa + pb) * 0.5); |
||
1124 |
// contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); |
||
1125 |
contacts.push_back(ContactPoint(-normal, pb, -*depth)); |
||
1126 |
*return_code = code; |
||
1127 |
|||
1128 |
return 1; |
||
1129 |
} |
||
1130 |
|||
1131 |
// okay, we have a face-something intersection (because the separating |
||
1132 |
// axis is perpendicular to a face). define face 'a' to be the reference |
||
1133 |
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be |
||
1134 |
// the incident face (the closest face of the other box). |
||
1135 |
|||
1136 |
const Matrix3f *Ra, *Rb; |
||
1137 |
const Vec3f *pa, *pb, *Sa, *Sb; |
||
1138 |
|||
1139 |
if (code <= 3) { |
||
1140 |
Ra = &R1; |
||
1141 |
Rb = &R2; |
||
1142 |
pa = &T1; |
||
1143 |
pb = &T2; |
||
1144 |
Sa = &A; |
||
1145 |
Sb = &B; |
||
1146 |
} else { |
||
1147 |
Ra = &R2; |
||
1148 |
Rb = &R1; |
||
1149 |
pa = &T2; |
||
1150 |
pb = &T1; |
||
1151 |
Sa = &B; |
||
1152 |
Sb = &A; |
||
1153 |
} |
||
1154 |
|||
1155 |
// nr = normal vector of reference face dotted with axes of incident box. |
||
1156 |
// anr = absolute values of nr. |
||
1157 |
Vec3f normal2, nr, anr; |
||
1158 |
if (code <= 3) |
||
1159 |
normal2 = normal; |
||
1160 |
else |
||
1161 |
normal2 = -normal; |
||
1162 |
|||
1163 |
nr.noalias() = Rb->transpose() * normal2; |
||
1164 |
anr = nr.cwiseAbs(); |
||
1165 |
|||
1166 |
// find the largest compontent of anr: this corresponds to the normal |
||
1167 |
// for the indident face. the other axis numbers of the indicent face |
||
1168 |
// are stored in a1,a2. |
||
1169 |
int lanr, a1, a2; |
||
1170 |
if (anr[1] > anr[0]) { |
||
1171 |
if (anr[1] > anr[2]) { |
||
1172 |
a1 = 0; |
||
1173 |
lanr = 1; |
||
1174 |
a2 = 2; |
||
1175 |
} else { |
||
1176 |
a1 = 0; |
||
1177 |
a2 = 1; |
||
1178 |
lanr = 2; |
||
1179 |
} |
||
1180 |
} else { |
||
1181 |
if (anr[0] > anr[2]) { |
||
1182 |
lanr = 0; |
||
1183 |
a1 = 1; |
||
1184 |
a2 = 2; |
||
1185 |
} else { |
||
1186 |
a1 = 0; |
||
1187 |
a2 = 1; |
||
1188 |
lanr = 2; |
||
1189 |
} |
||
1190 |
} |
||
1191 |
|||
1192 |
// compute center point of incident face, in reference-face coordinates |
||
1193 |
Vec3f center; |
||
1194 |
if (nr[lanr] < 0) |
||
1195 |
center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]); |
||
1196 |
else |
||
1197 |
center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]); |
||
1198 |
|||
1199 |
// find the normal and non-normal axis numbers of the reference box |
||
1200 |
int codeN, code1, code2; |
||
1201 |
if (code <= 3) |
||
1202 |
codeN = code - 1; |
||
1203 |
else |
||
1204 |
codeN = code - 4; |
||
1205 |
|||
1206 |
if (codeN == 0) { |
||
1207 |
code1 = 1; |
||
1208 |
code2 = 2; |
||
1209 |
} else if (codeN == 1) { |
||
1210 |
code1 = 0; |
||
1211 |
code2 = 2; |
||
1212 |
} else { |
||
1213 |
code1 = 0; |
||
1214 |
code2 = 1; |
||
1215 |
} |
||
1216 |
|||
1217 |
// find the four corners of the incident face, in reference-face coordinates |
||
1218 |
FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs) |
||
1219 |
FCL_REAL c1, c2, m11, m12, m21, m22; |
||
1220 |
c1 = Ra->col(code1).dot(center); |
||
1221 |
c2 = Ra->col(code2).dot(center); |
||
1222 |
// optimize this? - we have already computed this data above, but it is not |
||
1223 |
// stored in an easy-to-index format. for now it's quicker just to recompute |
||
1224 |
// the four dot products. |
||
1225 |
Vec3f tempRac = Ra->col(code1); |
||
1226 |
m11 = Rb->col(a1).dot(tempRac); |
||
1227 |
m12 = Rb->col(a2).dot(tempRac); |
||
1228 |
tempRac = Ra->col(code2); |
||
1229 |
m21 = Rb->col(a1).dot(tempRac); |
||
1230 |
m22 = Rb->col(a2).dot(tempRac); |
||
1231 |
|||
1232 |
FCL_REAL k1 = m11 * (*Sb)[a1]; |
||
1233 |
FCL_REAL k2 = m21 * (*Sb)[a1]; |
||
1234 |
FCL_REAL k3 = m12 * (*Sb)[a2]; |
||
1235 |
FCL_REAL k4 = m22 * (*Sb)[a2]; |
||
1236 |
quad[0] = c1 - k1 - k3; |
||
1237 |
quad[1] = c2 - k2 - k4; |
||
1238 |
quad[2] = c1 - k1 + k3; |
||
1239 |
quad[3] = c2 - k2 + k4; |
||
1240 |
quad[4] = c1 + k1 + k3; |
||
1241 |
quad[5] = c2 + k2 + k4; |
||
1242 |
quad[6] = c1 + k1 - k3; |
||
1243 |
quad[7] = c2 + k2 - k4; |
||
1244 |
|||
1245 |
// find the size of the reference face |
||
1246 |
FCL_REAL rect[2]; |
||
1247 |
rect[0] = (*Sa)[code1]; |
||
1248 |
rect[1] = (*Sa)[code2]; |
||
1249 |
|||
1250 |
// intersect the incident and reference faces |
||
1251 |
FCL_REAL ret[16]; |
||
1252 |
const unsigned int n_intersect = intersectRectQuad2(rect, quad, ret); |
||
1253 |
if (n_intersect < 1) { |
||
1254 |
*return_code = code; |
||
1255 |
return 0; |
||
1256 |
} // this should never happen |
||
1257 |
|||
1258 |
// convert the intersection points into reference-face coordinates, |
||
1259 |
// and compute the contact position and depth for each point. only keep |
||
1260 |
// those points that have a positive (penetrating) depth. delete points in |
||
1261 |
// the 'ret' array as necessary so that 'point' and 'ret' correspond. |
||
1262 |
Vec3f points[8]; // penetrating contact points |
||
1263 |
FCL_REAL dep[8]; // depths for those points |
||
1264 |
FCL_REAL det1 = 1.f / (m11 * m22 - m12 * m21); |
||
1265 |
m11 *= det1; |
||
1266 |
m12 *= det1; |
||
1267 |
m21 *= det1; |
||
1268 |
m22 *= det1; |
||
1269 |
unsigned int cnum = 0; // number of penetrating contact points found |
||
1270 |
for (unsigned int j = 0; j < n_intersect; ++j) { |
||
1271 |
FCL_REAL k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2); |
||
1272 |
FCL_REAL k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2); |
||
1273 |
points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2; |
||
1274 |
dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); |
||
1275 |
if (dep[cnum] >= 0) { |
||
1276 |
ret[cnum * 2] = ret[j * 2]; |
||
1277 |
ret[cnum * 2 + 1] = ret[j * 2 + 1]; |
||
1278 |
cnum++; |
||
1279 |
} |
||
1280 |
} |
||
1281 |
if (cnum < 1) { |
||
1282 |
*return_code = code; |
||
1283 |
return 0; |
||
1284 |
} // this should never happen |
||
1285 |
|||
1286 |
// we can't generate more contacts than we actually have |
||
1287 |
if (maxc > cnum) maxc = cnum; |
||
1288 |
if (maxc < 1) maxc = 1; |
||
1289 |
|||
1290 |
if (cnum <= maxc) { |
||
1291 |
if (code < 4) { |
||
1292 |
// we have less contacts than we need, so we use them all |
||
1293 |
for (unsigned int j = 0; j < cnum; ++j) { |
||
1294 |
Vec3f pointInWorld = points[j] + (*pa); |
||
1295 |
contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); |
||
1296 |
} |
||
1297 |
} else { |
||
1298 |
// we have less contacts than we need, so we use them all |
||
1299 |
for (unsigned int j = 0; j < cnum; ++j) { |
||
1300 |
Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j]; |
||
1301 |
contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j])); |
||
1302 |
} |
||
1303 |
} |
||
1304 |
} else { |
||
1305 |
// we have more contacts than are wanted, some of them must be culled. |
||
1306 |
// find the deepest point, it is always the first contact. |
||
1307 |
unsigned int i1 = 0; |
||
1308 |
FCL_REAL maxdepth = dep[0]; |
||
1309 |
for (unsigned int i = 1; i < cnum; ++i) { |
||
1310 |
if (dep[i] > maxdepth) { |
||
1311 |
maxdepth = dep[i]; |
||
1312 |
i1 = i; |
||
1313 |
} |
||
1314 |
} |
||
1315 |
|||
1316 |
unsigned int iret[8]; |
||
1317 |
cullPoints2(cnum, ret, maxc, i1, iret); |
||
1318 |
|||
1319 |
for (unsigned int j = 0; j < maxc; ++j) { |
||
1320 |
Vec3f posInWorld = points[iret[j]] + (*pa); |
||
1321 |
if (code < 4) |
||
1322 |
contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]])); |
||
1323 |
else |
||
1324 |
contacts.push_back(ContactPoint( |
||
1325 |
-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]])); |
||
1326 |
} |
||
1327 |
cnum = maxc; |
||
1328 |
} |
||
1329 |
|||
1330 |
*return_code = code; |
||
1331 |
return cnum; |
||
1332 |
} |
||
1333 |
|||
1334 |
inline bool compareContactPoints(const ContactPoint& c1, |
||
1335 |
const ContactPoint& c2) { |
||
1336 |
return c1.depth < c2.depth; |
||
1337 |
} // Accending order, assuming penetration depth is a negative number! |
||
1338 |
|||
1339 |
inline bool boxBoxIntersect(const Box& s1, const Transform3f& tf1, |
||
1340 |
const Box& s2, const Transform3f& tf2, |
||
1341 |
Vec3f* contact_points, FCL_REAL* penetration_depth_, |
||
1342 |
Vec3f* normal_) { |
||
1343 |
std::vector<ContactPoint> contacts; |
||
1344 |
int return_code; |
||
1345 |
Vec3f normal; |
||
1346 |
FCL_REAL depth; |
||
1347 |
/* int cnum = */ boxBox2(s1.halfSide, tf1.getRotation(), tf1.getTranslation(), |
||
1348 |
s2.halfSide, tf2.getRotation(), tf2.getTranslation(), |
||
1349 |
normal, &depth, &return_code, 4, contacts); |
||
1350 |
|||
1351 |
if (normal_) *normal_ = normal; |
||
1352 |
if (penetration_depth_) *penetration_depth_ = depth; |
||
1353 |
|||
1354 |
if (contact_points) { |
||
1355 |
if (contacts.size() > 0) { |
||
1356 |
std::sort(contacts.begin(), contacts.end(), compareContactPoints); |
||
1357 |
*contact_points = contacts[0].point; |
||
1358 |
if (penetration_depth_) *penetration_depth_ = -contacts[0].depth; |
||
1359 |
} |
||
1360 |
} |
||
1361 |
|||
1362 |
return return_code != 0; |
||
1363 |
} |
||
1364 |
|||
1365 |
template <typename T> |
||
1366 |
inline T halfspaceIntersectTolerance() { |
||
1367 |
return 0; |
||
1368 |
} |
||
1369 |
|||
1370 |
template <> |
||
1371 |
inline float halfspaceIntersectTolerance() { |
||
1372 |
return 0.0001f; |
||
1373 |
} |
||
1374 |
|||
1375 |
template <> |
||
1376 |
288 |
inline double halfspaceIntersectTolerance() { |
|
1377 |
288 |
return 0.0000001; |
|
1378 |
} |
||
1379 |
|||
1380 |
20 |
inline bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1, |
|
1381 |
const Halfspace& s2, |
||
1382 |
const Transform3f& tf2, FCL_REAL& distance, |
||
1383 |
Vec3f& p1, Vec3f& p2, Vec3f& normal) { |
||
1384 |
✓✗ | 40 |
Halfspace new_s2 = transform(s2, tf2); |
1385 |
20 |
const Vec3f& center = tf1.getTranslation(); |
|
1386 |
✓✗ | 20 |
distance = new_s2.signedDistance(center) - s1.radius; |
1387 |
✓✓ | 20 |
if (distance <= 0) { |
1388 |
✓✗✓✗ |
16 |
normal = -new_s2.n; // pointing from s1 to s2 |
1389 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
16 |
p1 = p2 = center - new_s2.n * s1.radius - (distance * 0.5) * new_s2.n; |
1390 |
16 |
return true; |
|
1391 |
} else { |
||
1392 |
✓✗✓✗ ✓✗ |
4 |
p1 = center - s1.radius * new_s2.n; |
1393 |
✓✗✓✗ ✓✗ |
4 |
p2 = p1 - distance * new_s2.n; |
1394 |
4 |
return false; |
|
1395 |
} |
||
1396 |
} |
||
1397 |
|||
1398 |
/// @brief box half space, a, b, c = +/- edge size |
||
1399 |
/// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d |
||
1400 |
/// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d |
||
1401 |
/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c |
||
1402 |
/// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, |
||
1403 |
/// check that is enough |
||
1404 |
inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, |
||
1405 |
const Halfspace& s2, const Transform3f& tf2) { |
||
1406 |
Halfspace new_s2 = transform(s2, tf2); |
||
1407 |
|||
1408 |
const Matrix3f& R = tf1.getRotation(); |
||
1409 |
const Vec3f& T = tf1.getTranslation(); |
||
1410 |
|||
1411 |
Vec3f Q(R.transpose() * new_s2.n); |
||
1412 |
|||
1413 |
FCL_REAL depth = |
||
1414 |
Q.cwiseProduct(s1.halfSide).lpNorm<1>() - new_s2.signedDistance(T); |
||
1415 |
return (depth >= 0); |
||
1416 |
} |
||
1417 |
|||
1418 |
22 |
inline bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, |
|
1419 |
const Halfspace& s2, const Transform3f& tf2, |
||
1420 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
1421 |
Vec3f& normal) { |
||
1422 |
✓✗ | 44 |
Halfspace new_s2 = transform(s2, tf2); |
1423 |
|||
1424 |
22 |
const Matrix3f& R = tf1.getRotation(); |
|
1425 |
22 |
const Vec3f& T = tf1.getTranslation(); |
|
1426 |
|||
1427 |
// Q: plane normal expressed in box frame |
||
1428 |
✓✗✓✗ ✓✗ |
22 |
const Vec3f Q(R.transpose() * new_s2.n); |
1429 |
// A: scalar products of each side with normal |
||
1430 |
✓✗✓✗ |
22 |
const Vec3f A(Q.cwiseProduct(s1.halfSide)); |
1431 |
|||
1432 |
✓✗✓✗ |
22 |
distance = new_s2.signedDistance(T) - A.lpNorm<1>(); |
1433 |
✓✓ | 22 |
if (distance > 0) { |
1434 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
p1.noalias() = T + R * (A.array() > 0).select(s1.halfSide, -s1.halfSide); |
1435 |
✓✗✓✗ ✓✗✓✗ |
4 |
p2.noalias() = p1 - distance * new_s2.n; |
1436 |
4 |
return false; |
|
1437 |
} |
||
1438 |
|||
1439 |
/// find deepest point |
||
1440 |
✓✗ | 18 |
Vec3f p(T); |
1441 |
18 |
int sign = 0; |
|
1442 |
|||
1443 |
✓✗✓✓ ✗✓✓✓ |
20 |
if (std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || |
1444 |
✓✗ | 2 |
std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) { |
1445 |
✓✗✓✗ |
16 |
sign = (A[0] > 0) ? -1 : 1; |
1446 |
✓✗✓✗ ✓✗✓✗ |
16 |
p += R.col(0) * (s1.halfSide[0] * sign); |
1447 |
✓✗✓✗ ✗✓✗✓ |
4 |
} else if (std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || |
1448 |
✓✗ | 2 |
std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) { |
1449 |
sign = (A[1] > 0) ? -1 : 1; |
||
1450 |
p += R.col(1) * (s1.halfSide[1] * sign); |
||
1451 |
✓✗✓✗ ✗✓✗✓ |
4 |
} else if (std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || |
1452 |
✓✗ | 2 |
std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>()) { |
1453 |
sign = (A[2] > 0) ? -1 : 1; |
||
1454 |
p += R.col(2) * (s1.halfSide[2] * sign); |
||
1455 |
} else { |
||
1456 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
p.noalias() += R * (A.array() > 0).select(-s1.halfSide, s1.halfSide); |
1457 |
} |
||
1458 |
|||
1459 |
/// compute the contact point from the deepest point |
||
1460 |
✓✗✓✗ |
18 |
normal = -new_s2.n; |
1461 |
✓✗✓✗ ✓✗✓✗ |
18 |
p1 = p2 = p - new_s2.n * (distance * 0.5); |
1462 |
|||
1463 |
18 |
return true; |
|
1464 |
} |
||
1465 |
|||
1466 |
60 |
inline bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1, |
|
1467 |
const Halfspace& s2, |
||
1468 |
const Transform3f& tf2, |
||
1469 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
1470 |
Vec3f& normal) { |
||
1471 |
✓✗ | 120 |
Halfspace new_s2 = transform(s2, tf2); |
1472 |
|||
1473 |
60 |
const Matrix3f& R = tf1.getRotation(); |
|
1474 |
60 |
const Vec3f& T = tf1.getTranslation(); |
|
1475 |
|||
1476 |
✓✗✓✗ |
60 |
Vec3f dir_z = R.col(2); |
1477 |
|||
1478 |
✓✗ | 60 |
FCL_REAL cosa = dir_z.dot(new_s2.n); |
1479 |
✓✓ | 60 |
if (std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>()) { |
1480 |
// Capsule parallel to plane |
||
1481 |
✓✗ | 40 |
FCL_REAL signed_dist = new_s2.signedDistance(T); |
1482 |
40 |
distance = signed_dist - s1.radius; |
|
1483 |
✓✓ | 40 |
if (distance > 0) { |
1484 |
✓✗✓✗ ✓✗ |
8 |
p1 = T - s1.radius * new_s2.n; |
1485 |
✓✗✓✗ ✓✗ |
8 |
p2 = p1 - distance * new_s2.n; |
1486 |
8 |
return false; |
|
1487 |
} |
||
1488 |
|||
1489 |
✓✗✓✗ |
32 |
normal = -new_s2.n; |
1490 |
✓✗✓✗ ✓✗✓✗ |
32 |
p1 = p2 = T + new_s2.n * (-0.5 * distance - s1.radius); |
1491 |
32 |
return true; |
|
1492 |
} else { |
||
1493 |
✓✗ | 20 |
int sign = (cosa > 0) ? -1 : 1; |
1494 |
// closest capsule vertex to halfspace if no collision, |
||
1495 |
// or deeper inside halspace if collision |
||
1496 |
✓✗✓✗ ✓✗ |
20 |
Vec3f p = T + dir_z * (s1.halfLength * sign); |
1497 |
|||
1498 |
✓✗ | 20 |
FCL_REAL signed_dist = new_s2.signedDistance(p); |
1499 |
20 |
distance = signed_dist - s1.radius; |
|
1500 |
✓✓ | 20 |
if (distance > 0) { |
1501 |
✓✗✓✗ ✓✗ |
4 |
p1 = T - s1.radius * new_s2.n; |
1502 |
✓✗✓✗ ✓✗ |
4 |
p2 = p1 - distance * new_s2.n; |
1503 |
4 |
return false; |
|
1504 |
} |
||
1505 |
✓✗✓✗ |
16 |
normal = -new_s2.n; |
1506 |
// deepest point |
||
1507 |
✓✗✓✗ ✓✗ |
16 |
Vec3f c = p - new_s2.n * s1.radius; |
1508 |
✓✗✓✗ ✓✗✓✗ |
16 |
p1 = p2 = c - (0.5 * distance) * new_s2.n; |
1509 |
16 |
return true; |
|
1510 |
} |
||
1511 |
} |
||
1512 |
|||
1513 |
60 |
inline bool cylinderHalfspaceIntersect(const Cylinder& s1, |
|
1514 |
const Transform3f& tf1, |
||
1515 |
const Halfspace& s2, |
||
1516 |
const Transform3f& tf2, |
||
1517 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
1518 |
Vec3f& normal) { |
||
1519 |
✓✗ | 120 |
Halfspace new_s2 = transform(s2, tf2); |
1520 |
|||
1521 |
60 |
const Matrix3f& R = tf1.getRotation(); |
|
1522 |
60 |
const Vec3f& T = tf1.getTranslation(); |
|
1523 |
|||
1524 |
✓✗✓✗ |
60 |
Vec3f dir_z = R.col(2); |
1525 |
✓✗ | 60 |
FCL_REAL cosa = dir_z.dot(new_s2.n); |
1526 |
|||
1527 |
✓✓ | 60 |
if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) { |
1528 |
✓✗ | 40 |
FCL_REAL signed_dist = new_s2.signedDistance(T); |
1529 |
40 |
distance = signed_dist - s1.radius; |
|
1530 |
✓✓ | 40 |
if (distance > 0) { |
1531 |
// TODO: compute closest points |
||
1532 |
✓✗✓✗ |
8 |
p1 = p2 = Vec3f(0, 0, 0); |
1533 |
8 |
return false; |
|
1534 |
} |
||
1535 |
|||
1536 |
✓✗✓✗ |
32 |
normal = -new_s2.n; |
1537 |
✓✗✓✗ ✓✗✓✗ |
32 |
p1 = p2 = T - (0.5 * distance + s1.radius) * new_s2.n; |
1538 |
32 |
return true; |
|
1539 |
} else { |
||
1540 |
✓✗✓✗ ✓✗ |
20 |
Vec3f C = dir_z * cosa - new_s2.n; |
1541 |
✓✗✓✗ ✓✗ |
40 |
if (std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() || |
1542 |
20 |
std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>()) |
|
1543 |
✓✗ | 20 |
C = Vec3f(0, 0, 0); |
1544 |
else { |
||
1545 |
FCL_REAL s = C.norm(); |
||
1546 |
s = s1.radius / s; |
||
1547 |
C *= s; |
||
1548 |
} |
||
1549 |
|||
1550 |
✓✗ | 20 |
int sign = (cosa > 0) ? -1 : 1; |
1551 |
// deepest point |
||
1552 |
✓✗✓✗ ✓✗✓✗ |
20 |
Vec3f p = T + dir_z * (s1.halfLength * sign) + C; |
1553 |
✓✗ | 20 |
distance = new_s2.signedDistance(p); |
1554 |
✓✓ | 20 |
if (distance > 0) { |
1555 |
// TODO: compute closest points |
||
1556 |
✓✗✓✗ |
4 |
p1 = p2 = Vec3f(0, 0, 0); |
1557 |
4 |
return false; |
|
1558 |
} else { |
||
1559 |
✓✗✓✗ |
16 |
normal = -new_s2.n; |
1560 |
✓✗✓✗ ✓✗✓✗ |
16 |
p1 = p2 = p - (0.5 * distance) * new_s2.n; |
1561 |
16 |
return true; |
|
1562 |
} |
||
1563 |
} |
||
1564 |
} |
||
1565 |
|||
1566 |
60 |
inline bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1, |
|
1567 |
const Halfspace& s2, const Transform3f& tf2, |
||
1568 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
1569 |
Vec3f& normal) { |
||
1570 |
✓✗ | 120 |
Halfspace new_s2 = transform(s2, tf2); |
1571 |
|||
1572 |
60 |
const Matrix3f& R = tf1.getRotation(); |
|
1573 |
60 |
const Vec3f& T = tf1.getTranslation(); |
|
1574 |
|||
1575 |
✓✗✓✗ |
60 |
Vec3f dir_z = R.col(2); |
1576 |
✓✗ | 60 |
FCL_REAL cosa = dir_z.dot(new_s2.n); |
1577 |
|||
1578 |
✓✓ | 60 |
if (cosa < halfspaceIntersectTolerance<FCL_REAL>()) { |
1579 |
✓✗ | 40 |
FCL_REAL signed_dist = new_s2.signedDistance(T); |
1580 |
40 |
distance = signed_dist - s1.radius; |
|
1581 |
✓✓ | 40 |
if (distance > 0) { |
1582 |
✓✗✓✗ |
8 |
p1 = p2 = Vec3f(0, 0, 0); |
1583 |
8 |
return false; |
|
1584 |
} else { |
||
1585 |
✓✗✓✗ |
32 |
normal = -new_s2.n; |
1586 |
p1 = p2 = |
||
1587 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
32 |
T - dir_z * (s1.halfLength) - new_s2.n * (0.5 * distance + s1.radius); |
1588 |
32 |
return true; |
|
1589 |
} |
||
1590 |
} else { |
||
1591 |
✓✗✓✗ ✓✗ |
20 |
Vec3f C = dir_z * cosa - new_s2.n; |
1592 |
✓✗✓✗ ✓✗ |
40 |
if (std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() || |
1593 |
20 |
std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>()) |
|
1594 |
✓✗ | 20 |
C = Vec3f(0, 0, 0); |
1595 |
else { |
||
1596 |
FCL_REAL s = C.norm(); |
||
1597 |
s = s1.radius / s; |
||
1598 |
C *= s; |
||
1599 |
} |
||
1600 |
|||
1601 |
✓✗✓✗ ✓✗ |
20 |
Vec3f a1 = T + dir_z * (s1.halfLength); |
1602 |
✓✗✓✗ ✓✗✓✗ |
20 |
Vec3f a2 = T - dir_z * (s1.halfLength) + C; |
1603 |
|||
1604 |
✓✗ | 20 |
FCL_REAL d1 = new_s2.signedDistance(a1); |
1605 |
✓✗ | 20 |
FCL_REAL d2 = new_s2.signedDistance(a2); |
1606 |
|||
1607 |
✓✓✓✓ |
20 |
if (d1 > 0 && d2 > 0) |
1608 |
4 |
return false; |
|
1609 |
else { |
||
1610 |
16 |
distance = std::min(d1, d2); |
|
1611 |
✓✗✓✗ |
16 |
normal = -new_s2.n; |
1612 |
✓✗✗✓ ✓✗✓✗ ✓✗ |
16 |
p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n; |
1613 |
16 |
return true; |
|
1614 |
} |
||
1615 |
} |
||
1616 |
} |
||
1617 |
|||
1618 |
inline bool convexHalfspaceIntersect( |
||
1619 |
const ConvexBase& s1, const Transform3f& tf1, const Halfspace& s2, |
||
1620 |
const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, |
||
1621 |
Vec3f* normal) { |
||
1622 |
Halfspace new_s2 = transform(s2, tf2); |
||
1623 |
|||
1624 |
Vec3f v; |
||
1625 |
FCL_REAL depth = (std::numeric_limits<FCL_REAL>::max)(); |
||
1626 |
|||
1627 |
for (unsigned int i = 0; i < s1.num_points; ++i) { |
||
1628 |
Vec3f p = tf1.transform(s1.points[i]); |
||
1629 |
|||
1630 |
FCL_REAL d = new_s2.signedDistance(p); |
||
1631 |
if (d < depth) { |
||
1632 |
depth = d; |
||
1633 |
v = p; |
||
1634 |
} |
||
1635 |
} |
||
1636 |
|||
1637 |
if (depth <= 0) { |
||
1638 |
if (contact_points) *contact_points = v - new_s2.n * (0.5 * depth); |
||
1639 |
if (penetration_depth) *penetration_depth = depth; |
||
1640 |
if (normal) *normal = -new_s2.n; |
||
1641 |
return true; |
||
1642 |
} else |
||
1643 |
return false; |
||
1644 |
} |
||
1645 |
|||
1646 |
// Intersection between half-space and triangle |
||
1647 |
// Half-space is in pose tf1, |
||
1648 |
// Triangle is in pose tf2 |
||
1649 |
// Computes distance and closest points (p1, p2) if no collision, |
||
1650 |
// contact point (p1 = p2), normal and penetration depth (-distance) |
||
1651 |
// if collision |
||
1652 |
12 |
inline bool halfspaceTriangleIntersect(const Halfspace& s1, |
|
1653 |
const Transform3f& tf1, const Vec3f& P1, |
||
1654 |
const Vec3f& P2, const Vec3f& P3, |
||
1655 |
const Transform3f& tf2, |
||
1656 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
1657 |
Vec3f& normal) { |
||
1658 |
✓✗ | 24 |
Halfspace new_s1 = transform(s1, tf1); |
1659 |
|||
1660 |
✓✗ | 12 |
Vec3f v = tf2.transform(P1); |
1661 |
✓✗ | 12 |
FCL_REAL depth = new_s1.signedDistance(v); |
1662 |
|||
1663 |
✓✗ | 12 |
Vec3f p = tf2.transform(P2); |
1664 |
✓✗ | 12 |
FCL_REAL d = new_s1.signedDistance(p); |
1665 |
✓✗ | 12 |
if (d < depth) { |
1666 |
12 |
depth = d; |
|
1667 |
✓✗ | 12 |
v = p; |
1668 |
} |
||
1669 |
|||
1670 |
✓✗ | 12 |
p = tf2.transform(P3); |
1671 |
✓✗ | 12 |
d = new_s1.signedDistance(p); |
1672 |
✗✓ | 12 |
if (d < depth) { |
1673 |
depth = d; |
||
1674 |
v = p; |
||
1675 |
} |
||
1676 |
// v is the vertex with minimal projection abscissa (depth) on normal to |
||
1677 |
// plane, |
||
1678 |
12 |
distance = depth; |
|
1679 |
✓✓ | 12 |
if (depth <= 0) { |
1680 |
✓✗ | 10 |
normal = new_s1.n; |
1681 |
✓✗✓✗ ✓✗✓✗ |
10 |
p1 = p2 = v - (0.5 * depth) * new_s1.n; |
1682 |
10 |
return true; |
|
1683 |
} else { |
||
1684 |
✓✗✓✗ ✓✗ |
2 |
p1 = v - depth * new_s1.n; |
1685 |
✓✗ | 2 |
p2 = v; |
1686 |
2 |
return false; |
|
1687 |
} |
||
1688 |
} |
||
1689 |
|||
1690 |
/// @brief return whether plane collides with halfspace |
||
1691 |
/// if the separation plane of the halfspace is parallel with the plane |
||
1692 |
/// return code 1, if the plane's normal is the same with halfspace's normal |
||
1693 |
/// and plane is inside halfspace, also return plane in pl return code 2, if |
||
1694 |
/// the plane's normal is oppositie to the halfspace's normal and plane is |
||
1695 |
/// inside halfspace, also return plane in pl plane is outside halfspace, |
||
1696 |
/// collision-free |
||
1697 |
/// if not parallel |
||
1698 |
/// return the intersection ray, return code 3. ray origin is p and |
||
1699 |
/// direction is d |
||
1700 |
inline bool planeHalfspaceIntersect(const Plane& s1, const Transform3f& tf1, |
||
1701 |
const Halfspace& s2, const Transform3f& tf2, |
||
1702 |
Plane& pl, Vec3f& p, Vec3f& d, |
||
1703 |
FCL_REAL& penetration_depth, int& ret) { |
||
1704 |
Plane new_s1 = transform(s1, tf1); |
||
1705 |
Halfspace new_s2 = transform(s2, tf2); |
||
1706 |
|||
1707 |
ret = 0; |
||
1708 |
|||
1709 |
penetration_depth = (std::numeric_limits<FCL_REAL>::max)(); |
||
1710 |
Vec3f dir = (new_s1.n).cross(new_s2.n); |
||
1711 |
FCL_REAL dir_norm = dir.squaredNorm(); |
||
1712 |
if (dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel |
||
1713 |
{ |
||
1714 |
if ((new_s1.n).dot(new_s2.n) > 0) { |
||
1715 |
penetration_depth = new_s2.d - new_s1.d; |
||
1716 |
if (penetration_depth < 0) |
||
1717 |
return false; |
||
1718 |
else { |
||
1719 |
ret = 1; |
||
1720 |
pl = new_s1; |
||
1721 |
return true; |
||
1722 |
} |
||
1723 |
} else { |
||
1724 |
penetration_depth = -(new_s1.d + new_s2.d); |
||
1725 |
if (penetration_depth < 0) |
||
1726 |
return false; |
||
1727 |
else { |
||
1728 |
ret = 2; |
||
1729 |
pl = new_s1; |
||
1730 |
return true; |
||
1731 |
} |
||
1732 |
} |
||
1733 |
} |
||
1734 |
|||
1735 |
Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; |
||
1736 |
Vec3f origin = n.cross(dir); |
||
1737 |
origin *= (1.0 / dir_norm); |
||
1738 |
|||
1739 |
p = origin; |
||
1740 |
d = dir; |
||
1741 |
ret = 3; |
||
1742 |
|||
1743 |
return true; |
||
1744 |
} |
||
1745 |
|||
1746 |
///@ brief return whether two halfspace intersect |
||
1747 |
/// if the separation planes of the two halfspaces are parallel |
||
1748 |
/// return code 1, if two halfspaces' normal are same and s1 is in s2, also |
||
1749 |
/// return s1 in s; return code 2, if two halfspaces' normal are same and s2 |
||
1750 |
/// is in s1, also return s2 in s; return code 3, if two halfspaces' normal |
||
1751 |
/// are opposite and s1 and s2 are into each other; collision free, if two |
||
1752 |
/// halfspaces' are separate; |
||
1753 |
/// if the separation planes of the two halfspaces are not parallel, return |
||
1754 |
/// intersection ray, return code 4. ray origin is p and direction is d |
||
1755 |
/// collision free return code 0 |
||
1756 |
inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1, |
||
1757 |
const Halfspace& s2, const Transform3f& tf2, |
||
1758 |
Vec3f& p, Vec3f& d, Halfspace& s, |
||
1759 |
FCL_REAL& penetration_depth, int& ret) { |
||
1760 |
Halfspace new_s1 = transform(s1, tf1); |
||
1761 |
Halfspace new_s2 = transform(s2, tf2); |
||
1762 |
|||
1763 |
ret = 0; |
||
1764 |
|||
1765 |
penetration_depth = (std::numeric_limits<FCL_REAL>::max)(); |
||
1766 |
Vec3f dir = (new_s1.n).cross(new_s2.n); |
||
1767 |
FCL_REAL dir_norm = dir.squaredNorm(); |
||
1768 |
if (dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel |
||
1769 |
{ |
||
1770 |
if ((new_s1.n).dot(new_s2.n) > 0) { |
||
1771 |
if (new_s1.d < new_s2.d) // s1 is inside s2 |
||
1772 |
{ |
||
1773 |
ret = 1; |
||
1774 |
s = new_s1; |
||
1775 |
} else // s2 is inside s1 |
||
1776 |
{ |
||
1777 |
ret = 2; |
||
1778 |
s = new_s2; |
||
1779 |
} |
||
1780 |
return true; |
||
1781 |
} else { |
||
1782 |
penetration_depth = -(new_s1.d + new_s2.d); |
||
1783 |
if (penetration_depth < 0) // not collision |
||
1784 |
return false; |
||
1785 |
else // in each other |
||
1786 |
{ |
||
1787 |
ret = 3; |
||
1788 |
return true; |
||
1789 |
} |
||
1790 |
} |
||
1791 |
} |
||
1792 |
|||
1793 |
Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; |
||
1794 |
Vec3f origin = n.cross(dir); |
||
1795 |
origin *= (1.0 / dir_norm); |
||
1796 |
|||
1797 |
p = origin; |
||
1798 |
d = dir; |
||
1799 |
ret = 4; |
||
1800 |
|||
1801 |
return true; |
||
1802 |
} |
||
1803 |
|||
1804 |
/// @param p1 closest (or most penetrating) point on the Halfspace, |
||
1805 |
/// @param p2 closest (or most penetrating) point on the shape, |
||
1806 |
/// @param normal the halfspace normal. |
||
1807 |
/// @return true if the distance is negative (the shape overlaps). |
||
1808 |
4 |
inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1, |
|
1809 |
const ShapeBase& s, const Transform3f& tf2, |
||
1810 |
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, |
||
1811 |
Vec3f& normal) { |
||
1812 |
✓✗✓✗ |
4 |
Vec3f n_w = tf1.getRotation() * h.n; |
1813 |
✓✗✓✗ ✓✗ |
4 |
Vec3f n_2(tf2.getRotation().transpose() * n_w); |
1814 |
4 |
int hint = 0; |
|
1815 |
✓✗✓✗ ✓✗ |
4 |
p2 = getSupport(&s, -n_2, true, hint); |
1816 |
✓✗ | 4 |
p2 = tf2.transform(p2); |
1817 |
|||
1818 |
✓✗✓✗ |
4 |
dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d; |
1819 |
✓✗✓✗ ✓✗ |
4 |
p1 = p2 - dist * n_w; |
1820 |
✓✗ | 4 |
normal = n_w; |
1821 |
|||
1822 |
4 |
return dist <= 0; |
|
1823 |
} |
||
1824 |
|||
1825 |
template <typename T> |
||
1826 |
inline T planeIntersectTolerance() { |
||
1827 |
return 0; |
||
1828 |
} |
||
1829 |
|||
1830 |
template <> |
||
1831 |
284 |
inline double planeIntersectTolerance<double>() { |
|
1832 |
284 |
return 0.0000001; |
|
1833 |
} |
||
1834 |
|||
1835 |
template <> |
||
1836 |
float inline planeIntersectTolerance<float>() { |
||
1837 |
return 0.0001f; |
||
1838 |
} |
||
1839 |
|||
1840 |
20 |
inline bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1, |
|
1841 |
const Plane& s2, const Transform3f& tf2, |
||
1842 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
1843 |
Vec3f& normal) |
||
1844 |
// Vec3f& contact_points, FCL_REAL& penetration_depth, Vec3f* normal) |
||
1845 |
{ |
||
1846 |
✓✗ | 40 |
Plane new_s2 = transform(s2, tf2); |
1847 |
|||
1848 |
20 |
const Vec3f& center = tf1.getTranslation(); |
|
1849 |
✓✗ | 20 |
FCL_REAL signed_dist = new_s2.signedDistance(center); |
1850 |
20 |
distance = std::abs(signed_dist) - s1.radius; |
|
1851 |
✓✓ | 20 |
if (distance <= 0) { |
1852 |
✓✓ | 12 |
if (signed_dist > 0) |
1853 |
✓✗✓✗ |
6 |
normal = -new_s2.n; |
1854 |
else |
||
1855 |
✓✗ | 6 |
normal = new_s2.n; |
1856 |
✓✗✓✗ ✓✗✓✗ |
12 |
p1 = p2 = center - new_s2.n * signed_dist; |
1857 |
12 |
return true; |
|
1858 |
} else { |
||
1859 |
✓✓ | 8 |
if (signed_dist > 0) { |
1860 |
✓✗✓✗ ✓✗ |
4 |
p1 = center - s1.radius * new_s2.n; |
1861 |
✓✗✓✗ ✓✗ |
4 |
p2 = center - signed_dist * new_s2.n; |
1862 |
} else { |
||
1863 |
✓✗✓✗ ✓✗ |
4 |
p1 = center + s1.radius * new_s2.n; |
1864 |
✓✗✓✗ ✓✗ |
4 |
p2 = center + signed_dist * new_s2.n; |
1865 |
} |
||
1866 |
8 |
return false; |
|
1867 |
} |
||
1868 |
} |
||
1869 |
|||
1870 |
/// @brief box half space, a, b, c = +/- edge size |
||
1871 |
/// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d |
||
1872 |
/// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d |
||
1873 |
/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c |
||
1874 |
/// and <=0 for some a, b, c so need to check whether |d - n * T| <= |(R^T n)(a |
||
1875 |
/// v1 + b v2 + c v3)|, the reason is as follows: (R^T n) (a v1 + b v2 + c v3) |
||
1876 |
/// can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. if |d - n * T| <= |
||
1877 |
/// |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value |
||
1878 |
/// on the right side. |
||
1879 |
22 |
inline bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1, |
|
1880 |
const Plane& s2, const Transform3f& tf2, |
||
1881 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
1882 |
Vec3f& normal) |
||
1883 |
// Vec3f* contact_points, |
||
1884 |
// FCL_REAL* penetration_depth, Vec3f* normal) |
||
1885 |
{ |
||
1886 |
static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon())); |
||
1887 |
✓✗ | 44 |
const Plane new_s2 = transform(s2, tf2); |
1888 |
|||
1889 |
22 |
const Matrix3f& R = tf1.getRotation(); |
|
1890 |
22 |
const Vec3f& T = tf1.getTranslation(); |
|
1891 |
|||
1892 |
✓✗✓✗ ✓✗ |
22 |
const Vec3f Q(R.transpose() * new_s2.n); |
1893 |
✓✗✓✗ |
22 |
const Vec3f A(Q.cwiseProduct(s1.halfSide)); |
1894 |
|||
1895 |
✓✗ | 22 |
const FCL_REAL signed_dist = new_s2.signedDistance(T); |
1896 |
✓✗ | 22 |
distance = std::abs(signed_dist) - A.lpNorm<1>(); |
1897 |
✓✓ | 22 |
if (distance > 0) { |
1898 |
// Is the box above or below the plane |
||
1899 |
8 |
const bool positive = signed_dist > 0; |
|
1900 |
// Set p1 at the center of the box |
||
1901 |
✓✗ | 8 |
p1 = T; |
1902 |
✓✓ | 32 |
for (Vec3f::Index i = 0; i < 3; ++i) { |
1903 |
// scalar product between box axis and plane normal |
||
1904 |
✓✓✓✗ ✓✗ |
24 |
FCL_REAL alpha((positive ? 1 : -1) * R.col(i).dot(new_s2.n)); |
1905 |
✓✓ | 24 |
if (alpha > eps) { |
1906 |
✓✗✓✗ ✓✗✓✗ |
4 |
p1 -= R.col(i) * s1.halfSide[i]; |
1907 |
✓✓ | 20 |
} else if (alpha < -eps) { |
1908 |
✓✗✓✗ ✓✗✓✗ |
4 |
p1 += R.col(i) * s1.halfSide[i]; |
1909 |
} |
||
1910 |
} |
||
1911 |
✓✓✓✗ ✓✗✓✗ |
8 |
p2 = p1 - (positive ? distance : -distance) * new_s2.n; |
1912 |
✓✗✗✓ |
8 |
assert(new_s2.distance(p2) < 3 * eps); |
1913 |
8 |
return false; |
|
1914 |
} |
||
1915 |
|||
1916 |
// find the deepest point |
||
1917 |
✓✗ | 14 |
Vec3f p = T; |
1918 |
|||
1919 |
// when center is on the positive side of the plane, use a, b, c |
||
1920 |
// make (R^T n) (a v1 + b v2 + c v3) the minimum |
||
1921 |
// otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum |
||
1922 |
✓✓ | 14 |
int sign = (signed_dist > 0) ? 1 : -1; |
1923 |
|||
1924 |
✓✗✓✓ ✗✓✓✓ |
16 |
if (std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() || |
1925 |
✓✗ | 2 |
std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>()) { |
1926 |
✓✗✓✗ |
12 |
int sign2 = (A[0] > 0) ? -sign : sign; |
1927 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
p.noalias() += R.col(0) * (s1.halfSide[0] * sign2); |
1928 |
✓✗✓✗ ✗✓✗✓ |
4 |
} else if (std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() || |
1929 |
✓✗ | 2 |
std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>()) { |
1930 |
int sign2 = (A[1] > 0) ? -sign : sign; |
||
1931 |
p.noalias() += R.col(1) * (s1.halfSide[1] * sign2); |
||
1932 |
✓✗✓✗ ✗✓✗✓ |
4 |
} else if (std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() || |
1933 |
✓✗ | 2 |
std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>()) { |
1934 |
int sign2 = (A[2] > 0) ? -sign : sign; |
||
1935 |
p.noalias() += R.col(2) * (s1.halfSide[2] * sign2); |
||
1936 |
} else { |
||
1937 |
✓✗✓✗ ✓✗ |
2 |
Vec3f tmp(sign * R * s1.halfSide); |
1938 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
p.noalias() += (A.array() > 0).select(-tmp, tmp); |
1939 |
} |
||
1940 |
|||
1941 |
// compute the contact point by project the deepest point onto the plane |
||
1942 |
✓✓ | 14 |
if (signed_dist > 0) |
1943 |
✓✗✓✗ |
4 |
normal = -new_s2.n; |
1944 |
else |
||
1945 |
✓✗ | 10 |
normal = new_s2.n; |
1946 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
14 |
p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p); |
1947 |
|||
1948 |
14 |
return true; |
|
1949 |
} |
||
1950 |
|||
1951 |
/// Taken from book Real Time Collision Detection, from Christer Ericson |
||
1952 |
/// @param pb the closest point to the sphere center on the box surface |
||
1953 |
/// @param ps when colliding, matches pb, which is inside the sphere. |
||
1954 |
/// when not colliding, the closest point on the sphere |
||
1955 |
/// @param normal direction of motion of the box |
||
1956 |
/// @return true if the distance is negative (the shape overlaps). |
||
1957 |
293328 |
inline bool boxSphereDistance(const Box& b, const Transform3f& tfb, |
|
1958 |
const Sphere& s, const Transform3f& tfs, |
||
1959 |
FCL_REAL& dist, Vec3f& pb, Vec3f& ps, |
||
1960 |
Vec3f& normal) { |
||
1961 |
293328 |
const Vec3f& os = tfs.getTranslation(); |
|
1962 |
293328 |
const Vec3f& ob = tfb.getTranslation(); |
|
1963 |
293328 |
const Matrix3f& Rb = tfb.getRotation(); |
|
1964 |
|||
1965 |
✓✗ | 293328 |
pb = ob; |
1966 |
|||
1967 |
293328 |
bool outside = false; |
|
1968 |
✓✗✓✗ ✓✗✓✗ |
293328 |
const Vec3f os_in_b_frame(Rb.transpose() * (os - ob)); |
1969 |
293328 |
int axis = -1; |
|
1970 |
293328 |
FCL_REAL min_d = (std::numeric_limits<FCL_REAL>::max)(); |
|
1971 |
✓✓ | 1173312 |
for (int i = 0; i < 3; ++i) { |
1972 |
FCL_REAL facedist; |
||
1973 |
✓✗✓✗ ✓✓ |
879984 |
if (os_in_b_frame(i) < -b.halfSide(i)) { // outside |
1974 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
432058 |
pb.noalias() -= b.halfSide(i) * Rb.col(i); |
1975 |
432058 |
outside = true; |
|
1976 |
✓✗✓✗ ✓✓ |
447926 |
} else if (os_in_b_frame(i) > b.halfSide(i)) { // outside |
1977 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
431508 |
pb.noalias() += b.halfSide(i) * Rb.col(i); |
1978 |
431508 |
outside = true; |
|
1979 |
} else { |
||
1980 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
16418 |
pb.noalias() += os_in_b_frame(i) * Rb.col(i); |
1981 |
✓✓✓✓ |
19712 |
if (!outside && |
1982 |
✓✗✓✗ ✓✓ |
3294 |
(facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) { |
1983 |
2974 |
axis = i; |
|
1984 |
2974 |
min_d = facedist; |
|
1985 |
} |
||
1986 |
} |
||
1987 |
} |
||
1988 |
✓✗✓✗ |
293328 |
normal = pb - os; |
1989 |
✓✗ | 293328 |
FCL_REAL pdist = normal.norm(); |
1990 |
✓✓ | 293328 |
if (outside) { // pb is on the box |
1991 |
293204 |
dist = pdist - s.radius; |
|
1992 |
✓✗ | 293204 |
normal /= -pdist; |
1993 |
} else { // pb is inside the box |
||
1994 |
✓✗✓✓ |
124 |
if (os_in_b_frame(axis) >= 0) |
1995 |
✓✗✓✗ |
79 |
normal = Rb.col(axis); |
1996 |
else |
||
1997 |
✓✗✓✗ ✓✗ |
45 |
normal = -Rb.col(axis); |
1998 |
124 |
dist = -min_d - s.radius; |
|
1999 |
} |
||
2000 |
✓✓✓✓ |
293328 |
if (!outside || dist <= 0) { |
2001 |
✓✗ | 1109 |
ps = pb; |
2002 |
1109 |
return true; |
|
2003 |
} else { |
||
2004 |
✓✗✓✗ ✓✗ |
292219 |
ps = os - s.radius * normal; |
2005 |
292219 |
return false; |
|
2006 |
} |
||
2007 |
} |
||
2008 |
|||
2009 |
60 |
inline bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1, |
|
2010 |
const Plane& s2, const Transform3f& tf2, |
||
2011 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
2012 |
Vec3f& normal) { |
||
2013 |
✓✗ | 120 |
Plane new_s2 = transform(s2, tf2); |
2014 |
|||
2015 |
// position orientation of capsule |
||
2016 |
60 |
const Matrix3f& R1 = tf1.getRotation(); |
|
2017 |
60 |
const Vec3f& T1 = tf1.getTranslation(); |
|
2018 |
|||
2019 |
✓✗✓✗ |
60 |
Vec3f dir_z = R1.col(2); |
2020 |
|||
2021 |
// ends of capsule inner segment |
||
2022 |
✓✗✓✗ ✓✗ |
60 |
Vec3f a1 = T1 + dir_z * s1.halfLength; |
2023 |
✓✗✓✗ ✓✗ |
60 |
Vec3f a2 = T1 - dir_z * s1.halfLength; |
2024 |
|||
2025 |
✓✗ | 60 |
FCL_REAL d1 = new_s2.signedDistance(a1); |
2026 |
✓✗ | 60 |
FCL_REAL d2 = new_s2.signedDistance(a2); |
2027 |
|||
2028 |
60 |
FCL_REAL abs_d1 = std::abs(d1); |
|
2029 |
60 |
FCL_REAL abs_d2 = std::abs(d2); |
|
2030 |
|||
2031 |
// two end points on different side of the plane |
||
2032 |
// the contact point is the intersect of axis with the plane |
||
2033 |
// the normal is the direction to avoid intersection |
||
2034 |
// the depth is the minimum distance to resolve the collision |
||
2035 |
✓✓ | 60 |
if (d1 * d2 < -planeIntersectTolerance<FCL_REAL>()) { |
2036 |
✓✓ | 12 |
if (abs_d1 < abs_d2) { |
2037 |
4 |
distance = -abs_d1 - s1.radius; |
|
2038 |
p1 = p2 = |
||
2039 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2)); |
2040 |
✗✓ | 4 |
if (d1 < 0) |
2041 |
normal = -new_s2.n; |
||
2042 |
else |
||
2043 |
✓✗ | 4 |
normal = new_s2.n; |
2044 |
} else { |
||
2045 |
8 |
distance = -abs_d2 - s1.radius; |
|
2046 |
p1 = p2 = |
||
2047 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8 |
a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2)); |
2048 |
✓✗ | 8 |
if (d2 < 0) |
2049 |
✓✗✓✗ |
8 |
normal = -new_s2.n; |
2050 |
else |
||
2051 |
normal = new_s2.n; |
||
2052 |
} |
||
2053 |
✓✗✓✗ ✓✗✓✗ |
12 |
assert(!p1.hasNaN() && !p2.hasNaN()); |
2054 |
12 |
return true; |
|
2055 |
} |
||
2056 |
|||
2057 |
✓✓✓✗ |
48 |
if (abs_d1 > s1.radius && abs_d2 > s1.radius) { |
2058 |
// Here both capsule ends are on the same side of the plane |
||
2059 |
✓✓ | 24 |
if (d1 > 0) |
2060 |
✓✗ | 12 |
normal = new_s2.n; |
2061 |
else |
||
2062 |
✓✗✓✗ |
12 |
normal = -new_s2.n; |
2063 |
✓✓ | 24 |
if (abs_d1 < abs_d2) { |
2064 |
4 |
distance = abs_d1 - s1.radius; |
|
2065 |
✓✗✓✗ ✓✗ |
4 |
p1 = a1 - s1.radius * normal; |
2066 |
✓✗✓✗ ✓✗ |
4 |
p2 = p1 - distance * normal; |
2067 |
} else { |
||
2068 |
20 |
distance = abs_d2 - s1.radius; |
|
2069 |
✓✗✓✗ ✓✗ |
20 |
p1 = a2 - s1.radius * normal; |
2070 |
✓✗✓✗ ✓✗ |
20 |
p2 = p1 - distance * normal; |
2071 |
} |
||
2072 |
✓✗✓✗ ✓✗✓✗ |
24 |
assert(!p1.hasNaN() && !p2.hasNaN()); |
2073 |
24 |
return false; |
|
2074 |
} else { |
||
2075 |
// Both capsule ends are on the same side of the plane, but one |
||
2076 |
// is closer than the capsule radius, hence collision |
||
2077 |
24 |
distance = std::min(abs_d1, abs_d2) - s1.radius; |
|
2078 |
|||
2079 |
✓✗✓✗ |
24 |
if (abs_d1 <= s1.radius && abs_d2 <= s1.radius) { |
2080 |
✓✗✓✗ ✓✗ |
24 |
Vec3f c1 = a1 - new_s2.n * d1; |
2081 |
✓✗✓✗ ✓✗ |
24 |
Vec3f c2 = a2 - new_s2.n * d2; |
2082 |
✓✗✓✗ ✓✗✓✗ |
24 |
p1 = p2 = (c1 + c2) * 0.5; |
2083 |
} else if (abs_d1 <= s1.radius) { |
||
2084 |
Vec3f c = a1 - new_s2.n * d1; |
||
2085 |
p1 = p2 = c; |
||
2086 |
} else if (abs_d2 <= s1.radius) { |
||
2087 |
Vec3f c = a2 - new_s2.n * d2; |
||
2088 |
p1 = p2 = c; |
||
2089 |
} else { |
||
2090 |
assert(false); |
||
2091 |
} |
||
2092 |
|||
2093 |
✓✓ | 24 |
if (d1 < 0) |
2094 |
✓✗ | 8 |
normal = new_s2.n; |
2095 |
else |
||
2096 |
✓✗✓✗ |
16 |
normal = -new_s2.n; |
2097 |
✓✗✓✗ ✓✗✓✗ |
24 |
assert(!p1.hasNaN() && !p2.hasNaN()); |
2098 |
24 |
return true; |
|
2099 |
} |
||
2100 |
assert(false); |
||
2101 |
} |
||
2102 |
|||
2103 |
/// @brief cylinder-plane intersect |
||
2104 |
/// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d |
||
2105 |
/// need one point to be positive and one to be negative |
||
2106 |
/// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * |
||
2107 |
/// v2)) ~ 0 (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * |
||
2108 |
/// v2)) + n * T - d ~ 0 |
||
2109 |
60 |
inline bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, |
|
2110 |
const Plane& s2, const Transform3f& tf2, |
||
2111 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
2112 |
Vec3f& normal) { |
||
2113 |
✓✗ | 120 |
Plane new_s2 = transform(s2, tf2); |
2114 |
|||
2115 |
60 |
const Matrix3f& R = tf1.getRotation(); |
|
2116 |
60 |
const Vec3f& T = tf1.getTranslation(); |
|
2117 |
|||
2118 |
✓✗✓✗ |
60 |
Vec3f dir_z = R.col(2); |
2119 |
✓✗ | 60 |
FCL_REAL cosa = dir_z.dot(new_s2.n); |
2120 |
|||
2121 |
✓✓ | 60 |
if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) { |
2122 |
✓✗ | 40 |
FCL_REAL d = new_s2.signedDistance(T); |
2123 |
40 |
distance = std::abs(d) - s1.radius; |
|
2124 |
✓✓ | 40 |
if (distance > 0) |
2125 |
16 |
return false; |
|
2126 |
else { |
||
2127 |
✓✓ | 24 |
if (d < 0) |
2128 |
✓✗ | 10 |
normal = new_s2.n; |
2129 |
else |
||
2130 |
✓✗✓✗ |
14 |
normal = -new_s2.n; |
2131 |
✓✗✓✗ ✓✗✓✗ |
24 |
p1 = p2 = T - new_s2.n * d; |
2132 |
24 |
return true; |
|
2133 |
} |
||
2134 |
} else { |
||
2135 |
✓✗✓✗ ✓✗ |
20 |
Vec3f C = dir_z * cosa - new_s2.n; |
2136 |
✓✗✓✗ ✓✗ |
40 |
if (std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() || |
2137 |
20 |
std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>()) |
|
2138 |
✓✗ | 20 |
C = Vec3f(0, 0, 0); |
2139 |
else { |
||
2140 |
FCL_REAL s = C.norm(); |
||
2141 |
s = s1.radius / s; |
||
2142 |
C *= s; |
||
2143 |
} |
||
2144 |
|||
2145 |
✓✗✓✗ ✓✗ |
20 |
Vec3f a1 = T + dir_z * (s1.halfLength); |
2146 |
✓✗✓✗ ✓✗ |
20 |
Vec3f a2 = T - dir_z * (s1.halfLength); |
2147 |
|||
2148 |
✓✗✓✗ |
20 |
Vec3f c1, c2; |
2149 |
✓✗ | 20 |
if (cosa > 0) { |
2150 |
✓✗✓✗ |
20 |
c1 = a1 - C; |
2151 |
✓✗✓✗ |
20 |
c2 = a2 + C; |
2152 |
} else { |
||
2153 |
c1 = a1 + C; |
||
2154 |
c2 = a2 - C; |
||
2155 |
} |
||
2156 |
|||
2157 |
✓✗ | 20 |
FCL_REAL d1 = new_s2.signedDistance(c1); |
2158 |
✓✗ | 20 |
FCL_REAL d2 = new_s2.signedDistance(c2); |
2159 |
|||
2160 |
✓✓ | 20 |
if (d1 * d2 <= 0) { |
2161 |
12 |
FCL_REAL abs_d1 = std::abs(d1); |
|
2162 |
12 |
FCL_REAL abs_d2 = std::abs(d2); |
|
2163 |
|||
2164 |
✓✓ | 12 |
if (abs_d1 > abs_d2) { |
2165 |
6 |
distance = -abs_d2; |
|
2166 |
✓✗✓✗ ✓✗✓✗ |
6 |
p1 = p2 = c2 - new_s2.n * d2; |
2167 |
✓✗ | 6 |
if (d2 < 0) |
2168 |
✓✗✓✗ |
6 |
normal = -new_s2.n; |
2169 |
else |
||
2170 |
normal = new_s2.n; |
||
2171 |
} else { |
||
2172 |
6 |
distance = -abs_d1; |
|
2173 |
✓✗✓✗ ✓✗✓✗ |
6 |
p1 = p2 = c1 - new_s2.n * d1; |
2174 |
✗✓ | 6 |
if (d1 < 0) |
2175 |
normal = -new_s2.n; |
||
2176 |
else |
||
2177 |
✓✗ | 6 |
normal = new_s2.n; |
2178 |
} |
||
2179 |
12 |
return true; |
|
2180 |
} else |
||
2181 |
8 |
return false; |
|
2182 |
} |
||
2183 |
} |
||
2184 |
|||
2185 |
60 |
inline bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1, |
|
2186 |
const Plane& s2, const Transform3f& tf2, |
||
2187 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
2188 |
Vec3f& normal) { |
||
2189 |
✓✗ | 120 |
Plane new_s2 = transform(s2, tf2); |
2190 |
|||
2191 |
60 |
const Matrix3f& R = tf1.getRotation(); |
|
2192 |
60 |
const Vec3f& T = tf1.getTranslation(); |
|
2193 |
|||
2194 |
✓✗✓✗ |
60 |
Vec3f dir_z = R.col(2); |
2195 |
✓✗ | 60 |
FCL_REAL cosa = dir_z.dot(new_s2.n); |
2196 |
|||
2197 |
✓✓ | 60 |
if (std::abs(cosa) < planeIntersectTolerance<FCL_REAL>()) { |
2198 |
✓✗ | 40 |
FCL_REAL d = new_s2.signedDistance(T); |
2199 |
40 |
distance = std::abs(d) - s1.radius; |
|
2200 |
✓✓ | 40 |
if (distance > 0) { |
2201 |
✓✗✓✗ |
16 |
p1 = p2 = Vec3f(0, 0, 0); |
2202 |
16 |
return false; |
|
2203 |
} else { |
||
2204 |
✓✓ | 24 |
if (d < 0) |
2205 |
✓✗ | 8 |
normal = new_s2.n; |
2206 |
else |
||
2207 |
✓✗✓✗ |
16 |
normal = -new_s2.n; |
2208 |
✓✗✓✗ ✓✗ |
24 |
p1 = p2 = T - dir_z * (s1.halfLength) + |
2209 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
48 |
dir_z * (-distance / s1.radius * s1.halfLength) - new_s2.n * d; |
2210 |
24 |
return true; |
|
2211 |
} |
||
2212 |
} else { |
||
2213 |
✓✗✓✗ ✓✗ |
20 |
Vec3f C = dir_z * cosa - new_s2.n; |
2214 |
✓✗✓✗ ✓✗ |
40 |
if (std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() || |
2215 |
20 |
std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>()) |
|
2216 |
✓✗ | 20 |
C = Vec3f(0, 0, 0); |
2217 |
else { |
||
2218 |
FCL_REAL s = C.norm(); |
||
2219 |
s = s1.radius / s; |
||
2220 |
C *= s; |
||
2221 |
} |
||
2222 |
|||
2223 |
✓✓✓✗ |
80 |
Vec3f c[3]; |
2224 |
✓✗✓✗ ✓✗ |
20 |
c[0] = T + dir_z * (s1.halfLength); |
2225 |
✓✗✓✗ ✓✗✓✗ |
20 |
c[1] = T - dir_z * (s1.halfLength) + C; |
2226 |
✓✗✓✗ ✓✗✓✗ |
20 |
c[2] = T - dir_z * (s1.halfLength) - C; |
2227 |
|||
2228 |
FCL_REAL d[3]; |
||
2229 |
✓✗ | 20 |
d[0] = new_s2.signedDistance(c[0]); |
2230 |
✓✗ | 20 |
d[1] = new_s2.signedDistance(c[1]); |
2231 |
✓✗ | 20 |
d[2] = new_s2.signedDistance(c[2]); |
2232 |
|||
2233 |
✓✓✓✓ ✗✓ |
20 |
if ((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || |
2234 |
✓✓✓✗ ✓✗ |
16 |
(d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) |
2235 |
8 |
return false; |
|
2236 |
else { |
||
2237 |
bool positive[3]; |
||
2238 |
✓✓ | 48 |
for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] >= 0); |
2239 |
|||
2240 |
12 |
int n_positive = 0; |
|
2241 |
12 |
FCL_REAL d_positive = 0, d_negative = 0; |
|
2242 |
✓✓ | 48 |
for (std::size_t i = 0; i < 3; ++i) { |
2243 |
✓✓ | 36 |
if (positive[i]) { |
2244 |
12 |
n_positive++; |
|
2245 |
✓✗ | 12 |
if (d_positive <= d[i]) d_positive = d[i]; |
2246 |
} else { |
||
2247 |
✓✗ | 24 |
if (d_negative <= -d[i]) d_negative = -d[i]; |
2248 |
} |
||
2249 |
} |
||
2250 |
|||
2251 |
12 |
distance = -std::min(d_positive, d_negative); |
|
2252 |
✓✓ | 12 |
if (d_positive > d_negative) |
2253 |
✓✗✓✗ |
6 |
normal = -new_s2.n; |
2254 |
else |
||
2255 |
✓✗ | 6 |
normal = new_s2.n; |
2256 |
✓✓✓✗ |
36 |
Vec3f p[2]; |
2257 |
✓✗ | 12 |
Vec3f q; |
2258 |
|||
2259 |
FCL_REAL p_d[2]; |
||
2260 |
12 |
FCL_REAL q_d(0); |
|
2261 |
|||
2262 |
✗✓ | 12 |
if (n_positive == 2) { |
2263 |
for (std::size_t i = 0, j = 0; i < 3; ++i) { |
||
2264 |
if (positive[i]) { |
||
2265 |
p[j] = c[i]; |
||
2266 |
p_d[j] = d[i]; |
||
2267 |
j++; |
||
2268 |
} else { |
||
2269 |
q = c[i]; |
||
2270 |
q_d = d[i]; |
||
2271 |
} |
||
2272 |
} |
||
2273 |
|||
2274 |
Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); |
||
2275 |
Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); |
||
2276 |
p1 = p2 = (t1 + t2) * 0.5; |
||
2277 |
} else { |
||
2278 |
✓✓ | 48 |
for (std::size_t i = 0, j = 0; i < 3; ++i) { |
2279 |
✓✓ | 36 |
if (!positive[i]) { |
2280 |
✓✗ | 24 |
p[j] = c[i]; |
2281 |
24 |
p_d[j] = d[i]; |
|
2282 |
24 |
j++; |
|
2283 |
} else { |
||
2284 |
✓✗ | 12 |
q = c[i]; |
2285 |
12 |
q_d = d[i]; |
|
2286 |
} |
||
2287 |
} |
||
2288 |
|||
2289 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); |
2290 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); |
2291 |
✓✗✓✗ ✓✗✓✗ |
12 |
p1 = p2 = (t1 + t2) * 0.5; |
2292 |
} |
||
2293 |
} |
||
2294 |
12 |
return true; |
|
2295 |
} |
||
2296 |
} |
||
2297 |
|||
2298 |
inline bool convexPlaneIntersect(const ConvexBase& s1, const Transform3f& tf1, |
||
2299 |
const Plane& s2, const Transform3f& tf2, |
||
2300 |
Vec3f* contact_points, |
||
2301 |
FCL_REAL* penetration_depth, Vec3f* normal) { |
||
2302 |
Plane new_s2 = transform(s2, tf2); |
||
2303 |
|||
2304 |
Vec3f v_min, v_max; |
||
2305 |
FCL_REAL d_min = (std::numeric_limits<FCL_REAL>::max)(), |
||
2306 |
d_max = -(std::numeric_limits<FCL_REAL>::max)(); |
||
2307 |
|||
2308 |
for (unsigned int i = 0; i < s1.num_points; ++i) { |
||
2309 |
Vec3f p = tf1.transform(s1.points[i]); |
||
2310 |
|||
2311 |
FCL_REAL d = new_s2.signedDistance(p); |
||
2312 |
|||
2313 |
if (d < d_min) { |
||
2314 |
d_min = d; |
||
2315 |
v_min = p; |
||
2316 |
} |
||
2317 |
if (d > d_max) { |
||
2318 |
d_max = d; |
||
2319 |
v_max = p; |
||
2320 |
} |
||
2321 |
} |
||
2322 |
|||
2323 |
if (d_min * d_max > 0) |
||
2324 |
return false; |
||
2325 |
else { |
||
2326 |
if (d_min + d_max > 0) { |
||
2327 |
if (penetration_depth) *penetration_depth = -d_min; |
||
2328 |
if (normal) *normal = -new_s2.n; |
||
2329 |
if (contact_points) *contact_points = v_min - new_s2.n * d_min; |
||
2330 |
} else { |
||
2331 |
if (penetration_depth) *penetration_depth = d_max; |
||
2332 |
if (normal) *normal = new_s2.n; |
||
2333 |
if (contact_points) *contact_points = v_max - new_s2.n * d_max; |
||
2334 |
} |
||
2335 |
return true; |
||
2336 |
} |
||
2337 |
} |
||
2338 |
|||
2339 |
11 |
inline bool planeTriangleIntersect(const Plane& s1, const Transform3f& tf1, |
|
2340 |
const Vec3f& P1, const Vec3f& P2, |
||
2341 |
const Vec3f& P3, const Transform3f& tf2, |
||
2342 |
FCL_REAL& distance, Vec3f& p1, Vec3f& p2, |
||
2343 |
Vec3f& normal) { |
||
2344 |
✓✗ | 22 |
Plane new_s1 = transform(s1, tf1); |
2345 |
|||
2346 |
✓✓✓✗ |
44 |
Vec3f c[3]; |
2347 |
✓✗ | 11 |
c[0] = tf2.transform(P1); |
2348 |
✓✗ | 11 |
c[1] = tf2.transform(P2); |
2349 |
✓✗ | 11 |
c[2] = tf2.transform(P3); |
2350 |
|||
2351 |
FCL_REAL d[3]; |
||
2352 |
✓✗ | 11 |
d[0] = new_s1.signedDistance(c[0]); |
2353 |
✓✗ | 11 |
d[1] = new_s1.signedDistance(c[1]); |
2354 |
✓✗ | 11 |
d[2] = new_s1.signedDistance(c[2]); |
2355 |
int imin; |
||
2356 |
✓✗✗✓ ✗✗ |
11 |
if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0) { |
2357 |
if (d[0] < d[1]) |
||
2358 |
if (d[0] < d[2]) { |
||
2359 |
imin = 0; |
||
2360 |
} else { // d [2] <= d[0] < d [1] |
||
2361 |
imin = 2; |
||
2362 |
} |
||
2363 |
else { // d[1] <= d[0] |
||
2364 |
if (d[2] < d[1]) { |
||
2365 |
imin = 2; |
||
2366 |
} else { // d[1] <= d[2] |
||
2367 |
imin = 1; |
||
2368 |
} |
||
2369 |
} |
||
2370 |
distance = d[imin]; |
||
2371 |
p2 = c[imin]; |
||
2372 |
p1 = c[imin] - d[imin] * new_s1.n; |
||
2373 |
return false; |
||
2374 |
} |
||
2375 |
✗✓✗✗ ✗✗ |
11 |
if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0) { |
2376 |
if (d[0] > d[1]) |
||
2377 |
if (d[0] > d[2]) { |
||
2378 |
imin = 0; |
||
2379 |
} else { // d [2] >= d[0] > d [1] |
||
2380 |
imin = 2; |
||
2381 |
} |
||
2382 |
else { // d[1] >= d[0] |
||
2383 |
if (d[2] > d[1]) { |
||
2384 |
imin = 2; |
||
2385 |
} else { // d[1] >= d[2] |
||
2386 |
imin = 1; |
||
2387 |
} |
||
2388 |
} |
||
2389 |
distance = -d[imin]; |
||
2390 |
p2 = c[imin]; |
||
2391 |
p1 = c[imin] - d[imin] * new_s1.n; |
||
2392 |
return false; |
||
2393 |
} |
||
2394 |
bool positive[3]; |
||
2395 |
✓✓ | 44 |
for (std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] > 0); |
2396 |
|||
2397 |
11 |
int n_positive = 0; |
|
2398 |
11 |
FCL_REAL d_positive = 0, d_negative = 0; |
|
2399 |
✓✓ | 44 |
for (std::size_t i = 0; i < 3; ++i) { |
2400 |
✓✓ | 33 |
if (positive[i]) { |
2401 |
12 |
n_positive++; |
|
2402 |
✓✓ | 12 |
if (d_positive <= d[i]) d_positive = d[i]; |
2403 |
} else { |
||
2404 |
✓✓ | 21 |
if (d_negative <= -d[i]) d_negative = -d[i]; |
2405 |
} |
||
2406 |
} |
||
2407 |
|||
2408 |
11 |
distance = -std::min(d_positive, d_negative); |
|
2409 |
✓✓ | 11 |
if (d_positive > d_negative) { |
2410 |
✓✗ | 7 |
normal = new_s1.n; |
2411 |
} else { |
||
2412 |
✓✗✓✗ |
4 |
normal = -new_s1.n; |
2413 |
} |
||
2414 |
✓✓✓✗ |
33 |
Vec3f p[2]; |
2415 |
✓✗ | 11 |
Vec3f q; |
2416 |
|||
2417 |
FCL_REAL p_d[2]; |
||
2418 |
11 |
FCL_REAL q_d(0); |
|
2419 |
|||
2420 |
✓✓ | 11 |
if (n_positive == 2) { |
2421 |
✓✓ | 4 |
for (std::size_t i = 0, j = 0; i < 3; ++i) { |
2422 |
✓✓ | 3 |
if (positive[i]) { |
2423 |
✓✗ | 2 |
p[j] = c[i]; |
2424 |
2 |
p_d[j] = d[i]; |
|
2425 |
2 |
j++; |
|
2426 |
} else { |
||
2427 |
✓✗ | 1 |
q = c[i]; |
2428 |
1 |
q_d = d[i]; |
|
2429 |
} |
||
2430 |
} |
||
2431 |
|||
2432 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); |
2433 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); |
2434 |
✓✗✓✗ ✓✗✓✗ |
1 |
p1 = p2 = (t1 + t2) * 0.5; |
2435 |
} else { |
||
2436 |
✓✓ | 40 |
for (std::size_t i = 0, j = 0; i < 3; ++i) { |
2437 |
✓✓ | 30 |
if (!positive[i]) { |
2438 |
✓✗ | 20 |
p[j] = c[i]; |
2439 |
20 |
p_d[j] = d[i]; |
|
2440 |
20 |
j++; |
|
2441 |
} else { |
||
2442 |
✓✗ | 10 |
q = c[i]; |
2443 |
10 |
q_d = d[i]; |
|
2444 |
} |
||
2445 |
} |
||
2446 |
|||
2447 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); |
2448 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); |
2449 |
✓✗✓✗ ✓✗✓✗ |
10 |
p1 = p2 = (t1 + t2) * 0.5; |
2450 |
} |
||
2451 |
11 |
return true; |
|
2452 |
} |
||
2453 |
|||
2454 |
inline bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3f& tf1, |
||
2455 |
const Plane& s2, const Transform3f& tf2, |
||
2456 |
Plane& pl, Vec3f& p, Vec3f& d, |
||
2457 |
FCL_REAL& penetration_depth, int& ret) { |
||
2458 |
return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, |
||
2459 |
ret); |
||
2460 |
} |
||
2461 |
|||
2462 |
inline bool planeIntersect(const Plane& s1, const Transform3f& tf1, |
||
2463 |
const Plane& s2, const Transform3f& tf2, |
||
2464 |
Vec3f* /*contact_points*/, |
||
2465 |
FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) { |
||
2466 |
Plane new_s1 = transform(s1, tf1); |
||
2467 |
Plane new_s2 = transform(s2, tf2); |
||
2468 |
|||
2469 |
FCL_REAL a = (new_s1.n).dot(new_s2.n); |
||
2470 |
if (a == 1 && new_s1.d != new_s2.d) return false; |
||
2471 |
if (a == -1 && new_s1.d != -new_s2.d) return false; |
||
2472 |
|||
2473 |
return true; |
||
2474 |
} |
||
2475 |
|||
2476 |
/// See the prototype below |
||
2477 |
21019 |
inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, |
|
2478 |
const Vec3f& P3, const Vec3f& Q1, |
||
2479 |
const Vec3f& Q2, const Vec3f& Q3, |
||
2480 |
Vec3f& normal) { |
||
2481 |
✓✗✓✗ ✓✗ |
21019 |
Vec3f u((P2 - P1).cross(P3 - P1)); |
2482 |
✓✗✓✗ |
21019 |
normal = u.normalized(); |
2483 |
✓✗✓✗ |
21019 |
FCL_REAL depth1((P1 - Q1).dot(normal)); |
2484 |
✓✗✓✗ |
21019 |
FCL_REAL depth2((P1 - Q2).dot(normal)); |
2485 |
✓✗✓✗ |
21019 |
FCL_REAL depth3((P1 - Q3).dot(normal)); |
2486 |
21019 |
return std::max(depth1, std::max(depth2, depth3)); |
|
2487 |
} |
||
2488 |
|||
2489 |
// Compute penetration distance and normal of two triangles in collision |
||
2490 |
// Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the |
||
2491 |
// minimal distance (Q1, Q2, Q3) should be translated along the normal so |
||
2492 |
// that the triangles are collision free. |
||
2493 |
// |
||
2494 |
// Note that we compute here an upper bound of the penetration distance, |
||
2495 |
// not the exact value. |
||
2496 |
inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2, |
||
2497 |
const Vec3f& P3, const Vec3f& Q1, |
||
2498 |
const Vec3f& Q2, const Vec3f& Q3, |
||
2499 |
const Transform3f& tf1, |
||
2500 |
const Transform3f& tf2, Vec3f& normal) { |
||
2501 |
Vec3f globalP1(tf1.transform(P1)); |
||
2502 |
Vec3f globalP2(tf1.transform(P2)); |
||
2503 |
Vec3f globalP3(tf1.transform(P3)); |
||
2504 |
Vec3f globalQ1(tf2.transform(Q1)); |
||
2505 |
Vec3f globalQ2(tf2.transform(Q2)); |
||
2506 |
Vec3f globalQ3(tf2.transform(Q3)); |
||
2507 |
return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2, |
||
2508 |
globalQ3, normal); |
||
2509 |
} |
||
2510 |
} // namespace details |
||
2511 |
} // namespace fcl |
||
2512 |
} // namespace hpp |
||
2513 |
|||
2514 |
#endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H |
Generated by: GCOVR (Version 4.2) |