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 |
* All rights reserved. |
||
7 |
* |
||
8 |
* Redistribution and use in source and binary forms, with or without |
||
9 |
* modification, are permitted provided that the following conditions |
||
10 |
* are met: |
||
11 |
* |
||
12 |
* * Redistributions of source code must retain the above copyright |
||
13 |
* notice, this list of conditions and the following disclaimer. |
||
14 |
* * Redistributions in binary form must reproduce the above |
||
15 |
* copyright notice, this list of conditions and the following |
||
16 |
* disclaimer in the documentation and/or other materials provided |
||
17 |
* with the distribution. |
||
18 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
19 |
* contributors may be used to endorse or promote products derived |
||
20 |
* from this software without specific prior written permission. |
||
21 |
* |
||
22 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
23 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
24 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
25 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
26 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
27 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
28 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
29 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
30 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
31 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
32 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
33 |
* POSSIBILITY OF SUCH DAMAGE. |
||
34 |
*/ |
||
35 |
|||
36 |
/** \author Jia Pan, Florent Lamiraux */ |
||
37 |
|||
38 |
#include <hpp/fcl/BV/OBB.h> |
||
39 |
#include <hpp/fcl/BVH/BVH_utility.h> |
||
40 |
#include <hpp/fcl/math/transform.h> |
||
41 |
#include <hpp/fcl/collision_data.h> |
||
42 |
#include <hpp/fcl/internal/tools.h> |
||
43 |
|||
44 |
#include <iostream> |
||
45 |
#include <limits> |
||
46 |
|||
47 |
namespace hpp { |
||
48 |
namespace fcl { |
||
49 |
|||
50 |
/// @brief Compute the 8 vertices of a OBB |
||
51 |
39222 |
inline void computeVertices(const OBB& b, Vec3f vertices[8]) { |
|
52 |
✓✗✓✗ ✓✗ |
39222 |
Matrix3f extAxes(b.axes * b.extent.asDiagonal()); |
53 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[0].noalias() = b.To + extAxes * Vec3f(-1, -1, -1); |
54 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[1].noalias() = b.To + extAxes * Vec3f(1, -1, -1); |
55 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[2].noalias() = b.To + extAxes * Vec3f(1, 1, -1); |
56 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[3].noalias() = b.To + extAxes * Vec3f(-1, 1, -1); |
57 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[4].noalias() = b.To + extAxes * Vec3f(-1, -1, 1); |
58 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[5].noalias() = b.To + extAxes * Vec3f(1, -1, 1); |
59 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[6].noalias() = b.To + extAxes * Vec3f(1, 1, 1); |
60 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
39222 |
vertices[7].noalias() = b.To + extAxes * Vec3f(-1, 1, 1); |
61 |
39222 |
} |
|
62 |
|||
63 |
/// @brief OBB merge method when the centers of two smaller OBB are far away |
||
64 |
21 |
inline OBB merge_largedist(const OBB& b1, const OBB& b2) { |
|
65 |
✓✗ | 21 |
OBB b; |
66 |
✓✓✓✗ |
357 |
Vec3f vertex[16]; |
67 |
✓✗ | 21 |
computeVertices(b1, vertex); |
68 |
✓✗ | 21 |
computeVertices(b2, vertex + 8); |
69 |
✓✗ | 21 |
Matrix3f M; |
70 |
✓✓✓✗ |
84 |
Vec3f E[3]; |
71 |
21 |
Matrix3f::Scalar s[3] = {0, 0, 0}; |
|
72 |
|||
73 |
// obb axes |
||
74 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
21 |
b.axes.col(0).noalias() = (b1.To - b2.To).normalized(); |
75 |
|||
76 |
✓✓✓✗ |
357 |
Vec3f vertex_proj[16]; |
77 |
✓✓ | 357 |
for (int i = 0; i < 16; ++i) |
78 |
✓✗ | 336 |
vertex_proj[i].noalias() = |
79 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
672 |
vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0)); |
80 |
|||
81 |
✓✗ | 21 |
getCovariance(vertex_proj, NULL, NULL, NULL, 16, M); |
82 |
✓✗ | 21 |
eigen(M, s, E); |
83 |
|||
84 |
int min, mid, max; |
||
85 |
✓✓ | 21 |
if (s[0] > s[1]) { |
86 |
3 |
max = 0; |
|
87 |
3 |
min = 1; |
|
88 |
} else { |
||
89 |
18 |
min = 0; |
|
90 |
18 |
max = 1; |
|
91 |
} |
||
92 |
✓✓ | 21 |
if (s[2] < s[min]) { |
93 |
9 |
mid = min; |
|
94 |
9 |
min = 2; |
|
95 |
✓✓ | 12 |
} else if (s[2] > s[max]) { |
96 |
6 |
mid = max; |
|
97 |
6 |
max = 2; |
|
98 |
} else { |
||
99 |
6 |
mid = 2; |
|
100 |
} |
||
101 |
|||
102 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
21 |
b.axes.col(1) << E[0][max], E[1][max], E[2][max]; |
103 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
21 |
b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid]; |
104 |
|||
105 |
// set obb centers and extensions |
||
106 |
✓✗✓✗ |
21 |
Vec3f center, extent; |
107 |
✓✗ | 21 |
getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent); |
108 |
|||
109 |
✓✗✓✗ |
21 |
b.To.noalias() = center; |
110 |
✓✗✓✗ |
21 |
b.extent.noalias() = extent; |
111 |
|||
112 |
42 |
return b; |
|
113 |
} |
||
114 |
|||
115 |
/// @brief OBB merge method when the centers of two smaller OBB are close |
||
116 |
19590 |
inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { |
|
117 |
✓✗ | 19590 |
OBB b; |
118 |
✓✗✓✗ ✓✗ |
19590 |
b.To = (b1.To + b2.To) * 0.5; |
119 |
✓✗✓✗ |
19590 |
Quaternion3f q0(b1.axes), q1(b2.axes); |
120 |
✓✗✓✓ ✓✗ |
19590 |
if (q0.dot(q1) < 0) q1.coeffs() *= -1; |
121 |
|||
122 |
✓✗✓✗ ✓✗ |
19590 |
Quaternion3f q((q0.coeffs() + q1.coeffs()).normalized()); |
123 |
✓✗ | 19590 |
b.axes = q.toRotationMatrix(); |
124 |
|||
125 |
✓✓✓✗ ✓✗ |
176310 |
Vec3f vertex[8], diff; |
126 |
19590 |
FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)(); |
|
127 |
✓✗ | 19590 |
Vec3f pmin(real_max, real_max, real_max); |
128 |
✓✗ | 19590 |
Vec3f pmax(-real_max, -real_max, -real_max); |
129 |
|||
130 |
✓✗ | 19590 |
computeVertices(b1, vertex); |
131 |
✓✓ | 176310 |
for (int i = 0; i < 8; ++i) { |
132 |
✓✗✓✗ |
156720 |
diff = vertex[i] - b.To; |
133 |
✓✓ | 626880 |
for (int j = 0; j < 3; ++j) { |
134 |
✓✗✓✗ |
470160 |
FCL_REAL dot = diff.dot(b.axes.col(j)); |
135 |
✓✗✓✓ |
470160 |
if (dot > pmax[j]) |
136 |
✓✗ | 175926 |
pmax[j] = dot; |
137 |
✓✗✓✓ |
294234 |
else if (dot < pmin[j]) |
138 |
✓✗ | 114381 |
pmin[j] = dot; |
139 |
} |
||
140 |
} |
||
141 |
|||
142 |
✓✗ | 19590 |
computeVertices(b2, vertex); |
143 |
✓✓ | 176310 |
for (int i = 0; i < 8; ++i) { |
144 |
✓✗✓✗ |
156720 |
diff = vertex[i] - b.To; |
145 |
✓✓ | 626880 |
for (int j = 0; j < 3; ++j) { |
146 |
✓✗✓✗ |
470160 |
FCL_REAL dot = diff.dot(b.axes.col(j)); |
147 |
✓✗✓✓ |
470160 |
if (dot > pmax[j]) |
148 |
✓✗ | 55653 |
pmax[j] = dot; |
149 |
✓✗✓✓ |
414507 |
else if (dot < pmin[j]) |
150 |
✓✗ | 52290 |
pmin[j] = dot; |
151 |
} |
||
152 |
} |
||
153 |
|||
154 |
✓✓ | 78360 |
for (int j = 0; j < 3; ++j) { |
155 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
58770 |
b.To.noalias() += (b.axes.col(j) * (0.5 * (pmax[j] + pmin[j]))); |
156 |
✓✗✓✗ ✓✗ |
58770 |
b.extent[j] = 0.5 * (pmax[j] - pmin[j]); |
157 |
} |
||
158 |
|||
159 |
39180 |
return b; |
|
160 |
} |
||
161 |
|||
162 |
6000 |
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, |
|
163 |
const Vec3f& b) { |
||
164 |
FCL_REAL t, s; |
||
165 |
6000 |
const FCL_REAL reps = 1e-6; |
|
166 |
|||
167 |
✓✗✓✗ ✓✗✓✗ |
6000 |
Matrix3f Bf(B.array().abs() + reps); |
168 |
// Bf += reps; |
||
169 |
|||
170 |
// if any of these tests are one-sided, then the polyhedra are disjoint |
||
171 |
|||
172 |
// A1 x A2 = A0 |
||
173 |
✓✗✓✓ ✓✗✓✗ |
6000 |
t = ((T[0] < 0.0) ? -T[0] : T[0]); |
174 |
|||
175 |
// if(t > (a[0] + Bf.dotX(b))) |
||
176 |
✓✗✓✗ ✓✗✓✓ |
6000 |
if (t > (a[0] + Bf.row(0).dot(b))) return true; |
177 |
|||
178 |
// B1 x B2 = B0 |
||
179 |
// s = B.transposeDotX(T); |
||
180 |
✓✗✓✗ |
5531 |
s = B.col(0).dot(T); |
181 |
✓✓ | 5531 |
t = ((s < 0.0) ? -s : s); |
182 |
|||
183 |
// if(t > (b[0] + Bf.transposeDotX(a))) |
||
184 |
✓✗✓✗ ✓✗✓✓ |
5531 |
if (t > (b[0] + Bf.col(0).dot(a))) return true; |
185 |
|||
186 |
// A2 x A0 = A1 |
||
187 |
✓✗✓✓ ✓✗✓✗ |
4967 |
t = ((T[1] < 0.0) ? -T[1] : T[1]); |
188 |
|||
189 |
// if(t > (a[1] + Bf.dotY(b))) |
||
190 |
✓✗✓✗ ✓✗✓✓ |
4967 |
if (t > (a[1] + Bf.row(1).dot(b))) return true; |
191 |
|||
192 |
// A0 x A1 = A2 |
||
193 |
✓✗✓✓ ✓✗✓✗ |
3865 |
t = ((T[2] < 0.0) ? -T[2] : T[2]); |
194 |
|||
195 |
// if(t > (a[2] + Bf.dotZ(b))) |
||
196 |
✓✗✓✗ ✓✗✓✓ |
3865 |
if (t > (a[2] + Bf.row(2).dot(b))) return true; |
197 |
|||
198 |
// B2 x B0 = B1 |
||
199 |
// s = B.transposeDotY(T); |
||
200 |
✓✗✓✗ |
3716 |
s = B.col(1).dot(T); |
201 |
✓✓ | 3716 |
t = ((s < 0.0) ? -s : s); |
202 |
|||
203 |
// if(t > (b[1] + Bf.transposeDotY(a))) |
||
204 |
✓✗✓✗ ✓✗✓✓ |
3716 |
if (t > (b[1] + Bf.col(1).dot(a))) return true; |
205 |
|||
206 |
// B0 x B1 = B2 |
||
207 |
// s = B.transposeDotZ(T); |
||
208 |
✓✗✓✗ |
3612 |
s = B.col(2).dot(T); |
209 |
✓✓ | 3612 |
t = ((s < 0.0) ? -s : s); |
210 |
|||
211 |
// if(t > (b[2] + Bf.transposeDotZ(a))) |
||
212 |
✓✗✓✗ ✓✗✓✓ |
3612 |
if (t > (b[2] + Bf.col(2).dot(a))) return true; |
213 |
|||
214 |
// A0 x B0 |
||
215 |
✓✗✓✗ ✓✗✓✗ |
3505 |
s = T[2] * B(1, 0) - T[1] * B(2, 0); |
216 |
✓✓ | 3505 |
t = ((s < 0.0) ? -s : s); |
217 |
|||
218 |
3505 |
if (t > |
|
219 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3505 |
(a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) |
220 |
21 |
return true; |
|
221 |
|||
222 |
// A0 x B1 |
||
223 |
✓✗✓✗ ✓✗✓✗ |
3484 |
s = T[2] * B(1, 1) - T[1] * B(2, 1); |
224 |
✓✓ | 3484 |
t = ((s < 0.0) ? -s : s); |
225 |
|||
226 |
3484 |
if (t > |
|
227 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3484 |
(a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) |
228 |
4 |
return true; |
|
229 |
|||
230 |
// A0 x B2 |
||
231 |
✓✗✓✗ ✓✗✓✗ |
3480 |
s = T[2] * B(1, 2) - T[1] * B(2, 2); |
232 |
✓✓ | 3480 |
t = ((s < 0.0) ? -s : s); |
233 |
|||
234 |
3480 |
if (t > |
|
235 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3480 |
(a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) |
236 |
18 |
return true; |
|
237 |
|||
238 |
// A1 x B0 |
||
239 |
✓✗✓✗ ✓✗✓✗ |
3462 |
s = T[0] * B(2, 0) - T[2] * B(0, 0); |
240 |
✓✓ | 3462 |
t = ((s < 0.0) ? -s : s); |
241 |
|||
242 |
3462 |
if (t > |
|
243 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3462 |
(a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) |
244 |
8 |
return true; |
|
245 |
|||
246 |
// A1 x B1 |
||
247 |
✓✗✓✗ ✓✗✓✗ |
3454 |
s = T[0] * B(2, 1) - T[2] * B(0, 1); |
248 |
✓✓ | 3454 |
t = ((s < 0.0) ? -s : s); |
249 |
|||
250 |
3454 |
if (t > |
|
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3454 |
(a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) |
252 |
2 |
return true; |
|
253 |
|||
254 |
// A1 x B2 |
||
255 |
✓✗✓✗ ✓✗✓✗ |
3452 |
s = T[0] * B(2, 2) - T[2] * B(0, 2); |
256 |
✓✓ | 3452 |
t = ((s < 0.0) ? -s : s); |
257 |
|||
258 |
3452 |
if (t > |
|
259 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3452 |
(a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) |
260 |
12 |
return true; |
|
261 |
|||
262 |
// A2 x B0 |
||
263 |
✓✗✓✗ ✓✗✓✗ |
3440 |
s = T[1] * B(0, 0) - T[0] * B(1, 0); |
264 |
✓✓ | 3440 |
t = ((s < 0.0) ? -s : s); |
265 |
|||
266 |
3440 |
if (t > |
|
267 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3440 |
(a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) |
268 |
30 |
return true; |
|
269 |
|||
270 |
// A2 x B1 |
||
271 |
✓✗✓✗ ✓✗✓✗ |
3410 |
s = T[1] * B(0, 1) - T[0] * B(1, 1); |
272 |
✓✓ | 3410 |
t = ((s < 0.0) ? -s : s); |
273 |
|||
274 |
3410 |
if (t > |
|
275 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3410 |
(a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) |
276 |
50 |
return true; |
|
277 |
|||
278 |
// A2 x B2 |
||
279 |
✓✗✓✗ ✓✗✓✗ |
3360 |
s = T[1] * B(0, 2) - T[0] * B(1, 2); |
280 |
✓✓ | 3360 |
t = ((s < 0.0) ? -s : s); |
281 |
|||
282 |
3360 |
if (t > |
|
283 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓ |
3360 |
(a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) |
284 |
8 |
return true; |
|
285 |
|||
286 |
3352 |
return false; |
|
287 |
} |
||
288 |
|||
289 |
namespace internal { |
||
290 |
124361 |
inline FCL_REAL obbDisjoint_check_A_axis(const Vec3f& T, const Vec3f& a, |
|
291 |
const Vec3f& b, const Matrix3f& Bf) { |
||
292 |
// |T| - |B| * b - a |
||
293 |
✓✗✓✗ ✓✗ |
124361 |
Vec3f AABB_corner(T.cwiseAbs() - a); |
294 |
✓✗✓✗ ✓✗ |
124361 |
AABB_corner.noalias() -= Bf * b; |
295 |
✓✗✓✗ ✓✗✓✗ |
124361 |
return AABB_corner.array().max(FCL_REAL(0)).matrix().squaredNorm(); |
296 |
} |
||
297 |
|||
298 |
85075 |
inline FCL_REAL obbDisjoint_check_B_axis(const Matrix3f& B, const Vec3f& T, |
|
299 |
const Vec3f& a, const Vec3f& b, |
||
300 |
const Matrix3f& Bf) { |
||
301 |
// Bf = |B| |
||
302 |
// | B^T T| - Bf^T * a - b |
||
303 |
85075 |
FCL_REAL s, t = 0; |
|
304 |
✓✗✓✗ ✓✗✓✗ |
85075 |
s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; |
305 |
✓✓ | 85075 |
if (s > 0) t += s * s; |
306 |
✓✗✓✗ ✓✗✓✗ |
85075 |
s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; |
307 |
✓✓ | 85075 |
if (s > 0) t += s * s; |
308 |
✓✗✓✗ ✓✗✓✗ |
85075 |
s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; |
309 |
✓✓ | 85075 |
if (s > 0) t += s * s; |
310 |
85075 |
return t; |
|
311 |
} |
||
312 |
|||
313 |
template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3> |
||
314 |
struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi { |
||
315 |
1203438 |
static inline bool run(int ia, int ja, int ka, const Matrix3f& B, |
|
316 |
const Vec3f& T, const Vec3f& a, const Vec3f& b, |
||
317 |
const Matrix3f& Bf, const FCL_REAL& breakDistance2, |
||
318 |
FCL_REAL& squaredLowerBoundDistance) { |
||
319 |
1203438 |
FCL_REAL sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); |
|
320 |
✓✓ | 1203438 |
if (sinus2 < 1e-6) return false; |
321 |
|||
322 |
1200238 |
const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); |
|
323 |
|||
324 |
1200238 |
const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + |
|
325 |
1200238 |
b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); |
|
326 |
// We need to divide by the norm || Aia x Bib || |
||
327 |
// As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 |
||
328 |
✓✓ | 1200238 |
if (diff > 0) { |
329 |
10080 |
squaredLowerBoundDistance = diff * diff / sinus2; |
|
330 |
✓✗ | 10080 |
if (squaredLowerBoundDistance > breakDistance2) { |
331 |
10080 |
return true; |
|
332 |
} |
||
333 |
} |
||
334 |
1190158 |
return false; |
|
335 |
} |
||
336 |
}; |
||
337 |
} // namespace internal |
||
338 |
|||
339 |
// B, T orientation and position of 2nd OBB in frame of 1st OBB, |
||
340 |
// a extent of 1st OBB, |
||
341 |
// b extent of 2nd OBB. |
||
342 |
// |
||
343 |
// This function tests whether bounding boxes should be broken down. |
||
344 |
// |
||
345 |
124361 |
bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, |
|
346 |
const Vec3f& a_, const Vec3f& b_, |
||
347 |
const CollisionRequest& request, |
||
348 |
FCL_REAL& squaredLowerBoundDistance) { |
||
349 |
✓✗✓✗ ✓✗ |
124361 |
assert(request.security_margin > |
350 |
-2 * (std::min)(a_.minCoeff(), b_.minCoeff()) - |
||
351 |
10 * Eigen::NumTraits<FCL_REAL>::epsilon() && |
||
352 |
"A negative security margin could not be lower than the OBB extent."); |
||
353 |
// const FCL_REAL breakDistance(request.break_distance + |
||
354 |
// request.security_margin); |
||
355 |
124361 |
const FCL_REAL breakDistance2 = |
|
356 |
124361 |
request.break_distance * request.break_distance; |
|
357 |
|||
358 |
✓✗✓✗ |
124361 |
Matrix3f Bf(B.cwiseAbs()); |
359 |
✓✗✓✗ |
124361 |
const Vec3f a((a_ + Vec3f::Constant(request.security_margin / 2)) |
360 |
✓✗ | 124361 |
.array() |
361 |
✓✗✓✗ |
124361 |
.max(FCL_REAL(0))); |
362 |
✓✗✓✗ |
124361 |
const Vec3f b((b_ + Vec3f::Constant(request.security_margin / 2)) |
363 |
✓✗ | 124361 |
.array() |
364 |
✓✗✓✗ |
124361 |
.max(FCL_REAL(0))); |
365 |
|||
366 |
// Corner of b axis aligned bounding box the closest to the origin |
||
367 |
✓✗ | 124361 |
squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); |
368 |
✓✓ | 124361 |
if (squaredLowerBoundDistance > breakDistance2) return true; |
369 |
|||
370 |
85075 |
squaredLowerBoundDistance = |
|
371 |
✓✗ | 85075 |
internal::obbDisjoint_check_B_axis(B, T, a, b, Bf); |
372 |
✓✓ | 85075 |
if (squaredLowerBoundDistance > breakDistance2) return true; |
373 |
|||
374 |
// Ai x Bj |
||
375 |
70607 |
int ja = 1, ka = 2; |
|
376 |
✓✓ | 269053 |
for (int ia = 0; ia < 3; ++ia) { |
377 |
✓✗✓✓ |
203486 |
if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run( |
378 |
ia, ja, ka, B, T, a, b, Bf, breakDistance2, |
||
379 |
squaredLowerBoundDistance)) |
||
380 |
3928 |
return true; |
|
381 |
✓✗✓✓ |
199558 |
if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run( |
382 |
ia, ja, ka, B, T, a, b, Bf, breakDistance2, |
||
383 |
squaredLowerBoundDistance)) |
||
384 |
883 |
return true; |
|
385 |
✓✗✓✓ |
198675 |
if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run( |
386 |
ia, ja, ka, B, T, a, b, Bf, breakDistance2, |
||
387 |
squaredLowerBoundDistance)) |
||
388 |
229 |
return true; |
|
389 |
198446 |
ja = ka; |
|
390 |
198446 |
ka = ia; |
|
391 |
} |
||
392 |
|||
393 |
65567 |
return false; |
|
394 |
} |
||
395 |
|||
396 |
6000 |
bool OBB::overlap(const OBB& other) const { |
|
397 |
/// compute what transform [R,T] that takes us from cs1 to cs2. |
||
398 |
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] |
||
399 |
/// First compute the rotation part, then translation part |
||
400 |
✓✗✓✗ ✓✗✓✗ |
6000 |
Vec3f T(axes.transpose() * (other.To - To)); |
401 |
✓✗✓✗ ✓✗ |
6000 |
Matrix3f R(axes.transpose() * other.axes); |
402 |
|||
403 |
✓✗ | 6000 |
return !obbDisjoint(R, T, extent, other.extent); |
404 |
} |
||
405 |
|||
406 |
69552 |
bool OBB::overlap(const OBB& other, const CollisionRequest& request, |
|
407 |
FCL_REAL& sqrDistLowerBound) const { |
||
408 |
/// compute what transform [R,T] that takes us from cs1 to cs2. |
||
409 |
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] |
||
410 |
/// First compute the rotation part, then translation part |
||
411 |
// Vec3f t = other.To - To; // T2 - T1 |
||
412 |
// Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) |
||
413 |
// Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), |
||
414 |
// axis[0].dot(other.axis[2]), |
||
415 |
// axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), |
||
416 |
// axis[1].dot(other.axis[2]), |
||
417 |
// axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), |
||
418 |
// axis[2].dot(other.axis[2])); |
||
419 |
✓✗✓✗ ✓✗✓✗ |
69552 |
Vec3f T(axes.transpose() * (other.To - To)); |
420 |
✓✗✓✗ ✓✗ |
69552 |
Matrix3f R(axes.transpose() * other.axes); |
421 |
|||
422 |
✓✗ | 69552 |
return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request, |
423 |
69552 |
sqrDistLowerBound); |
|
424 |
} |
||
425 |
|||
426 |
bool OBB::contain(const Vec3f& p) const { |
||
427 |
Vec3f local_p(p - To); |
||
428 |
FCL_REAL proj = local_p.dot(axes.col(0)); |
||
429 |
if ((proj > extent[0]) || (proj < -extent[0])) return false; |
||
430 |
|||
431 |
proj = local_p.dot(axes.col(1)); |
||
432 |
if ((proj > extent[1]) || (proj < -extent[1])) return false; |
||
433 |
|||
434 |
proj = local_p.dot(axes.col(2)); |
||
435 |
if ((proj > extent[2]) || (proj < -extent[2])) return false; |
||
436 |
|||
437 |
return true; |
||
438 |
} |
||
439 |
|||
440 |
OBB& OBB::operator+=(const Vec3f& p) { |
||
441 |
OBB bvp; |
||
442 |
bvp.To = p; |
||
443 |
bvp.axes.noalias() = axes; |
||
444 |
bvp.extent.setZero(); |
||
445 |
|||
446 |
*this += bvp; |
||
447 |
return *this; |
||
448 |
} |
||
449 |
|||
450 |
19611 |
OBB OBB::operator+(const OBB& other) const { |
|
451 |
✓✗✓✗ |
19611 |
Vec3f center_diff = To - other.To; |
452 |
✓✗✓✗ ✓✗ |
19611 |
FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); |
453 |
FCL_REAL max_extent2 = |
||
454 |
✓✗✓✗ ✓✗ |
19611 |
std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); |
455 |
✓✗✓✓ |
19611 |
if (center_diff.norm() > 2 * (max_extent + max_extent2)) { |
456 |
✓✗ | 21 |
return merge_largedist(*this, other); |
457 |
} else { |
||
458 |
✓✗ | 19590 |
return merge_smalldist(*this, other); |
459 |
} |
||
460 |
} |
||
461 |
|||
462 |
FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const { |
||
463 |
std::cerr << "OBB distance not implemented!" << std::endl; |
||
464 |
return 0.0; |
||
465 |
} |
||
466 |
|||
467 |
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, |
||
468 |
const OBB& b2) { |
||
469 |
Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To); |
||
470 |
Vec3f T(b1.axes.transpose() * Ttemp); |
||
471 |
Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); |
||
472 |
|||
473 |
return !obbDisjoint(R, T, b1.extent, b2.extent); |
||
474 |
} |
||
475 |
|||
476 |
54809 |
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2, |
|
477 |
const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { |
||
478 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
54809 |
Vec3f Ttemp(R0.transpose() * (b2.To - T0) - b1.To); |
479 |
✓✗✓✗ ✓✗ |
54809 |
Vec3f T(b1.axes.transpose() * Ttemp); |
480 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
54809 |
Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); |
481 |
|||
482 |
✓✗ | 54809 |
return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request, |
483 |
54809 |
sqrDistLowerBound); |
|
484 |
} |
||
485 |
|||
486 |
OBB translate(const OBB& bv, const Vec3f& t) { |
||
487 |
OBB res(bv); |
||
488 |
res.To += t; |
||
489 |
return res; |
||
490 |
} |
||
491 |
|||
492 |
} // namespace fcl |
||
493 |
|||
494 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |