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 */ |
||
37 |
|||
38 |
#include <hpp/fcl/shape/geometric_shapes_utility.h> |
||
39 |
#include <hpp/fcl/internal/BV_fitter.h> |
||
40 |
#include <hpp/fcl/internal/tools.h> |
||
41 |
|||
42 |
namespace hpp { |
||
43 |
namespace fcl { |
||
44 |
|||
45 |
namespace details { |
||
46 |
|||
47 |
202 |
std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf) { |
|
48 |
✓✗ | 202 |
std::vector<Vec3f> result(8); |
49 |
✓✗ | 202 |
FCL_REAL a = box.halfSide[0]; |
50 |
✓✗ | 202 |
FCL_REAL b = box.halfSide[1]; |
51 |
✓✗ | 202 |
FCL_REAL c = box.halfSide[2]; |
52 |
✓✗✓✗ |
202 |
result[0] = tf.transform(Vec3f(a, b, c)); |
53 |
✓✗✓✗ |
202 |
result[1] = tf.transform(Vec3f(a, b, -c)); |
54 |
✓✗✓✗ |
202 |
result[2] = tf.transform(Vec3f(a, -b, c)); |
55 |
✓✗✓✗ |
202 |
result[3] = tf.transform(Vec3f(a, -b, -c)); |
56 |
✓✗✓✗ |
202 |
result[4] = tf.transform(Vec3f(-a, b, c)); |
57 |
✓✗✓✗ |
202 |
result[5] = tf.transform(Vec3f(-a, b, -c)); |
58 |
✓✗✓✗ |
202 |
result[6] = tf.transform(Vec3f(-a, -b, c)); |
59 |
✓✗✓✗ |
202 |
result[7] = tf.transform(Vec3f(-a, -b, -c)); |
60 |
|||
61 |
404 |
return result; |
|
62 |
} |
||
63 |
|||
64 |
// we use icosahedron to bound the sphere |
||
65 |
17 |
std::vector<Vec3f> getBoundVertices(const Sphere& sphere, |
|
66 |
const Transform3f& tf) { |
||
67 |
✓✗ | 17 |
std::vector<Vec3f> result(12); |
68 |
17 |
const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; |
|
69 |
17 |
FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); |
|
70 |
|||
71 |
17 |
FCL_REAL a = edge_size; |
|
72 |
17 |
FCL_REAL b = m * edge_size; |
|
73 |
✓✗✓✗ |
17 |
result[0] = tf.transform(Vec3f(0, a, b)); |
74 |
✓✗✓✗ |
17 |
result[1] = tf.transform(Vec3f(0, -a, b)); |
75 |
✓✗✓✗ |
17 |
result[2] = tf.transform(Vec3f(0, a, -b)); |
76 |
✓✗✓✗ |
17 |
result[3] = tf.transform(Vec3f(0, -a, -b)); |
77 |
✓✗✓✗ |
17 |
result[4] = tf.transform(Vec3f(a, b, 0)); |
78 |
✓✗✓✗ |
17 |
result[5] = tf.transform(Vec3f(-a, b, 0)); |
79 |
✓✗✓✗ |
17 |
result[6] = tf.transform(Vec3f(a, -b, 0)); |
80 |
✓✗✓✗ |
17 |
result[7] = tf.transform(Vec3f(-a, -b, 0)); |
81 |
✓✗✓✗ |
17 |
result[8] = tf.transform(Vec3f(b, 0, a)); |
82 |
✓✗✓✗ |
17 |
result[9] = tf.transform(Vec3f(b, 0, -a)); |
83 |
✓✗✓✗ |
17 |
result[10] = tf.transform(Vec3f(-b, 0, a)); |
84 |
✓✗✓✗ |
17 |
result[11] = tf.transform(Vec3f(-b, 0, -a)); |
85 |
|||
86 |
34 |
return result; |
|
87 |
} |
||
88 |
|||
89 |
// we use scaled icosahedron to bound the ellipsoid |
||
90 |
std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid, |
||
91 |
const Transform3f& tf) { |
||
92 |
std::vector<Vec3f> result(12); |
||
93 |
const FCL_REAL phi = (1 + sqrt(5.0)) / 2.0; |
||
94 |
|||
95 |
const FCL_REAL a = sqrt(3.0) / (phi * phi); |
||
96 |
const FCL_REAL b = phi * a; |
||
97 |
|||
98 |
const FCL_REAL& A = ellipsoid.radii[0]; |
||
99 |
const FCL_REAL& B = ellipsoid.radii[1]; |
||
100 |
const FCL_REAL& C = ellipsoid.radii[2]; |
||
101 |
|||
102 |
FCL_REAL Aa = A * a; |
||
103 |
FCL_REAL Ab = A * b; |
||
104 |
FCL_REAL Ba = B * a; |
||
105 |
FCL_REAL Bb = B * b; |
||
106 |
FCL_REAL Ca = C * a; |
||
107 |
FCL_REAL Cb = C * b; |
||
108 |
result[0] = tf.transform(Vec3f(0, Ba, Cb)); |
||
109 |
result[1] = tf.transform(Vec3f(0, -Ba, Cb)); |
||
110 |
result[2] = tf.transform(Vec3f(0, Ba, -Cb)); |
||
111 |
result[3] = tf.transform(Vec3f(0, -Ba, -Cb)); |
||
112 |
result[4] = tf.transform(Vec3f(Aa, Bb, 0)); |
||
113 |
result[5] = tf.transform(Vec3f(-Aa, Bb, 0)); |
||
114 |
result[6] = tf.transform(Vec3f(Aa, -Bb, 0)); |
||
115 |
result[7] = tf.transform(Vec3f(-Aa, -Bb, 0)); |
||
116 |
result[8] = tf.transform(Vec3f(Ab, 0, Ca)); |
||
117 |
result[9] = tf.transform(Vec3f(Ab, 0, -Ca)); |
||
118 |
result[10] = tf.transform(Vec3f(-Ab, 0, Ca)); |
||
119 |
result[11] = tf.transform(Vec3f(-Ab, 0, -Ca)); |
||
120 |
|||
121 |
return result; |
||
122 |
} |
||
123 |
|||
124 |
2 |
std::vector<Vec3f> getBoundVertices(const Capsule& capsule, |
|
125 |
const Transform3f& tf) { |
||
126 |
✓✗ | 2 |
std::vector<Vec3f> result(36); |
127 |
2 |
const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; |
|
128 |
|||
129 |
2 |
FCL_REAL hl = capsule.halfLength; |
|
130 |
2 |
FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0)); |
|
131 |
2 |
FCL_REAL a = edge_size; |
|
132 |
2 |
FCL_REAL b = m * edge_size; |
|
133 |
2 |
FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0); |
|
134 |
|||
135 |
✓✗✓✗ |
2 |
result[0] = tf.transform(Vec3f(0, a, b + hl)); |
136 |
✓✗✓✗ |
2 |
result[1] = tf.transform(Vec3f(0, -a, b + hl)); |
137 |
✓✗✓✗ |
2 |
result[2] = tf.transform(Vec3f(0, a, -b + hl)); |
138 |
✓✗✓✗ |
2 |
result[3] = tf.transform(Vec3f(0, -a, -b + hl)); |
139 |
✓✗✓✗ |
2 |
result[4] = tf.transform(Vec3f(a, b, hl)); |
140 |
✓✗✓✗ |
2 |
result[5] = tf.transform(Vec3f(-a, b, hl)); |
141 |
✓✗✓✗ |
2 |
result[6] = tf.transform(Vec3f(a, -b, hl)); |
142 |
✓✗✓✗ |
2 |
result[7] = tf.transform(Vec3f(-a, -b, hl)); |
143 |
✓✗✓✗ |
2 |
result[8] = tf.transform(Vec3f(b, 0, a + hl)); |
144 |
✓✗✓✗ |
2 |
result[9] = tf.transform(Vec3f(b, 0, -a + hl)); |
145 |
✓✗✓✗ |
2 |
result[10] = tf.transform(Vec3f(-b, 0, a + hl)); |
146 |
✓✗✓✗ |
2 |
result[11] = tf.transform(Vec3f(-b, 0, -a + hl)); |
147 |
|||
148 |
✓✗✓✗ |
2 |
result[12] = tf.transform(Vec3f(0, a, b - hl)); |
149 |
✓✗✓✗ |
2 |
result[13] = tf.transform(Vec3f(0, -a, b - hl)); |
150 |
✓✗✓✗ |
2 |
result[14] = tf.transform(Vec3f(0, a, -b - hl)); |
151 |
✓✗✓✗ |
2 |
result[15] = tf.transform(Vec3f(0, -a, -b - hl)); |
152 |
✓✗✓✗ |
2 |
result[16] = tf.transform(Vec3f(a, b, -hl)); |
153 |
✓✗✓✗ |
2 |
result[17] = tf.transform(Vec3f(-a, b, -hl)); |
154 |
✓✗✓✗ |
2 |
result[18] = tf.transform(Vec3f(a, -b, -hl)); |
155 |
✓✗✓✗ |
2 |
result[19] = tf.transform(Vec3f(-a, -b, -hl)); |
156 |
✓✗✓✗ |
2 |
result[20] = tf.transform(Vec3f(b, 0, a - hl)); |
157 |
✓✗✓✗ |
2 |
result[21] = tf.transform(Vec3f(b, 0, -a - hl)); |
158 |
✓✗✓✗ |
2 |
result[22] = tf.transform(Vec3f(-b, 0, a - hl)); |
159 |
✓✗✓✗ |
2 |
result[23] = tf.transform(Vec3f(-b, 0, -a - hl)); |
160 |
|||
161 |
2 |
FCL_REAL c = 0.5 * r2; |
|
162 |
2 |
FCL_REAL d = capsule.radius; |
|
163 |
✓✗✓✗ |
2 |
result[24] = tf.transform(Vec3f(r2, 0, hl)); |
164 |
✓✗✓✗ |
2 |
result[25] = tf.transform(Vec3f(c, d, hl)); |
165 |
✓✗✓✗ |
2 |
result[26] = tf.transform(Vec3f(-c, d, hl)); |
166 |
✓✗✓✗ |
2 |
result[27] = tf.transform(Vec3f(-r2, 0, hl)); |
167 |
✓✗✓✗ |
2 |
result[28] = tf.transform(Vec3f(-c, -d, hl)); |
168 |
✓✗✓✗ |
2 |
result[29] = tf.transform(Vec3f(c, -d, hl)); |
169 |
|||
170 |
✓✗✓✗ |
2 |
result[30] = tf.transform(Vec3f(r2, 0, -hl)); |
171 |
✓✗✓✗ |
2 |
result[31] = tf.transform(Vec3f(c, d, -hl)); |
172 |
✓✗✓✗ |
2 |
result[32] = tf.transform(Vec3f(-c, d, -hl)); |
173 |
✓✗✓✗ |
2 |
result[33] = tf.transform(Vec3f(-r2, 0, -hl)); |
174 |
✓✗✓✗ |
2 |
result[34] = tf.transform(Vec3f(-c, -d, -hl)); |
175 |
✓✗✓✗ |
2 |
result[35] = tf.transform(Vec3f(c, -d, -hl)); |
176 |
|||
177 |
4 |
return result; |
|
178 |
} |
||
179 |
|||
180 |
2 |
std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf) { |
|
181 |
✓✗ | 2 |
std::vector<Vec3f> result(7); |
182 |
|||
183 |
2 |
FCL_REAL hl = cone.halfLength; |
|
184 |
2 |
FCL_REAL r2 = cone.radius * 2 / sqrt(3.0); |
|
185 |
2 |
FCL_REAL a = 0.5 * r2; |
|
186 |
2 |
FCL_REAL b = cone.radius; |
|
187 |
|||
188 |
✓✗✓✗ |
2 |
result[0] = tf.transform(Vec3f(r2, 0, -hl)); |
189 |
✓✗✓✗ |
2 |
result[1] = tf.transform(Vec3f(a, b, -hl)); |
190 |
✓✗✓✗ |
2 |
result[2] = tf.transform(Vec3f(-a, b, -hl)); |
191 |
✓✗✓✗ |
2 |
result[3] = tf.transform(Vec3f(-r2, 0, -hl)); |
192 |
✓✗✓✗ |
2 |
result[4] = tf.transform(Vec3f(-a, -b, -hl)); |
193 |
✓✗✓✗ |
2 |
result[5] = tf.transform(Vec3f(a, -b, -hl)); |
194 |
|||
195 |
✓✗✓✗ |
2 |
result[6] = tf.transform(Vec3f(0, 0, hl)); |
196 |
|||
197 |
4 |
return result; |
|
198 |
} |
||
199 |
|||
200 |
5 |
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, |
|
201 |
const Transform3f& tf) { |
||
202 |
✓✗ | 5 |
std::vector<Vec3f> result(12); |
203 |
|||
204 |
5 |
FCL_REAL hl = cylinder.halfLength; |
|
205 |
5 |
FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0); |
|
206 |
5 |
FCL_REAL a = 0.5 * r2; |
|
207 |
5 |
FCL_REAL b = cylinder.radius; |
|
208 |
|||
209 |
✓✗✓✗ |
5 |
result[0] = tf.transform(Vec3f(r2, 0, -hl)); |
210 |
✓✗✓✗ |
5 |
result[1] = tf.transform(Vec3f(a, b, -hl)); |
211 |
✓✗✓✗ |
5 |
result[2] = tf.transform(Vec3f(-a, b, -hl)); |
212 |
✓✗✓✗ |
5 |
result[3] = tf.transform(Vec3f(-r2, 0, -hl)); |
213 |
✓✗✓✗ |
5 |
result[4] = tf.transform(Vec3f(-a, -b, -hl)); |
214 |
✓✗✓✗ |
5 |
result[5] = tf.transform(Vec3f(a, -b, -hl)); |
215 |
|||
216 |
✓✗✓✗ |
5 |
result[6] = tf.transform(Vec3f(r2, 0, hl)); |
217 |
✓✗✓✗ |
5 |
result[7] = tf.transform(Vec3f(a, b, hl)); |
218 |
✓✗✓✗ |
5 |
result[8] = tf.transform(Vec3f(-a, b, hl)); |
219 |
✓✗✓✗ |
5 |
result[9] = tf.transform(Vec3f(-r2, 0, hl)); |
220 |
✓✗✓✗ |
5 |
result[10] = tf.transform(Vec3f(-a, -b, hl)); |
221 |
✓✗✓✗ |
5 |
result[11] = tf.transform(Vec3f(a, -b, hl)); |
222 |
|||
223 |
10 |
return result; |
|
224 |
} |
||
225 |
|||
226 |
std::vector<Vec3f> getBoundVertices(const ConvexBase& convex, |
||
227 |
const Transform3f& tf) { |
||
228 |
std::vector<Vec3f> result(convex.num_points); |
||
229 |
for (std::size_t i = 0; i < convex.num_points; ++i) { |
||
230 |
result[i] = tf.transform(convex.points[i]); |
||
231 |
} |
||
232 |
|||
233 |
return result; |
||
234 |
} |
||
235 |
|||
236 |
std::vector<Vec3f> getBoundVertices(const TriangleP& triangle, |
||
237 |
const Transform3f& tf) { |
||
238 |
std::vector<Vec3f> result(3); |
||
239 |
result[0] = tf.transform(triangle.a); |
||
240 |
result[1] = tf.transform(triangle.b); |
||
241 |
result[2] = tf.transform(triangle.c); |
||
242 |
|||
243 |
return result; |
||
244 |
} |
||
245 |
|||
246 |
} // namespace details |
||
247 |
|||
248 |
234 |
Halfspace transform(const Halfspace& a, const Transform3f& tf) { |
|
249 |
/// suppose the initial halfspace is n * x <= d |
||
250 |
/// after transform (R, T), x --> x' = R x + T |
||
251 |
/// and the new half space becomes n' * x' <= d' |
||
252 |
/// where n' = R * n |
||
253 |
/// and d' = d + n' * T |
||
254 |
|||
255 |
✓✗✓✗ |
234 |
Vec3f n = tf.getRotation() * a.n; |
256 |
✓✗ | 234 |
FCL_REAL d = a.d + n.dot(tf.getTranslation()); |
257 |
|||
258 |
✓✗ | 468 |
return Halfspace(n, d); |
259 |
} |
||
260 |
|||
261 |
233 |
Plane transform(const Plane& a, const Transform3f& tf) { |
|
262 |
/// suppose the initial halfspace is n * x <= d |
||
263 |
/// after transform (R, T), x --> x' = R x + T |
||
264 |
/// and the new half space becomes n' * x' <= d' |
||
265 |
/// where n' = R * n |
||
266 |
/// and d' = d + n' * T |
||
267 |
|||
268 |
✓✗✓✗ |
233 |
Vec3f n = tf.getRotation() * a.n; |
269 |
✓✗ | 233 |
FCL_REAL d = a.d + n.dot(tf.getTranslation()); |
270 |
|||
271 |
✓✗ | 466 |
return Plane(n, d); |
272 |
} |
||
273 |
|||
274 |
template <> |
||
275 |
14121 |
void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv) { |
|
276 |
14121 |
const Matrix3f& R = tf.getRotation(); |
|
277 |
14121 |
const Vec3f& T = tf.getTranslation(); |
|
278 |
|||
279 |
✓✗✓✗ ✓✗ |
14121 |
Vec3f v_delta(R.cwiseAbs() * s.halfSide); |
280 |
✓✗✓✗ |
14121 |
bv.max_ = T + v_delta; |
281 |
✓✗✓✗ |
14121 |
bv.min_ = T - v_delta; |
282 |
14121 |
} |
|
283 |
|||
284 |
template <> |
||
285 |
16942 |
void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv) { |
|
286 |
16942 |
const Vec3f& T = tf.getTranslation(); |
|
287 |
|||
288 |
✓✗✓✗ |
16942 |
Vec3f v_delta(Vec3f::Constant(s.radius)); |
289 |
✓✗✓✗ |
16942 |
bv.max_ = T + v_delta; |
290 |
✓✗✓✗ |
16942 |
bv.min_ = T - v_delta; |
291 |
16942 |
} |
|
292 |
|||
293 |
template <> |
||
294 |
void computeBV<AABB, Ellipsoid>(const Ellipsoid& e, const Transform3f& tf, |
||
295 |
AABB& bv) { |
||
296 |
const Matrix3f& R = tf.getRotation(); |
||
297 |
const Vec3f& T = tf.getTranslation(); |
||
298 |
|||
299 |
Vec3f v_delta = R * e.radii; |
||
300 |
bv.max_ = T + v_delta; |
||
301 |
bv.min_ = T - v_delta; |
||
302 |
} |
||
303 |
|||
304 |
template <> |
||
305 |
10010 |
void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, |
|
306 |
AABB& bv) { |
||
307 |
10010 |
const Matrix3f& R = tf.getRotation(); |
|
308 |
10010 |
const Vec3f& T = tf.getTranslation(); |
|
309 |
|||
310 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10010 |
Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius)); |
311 |
✓✗✓✗ |
10010 |
bv.max_ = T + v_delta; |
312 |
✓✗✓✗ |
10010 |
bv.min_ = T - v_delta; |
313 |
10010 |
} |
|
314 |
|||
315 |
template <> |
||
316 |
void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv) { |
||
317 |
const Matrix3f& R = tf.getRotation(); |
||
318 |
const Vec3f& T = tf.getTranslation(); |
||
319 |
|||
320 |
FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + |
||
321 |
fabs(R(0, 2) * s.halfLength); |
||
322 |
FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + |
||
323 |
fabs(R(1, 2) * s.halfLength); |
||
324 |
FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + |
||
325 |
fabs(R(2, 2) * s.halfLength); |
||
326 |
|||
327 |
Vec3f v_delta(x_range, y_range, z_range); |
||
328 |
bv.max_ = T + v_delta; |
||
329 |
bv.min_ = T - v_delta; |
||
330 |
} |
||
331 |
|||
332 |
template <> |
||
333 |
8912 |
void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, |
|
334 |
AABB& bv) { |
||
335 |
8912 |
const Matrix3f& R = tf.getRotation(); |
|
336 |
8912 |
const Vec3f& T = tf.getTranslation(); |
|
337 |
|||
338 |
✓✗✓✗ |
8912 |
FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + |
339 |
✓✗ | 8912 |
fabs(R(0, 2) * s.halfLength); |
340 |
✓✗✓✗ |
8912 |
FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + |
341 |
✓✗ | 8912 |
fabs(R(1, 2) * s.halfLength); |
342 |
✓✗✓✗ |
8912 |
FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + |
343 |
✓✗ | 8912 |
fabs(R(2, 2) * s.halfLength); |
344 |
|||
345 |
✓✗ | 8912 |
Vec3f v_delta(x_range, y_range, z_range); |
346 |
✓✗✓✗ |
8912 |
bv.max_ = T + v_delta; |
347 |
✓✗✓✗ |
8912 |
bv.min_ = T - v_delta; |
348 |
8912 |
} |
|
349 |
|||
350 |
template <> |
||
351 |
1 |
void computeBV<AABB, ConvexBase>(const ConvexBase& s, const Transform3f& tf, |
|
352 |
AABB& bv) { |
||
353 |
1 |
const Matrix3f& R = tf.getRotation(); |
|
354 |
1 |
const Vec3f& T = tf.getTranslation(); |
|
355 |
|||
356 |
✓✗ | 1 |
AABB bv_; |
357 |
✓✓ | 9 |
for (std::size_t i = 0; i < s.num_points; ++i) { |
358 |
✓✗✓✗ ✓✗ |
8 |
Vec3f new_p = R * s.points[i] + T; |
359 |
✓✗ | 8 |
bv_ += new_p; |
360 |
} |
||
361 |
|||
362 |
✓✗ | 1 |
bv = bv_; |
363 |
1 |
} |
|
364 |
|||
365 |
template <> |
||
366 |
void computeBV<AABB, TriangleP>(const TriangleP& s, const Transform3f& tf, |
||
367 |
AABB& bv) { |
||
368 |
bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c)); |
||
369 |
} |
||
370 |
|||
371 |
template <> |
||
372 |
void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, |
||
373 |
AABB& bv) { |
||
374 |
Halfspace new_s = transform(s, tf); |
||
375 |
const Vec3f& n = new_s.n; |
||
376 |
const FCL_REAL& d = new_s.d; |
||
377 |
|||
378 |
AABB bv_; |
||
379 |
bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)()); |
||
380 |
bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)()); |
||
381 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
382 |
// normal aligned with x axis |
||
383 |
if (n[0] < 0) |
||
384 |
bv_.min_[0] = -d; |
||
385 |
else if (n[0] > 0) |
||
386 |
bv_.max_[0] = d; |
||
387 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
388 |
// normal aligned with y axis |
||
389 |
if (n[1] < 0) |
||
390 |
bv_.min_[1] = -d; |
||
391 |
else if (n[1] > 0) |
||
392 |
bv_.max_[1] = d; |
||
393 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
394 |
// normal aligned with z axis |
||
395 |
if (n[2] < 0) |
||
396 |
bv_.min_[2] = -d; |
||
397 |
else if (n[2] > 0) |
||
398 |
bv_.max_[2] = d; |
||
399 |
} |
||
400 |
|||
401 |
bv = bv_; |
||
402 |
} |
||
403 |
|||
404 |
template <> |
||
405 |
void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv) { |
||
406 |
Plane new_s = transform(s, tf); |
||
407 |
const Vec3f& n = new_s.n; |
||
408 |
const FCL_REAL& d = new_s.d; |
||
409 |
|||
410 |
AABB bv_; |
||
411 |
bv_.min_ = Vec3f::Constant(-(std::numeric_limits<FCL_REAL>::max)()); |
||
412 |
bv_.max_ = Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)()); |
||
413 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
414 |
// normal aligned with x axis |
||
415 |
if (n[0] < 0) { |
||
416 |
bv_.min_[0] = bv_.max_[0] = -d; |
||
417 |
} else if (n[0] > 0) { |
||
418 |
bv_.min_[0] = bv_.max_[0] = d; |
||
419 |
} |
||
420 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
421 |
// normal aligned with y axis |
||
422 |
if (n[1] < 0) { |
||
423 |
bv_.min_[1] = bv_.max_[1] = -d; |
||
424 |
} else if (n[1] > 0) { |
||
425 |
bv_.min_[1] = bv_.max_[1] = d; |
||
426 |
} |
||
427 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
428 |
// normal aligned with z axis |
||
429 |
if (n[2] < 0) { |
||
430 |
bv_.min_[2] = bv_.max_[2] = -d; |
||
431 |
} else if (n[2] > 0) { |
||
432 |
bv_.min_[2] = bv_.max_[2] = d; |
||
433 |
} |
||
434 |
} |
||
435 |
|||
436 |
bv = bv_; |
||
437 |
} |
||
438 |
|||
439 |
template <> |
||
440 |
2 |
void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv) { |
|
441 |
2 |
const Matrix3f& R = tf.getRotation(); |
|
442 |
2 |
const Vec3f& T = tf.getTranslation(); |
|
443 |
|||
444 |
2 |
bv.To = T; |
|
445 |
2 |
bv.axes = R; |
|
446 |
2 |
bv.extent = s.halfSide; |
|
447 |
2 |
} |
|
448 |
|||
449 |
template <> |
||
450 |
1002 |
void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv) { |
|
451 |
1002 |
const Vec3f& T = tf.getTranslation(); |
|
452 |
|||
453 |
✓✗ | 1002 |
bv.To.noalias() = T; |
454 |
1002 |
bv.axes.setIdentity(); |
|
455 |
1002 |
bv.extent.setConstant(s.radius); |
|
456 |
1002 |
} |
|
457 |
|||
458 |
template <> |
||
459 |
1002 |
void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv) { |
|
460 |
1002 |
const Matrix3f& R = tf.getRotation(); |
|
461 |
1002 |
const Vec3f& T = tf.getTranslation(); |
|
462 |
|||
463 |
✓✗ | 1002 |
bv.To.noalias() = T; |
464 |
✓✗ | 1002 |
bv.axes.noalias() = R; |
465 |
✓✗✓✗ |
1002 |
bv.extent << s.radius, s.radius, s.halfLength + s.radius; |
466 |
1002 |
} |
|
467 |
|||
468 |
template <> |
||
469 |
1002 |
void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv) { |
|
470 |
1002 |
const Matrix3f& R = tf.getRotation(); |
|
471 |
1002 |
const Vec3f& T = tf.getTranslation(); |
|
472 |
|||
473 |
✓✗ | 1002 |
bv.To.noalias() = T; |
474 |
✓✗ | 1002 |
bv.axes.noalias() = R; |
475 |
✓✗✓✗ |
1002 |
bv.extent << s.radius, s.radius, s.halfLength; |
476 |
1002 |
} |
|
477 |
|||
478 |
template <> |
||
479 |
1003 |
void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, |
|
480 |
OBB& bv) { |
||
481 |
1003 |
const Matrix3f& R = tf.getRotation(); |
|
482 |
1003 |
const Vec3f& T = tf.getTranslation(); |
|
483 |
|||
484 |
✓✗ | 1003 |
bv.To.noalias() = T; |
485 |
✓✗ | 1003 |
bv.axes.noalias() = R; |
486 |
✓✗✓✗ |
1003 |
bv.extent << s.radius, s.radius, s.halfLength; |
487 |
1003 |
} |
|
488 |
|||
489 |
template <> |
||
490 |
void computeBV<OBB, ConvexBase>(const ConvexBase& s, const Transform3f& tf, |
||
491 |
OBB& bv) { |
||
492 |
const Matrix3f& R = tf.getRotation(); |
||
493 |
const Vec3f& T = tf.getTranslation(); |
||
494 |
|||
495 |
fit(s.points, s.num_points, bv); |
||
496 |
|||
497 |
bv.axes.applyOnTheLeft(R); |
||
498 |
|||
499 |
bv.To = R * bv.To + T; |
||
500 |
} |
||
501 |
|||
502 |
template <> |
||
503 |
void computeBV<OBB, Halfspace>(const Halfspace&, const Transform3f&, OBB& bv) { |
||
504 |
/// Half space can only have very rough OBB |
||
505 |
bv.axes.setIdentity(); |
||
506 |
bv.To.setZero(); |
||
507 |
bv.extent.setConstant(((std::numeric_limits<FCL_REAL>::max)())); |
||
508 |
} |
||
509 |
|||
510 |
template <> |
||
511 |
void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv) { |
||
512 |
/// Half space can only have very rough RSS |
||
513 |
bv.axes.setIdentity(); |
||
514 |
bv.Tr.setZero(); |
||
515 |
bv.length[0] = bv.length[1] = bv.radius = |
||
516 |
(std::numeric_limits<FCL_REAL>::max)(); |
||
517 |
} |
||
518 |
|||
519 |
template <> |
||
520 |
void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, |
||
521 |
OBBRSS& bv) { |
||
522 |
computeBV<OBB, Halfspace>(s, tf, bv.obb); |
||
523 |
computeBV<RSS, Halfspace>(s, tf, bv.rss); |
||
524 |
} |
||
525 |
|||
526 |
template <> |
||
527 |
void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, |
||
528 |
kIOS& bv) { |
||
529 |
bv.num_spheres = 1; |
||
530 |
computeBV<OBB, Halfspace>(s, tf, bv.obb); |
||
531 |
bv.spheres[0].o = Vec3f(); |
||
532 |
bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)(); |
||
533 |
} |
||
534 |
|||
535 |
template <> |
||
536 |
void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, |
||
537 |
KDOP<16>& bv) { |
||
538 |
Halfspace new_s = transform(s, tf); |
||
539 |
const Vec3f& n = new_s.n; |
||
540 |
const FCL_REAL& d = new_s.d; |
||
541 |
|||
542 |
const short D = 8; |
||
543 |
for (short i = 0; i < D; ++i) |
||
544 |
bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)(); |
||
545 |
for (short i = D; i < 2 * D; ++i) |
||
546 |
bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)(); |
||
547 |
|||
548 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
549 |
if (n[0] > 0) |
||
550 |
bv.dist(D) = d; |
||
551 |
else |
||
552 |
bv.dist(0) = -d; |
||
553 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
554 |
if (n[1] > 0) |
||
555 |
bv.dist(D + 1) = d; |
||
556 |
else |
||
557 |
bv.dist(1) = -d; |
||
558 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
559 |
if (n[2] > 0) |
||
560 |
bv.dist(D + 2) = d; |
||
561 |
else |
||
562 |
bv.dist(2) = -d; |
||
563 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { |
||
564 |
if (n[0] > 0) |
||
565 |
bv.dist(D + 3) = n[0] * d * 2; |
||
566 |
else |
||
567 |
bv.dist(3) = n[0] * d * 2; |
||
568 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { |
||
569 |
if (n[1] > 0) |
||
570 |
bv.dist(D + 4) = n[0] * d * 2; |
||
571 |
else |
||
572 |
bv.dist(4) = n[0] * d * 2; |
||
573 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { |
||
574 |
if (n[1] > 0) |
||
575 |
bv.dist(D + 5) = n[1] * d * 2; |
||
576 |
else |
||
577 |
bv.dist(5) = n[1] * d * 2; |
||
578 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
579 |
if (n[0] > 0) |
||
580 |
bv.dist(D + 6) = n[0] * d * 2; |
||
581 |
else |
||
582 |
bv.dist(6) = n[0] * d * 2; |
||
583 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
584 |
if (n[0] > 0) |
||
585 |
bv.dist(D + 7) = n[0] * d * 2; |
||
586 |
else |
||
587 |
bv.dist(7) = n[0] * d * 2; |
||
588 |
} |
||
589 |
} |
||
590 |
|||
591 |
template <> |
||
592 |
void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, |
||
593 |
KDOP<18>& bv) { |
||
594 |
Halfspace new_s = transform(s, tf); |
||
595 |
const Vec3f& n = new_s.n; |
||
596 |
const FCL_REAL& d = new_s.d; |
||
597 |
|||
598 |
const short D = 9; |
||
599 |
|||
600 |
for (short i = 0; i < D; ++i) |
||
601 |
bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)(); |
||
602 |
for (short i = D; i < 2 * D; ++i) |
||
603 |
bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)(); |
||
604 |
|||
605 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
606 |
if (n[0] > 0) |
||
607 |
bv.dist(D) = d; |
||
608 |
else |
||
609 |
bv.dist(0) = -d; |
||
610 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
611 |
if (n[1] > 0) |
||
612 |
bv.dist(D + 1) = d; |
||
613 |
else |
||
614 |
bv.dist(1) = -d; |
||
615 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
616 |
if (n[2] > 0) |
||
617 |
bv.dist(D + 2) = d; |
||
618 |
else |
||
619 |
bv.dist(2) = -d; |
||
620 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { |
||
621 |
if (n[0] > 0) |
||
622 |
bv.dist(D + 3) = n[0] * d * 2; |
||
623 |
else |
||
624 |
bv.dist(3) = n[0] * d * 2; |
||
625 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { |
||
626 |
if (n[1] > 0) |
||
627 |
bv.dist(D + 4) = n[0] * d * 2; |
||
628 |
else |
||
629 |
bv.dist(4) = n[0] * d * 2; |
||
630 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { |
||
631 |
if (n[1] > 0) |
||
632 |
bv.dist(D + 5) = n[1] * d * 2; |
||
633 |
else |
||
634 |
bv.dist(5) = n[1] * d * 2; |
||
635 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
636 |
if (n[0] > 0) |
||
637 |
bv.dist(D + 6) = n[0] * d * 2; |
||
638 |
else |
||
639 |
bv.dist(6) = n[0] * d * 2; |
||
640 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
641 |
if (n[0] > 0) |
||
642 |
bv.dist(D + 7) = n[0] * d * 2; |
||
643 |
else |
||
644 |
bv.dist(7) = n[0] * d * 2; |
||
645 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { |
||
646 |
if (n[1] > 0) |
||
647 |
bv.dist(D + 8) = n[1] * d * 2; |
||
648 |
else |
||
649 |
bv.dist(8) = n[1] * d * 2; |
||
650 |
} |
||
651 |
} |
||
652 |
|||
653 |
template <> |
||
654 |
void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, |
||
655 |
KDOP<24>& bv) { |
||
656 |
Halfspace new_s = transform(s, tf); |
||
657 |
const Vec3f& n = new_s.n; |
||
658 |
const FCL_REAL& d = new_s.d; |
||
659 |
|||
660 |
const short D = 12; |
||
661 |
|||
662 |
for (short i = 0; i < D; ++i) |
||
663 |
bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)(); |
||
664 |
for (short i = D; i < 2 * D; ++i) |
||
665 |
bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)(); |
||
666 |
|||
667 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
668 |
if (n[0] > 0) |
||
669 |
bv.dist(D) = d; |
||
670 |
else |
||
671 |
bv.dist(0) = -d; |
||
672 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
673 |
if (n[1] > 0) |
||
674 |
bv.dist(D + 1) = d; |
||
675 |
else |
||
676 |
bv.dist(1) = -d; |
||
677 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
678 |
if (n[2] > 0) |
||
679 |
bv.dist(D + 2) = d; |
||
680 |
else |
||
681 |
bv.dist(2) = -d; |
||
682 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { |
||
683 |
if (n[0] > 0) |
||
684 |
bv.dist(D + 3) = n[0] * d * 2; |
||
685 |
else |
||
686 |
bv.dist(3) = n[0] * d * 2; |
||
687 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { |
||
688 |
if (n[1] > 0) |
||
689 |
bv.dist(D + 4) = n[0] * d * 2; |
||
690 |
else |
||
691 |
bv.dist(4) = n[0] * d * 2; |
||
692 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { |
||
693 |
if (n[1] > 0) |
||
694 |
bv.dist(D + 5) = n[1] * d * 2; |
||
695 |
else |
||
696 |
bv.dist(5) = n[1] * d * 2; |
||
697 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
698 |
if (n[0] > 0) |
||
699 |
bv.dist(D + 6) = n[0] * d * 2; |
||
700 |
else |
||
701 |
bv.dist(6) = n[0] * d * 2; |
||
702 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
703 |
if (n[0] > 0) |
||
704 |
bv.dist(D + 7) = n[0] * d * 2; |
||
705 |
else |
||
706 |
bv.dist(7) = n[0] * d * 2; |
||
707 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { |
||
708 |
if (n[1] > 0) |
||
709 |
bv.dist(D + 8) = n[1] * d * 2; |
||
710 |
else |
||
711 |
bv.dist(8) = n[1] * d * 2; |
||
712 |
} else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
713 |
if (n[0] > 0) |
||
714 |
bv.dist(D + 9) = n[0] * d * 3; |
||
715 |
else |
||
716 |
bv.dist(9) = n[0] * d * 3; |
||
717 |
} else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { |
||
718 |
if (n[0] > 0) |
||
719 |
bv.dist(D + 10) = n[0] * d * 3; |
||
720 |
else |
||
721 |
bv.dist(10) = n[0] * d * 3; |
||
722 |
} else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
723 |
if (n[1] > 0) |
||
724 |
bv.dist(D + 11) = n[1] * d * 3; |
||
725 |
else |
||
726 |
bv.dist(11) = n[1] * d * 3; |
||
727 |
} |
||
728 |
} |
||
729 |
|||
730 |
template <> |
||
731 |
void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv) { |
||
732 |
Vec3f n = tf.getRotation() * s.n; |
||
733 |
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); |
||
734 |
bv.axes.col(0).noalias() = n; |
||
735 |
|||
736 |
bv.extent << 0, (std::numeric_limits<FCL_REAL>::max)(), |
||
737 |
(std::numeric_limits<FCL_REAL>::max)(); |
||
738 |
|||
739 |
Vec3f p = s.n * s.d; |
||
740 |
bv.To = |
||
741 |
tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T |
||
742 |
} |
||
743 |
|||
744 |
template <> |
||
745 |
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv) { |
||
746 |
Vec3f n = tf.getRotation() * s.n; |
||
747 |
|||
748 |
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2)); |
||
749 |
bv.axes.col(0).noalias() = n; |
||
750 |
|||
751 |
bv.length[0] = (std::numeric_limits<FCL_REAL>::max)(); |
||
752 |
bv.length[1] = (std::numeric_limits<FCL_REAL>::max)(); |
||
753 |
|||
754 |
bv.radius = 0; |
||
755 |
|||
756 |
Vec3f p = s.n * s.d; |
||
757 |
bv.Tr = tf.transform(p); |
||
758 |
} |
||
759 |
|||
760 |
template <> |
||
761 |
void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, |
||
762 |
OBBRSS& bv) { |
||
763 |
computeBV<OBB, Plane>(s, tf, bv.obb); |
||
764 |
computeBV<RSS, Plane>(s, tf, bv.rss); |
||
765 |
} |
||
766 |
|||
767 |
template <> |
||
768 |
void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv) { |
||
769 |
bv.num_spheres = 1; |
||
770 |
computeBV<OBB, Plane>(s, tf, bv.obb); |
||
771 |
bv.spheres[0].o = Vec3f(); |
||
772 |
bv.spheres[0].r = (std::numeric_limits<FCL_REAL>::max)(); |
||
773 |
} |
||
774 |
|||
775 |
template <> |
||
776 |
void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, |
||
777 |
KDOP<16>& bv) { |
||
778 |
Plane new_s = transform(s, tf); |
||
779 |
const Vec3f& n = new_s.n; |
||
780 |
const FCL_REAL& d = new_s.d; |
||
781 |
|||
782 |
const short D = 8; |
||
783 |
|||
784 |
for (short i = 0; i < D; ++i) |
||
785 |
bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)(); |
||
786 |
for (short i = D; i < 2 * D; ++i) |
||
787 |
bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)(); |
||
788 |
|||
789 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
790 |
if (n[0] > 0) |
||
791 |
bv.dist(0) = bv.dist(D) = d; |
||
792 |
else |
||
793 |
bv.dist(0) = bv.dist(D) = -d; |
||
794 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
795 |
if (n[1] > 0) |
||
796 |
bv.dist(1) = bv.dist(D + 1) = d; |
||
797 |
else |
||
798 |
bv.dist(1) = bv.dist(D + 1) = -d; |
||
799 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
800 |
if (n[2] > 0) |
||
801 |
bv.dist(2) = bv.dist(D + 2) = d; |
||
802 |
else |
||
803 |
bv.dist(2) = bv.dist(D + 2) = -d; |
||
804 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { |
||
805 |
bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; |
||
806 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { |
||
807 |
bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; |
||
808 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { |
||
809 |
bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; |
||
810 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
811 |
bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; |
||
812 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
813 |
bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; |
||
814 |
} |
||
815 |
} |
||
816 |
|||
817 |
template <> |
||
818 |
void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, |
||
819 |
KDOP<18>& bv) { |
||
820 |
Plane new_s = transform(s, tf); |
||
821 |
const Vec3f& n = new_s.n; |
||
822 |
const FCL_REAL& d = new_s.d; |
||
823 |
|||
824 |
const short D = 9; |
||
825 |
|||
826 |
for (short i = 0; i < D; ++i) |
||
827 |
bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)(); |
||
828 |
for (short i = D; i < 2 * D; ++i) |
||
829 |
bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)(); |
||
830 |
|||
831 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
832 |
if (n[0] > 0) |
||
833 |
bv.dist(0) = bv.dist(D) = d; |
||
834 |
else |
||
835 |
bv.dist(0) = bv.dist(D) = -d; |
||
836 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
837 |
if (n[1] > 0) |
||
838 |
bv.dist(1) = bv.dist(D + 1) = d; |
||
839 |
else |
||
840 |
bv.dist(1) = bv.dist(D + 1) = -d; |
||
841 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
842 |
if (n[2] > 0) |
||
843 |
bv.dist(2) = bv.dist(D + 2) = d; |
||
844 |
else |
||
845 |
bv.dist(2) = bv.dist(D + 2) = -d; |
||
846 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { |
||
847 |
bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; |
||
848 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { |
||
849 |
bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; |
||
850 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { |
||
851 |
bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; |
||
852 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
853 |
bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; |
||
854 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
855 |
bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; |
||
856 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { |
||
857 |
bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; |
||
858 |
} |
||
859 |
} |
||
860 |
|||
861 |
template <> |
||
862 |
void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, |
||
863 |
KDOP<24>& bv) { |
||
864 |
Plane new_s = transform(s, tf); |
||
865 |
const Vec3f& n = new_s.n; |
||
866 |
const FCL_REAL& d = new_s.d; |
||
867 |
|||
868 |
const short D = 12; |
||
869 |
|||
870 |
for (short i = 0; i < D; ++i) |
||
871 |
bv.dist(i) = -(std::numeric_limits<FCL_REAL>::max)(); |
||
872 |
for (short i = D; i < 2 * D; ++i) |
||
873 |
bv.dist(i) = (std::numeric_limits<FCL_REAL>::max)(); |
||
874 |
|||
875 |
if (n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
876 |
if (n[0] > 0) |
||
877 |
bv.dist(0) = bv.dist(D) = d; |
||
878 |
else |
||
879 |
bv.dist(0) = bv.dist(D) = -d; |
||
880 |
} else if (n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { |
||
881 |
if (n[1] > 0) |
||
882 |
bv.dist(1) = bv.dist(D + 1) = d; |
||
883 |
else |
||
884 |
bv.dist(1) = bv.dist(D + 1) = -d; |
||
885 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { |
||
886 |
if (n[2] > 0) |
||
887 |
bv.dist(2) = bv.dist(D + 2) = d; |
||
888 |
else |
||
889 |
bv.dist(2) = bv.dist(D + 2) = -d; |
||
890 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { |
||
891 |
bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; |
||
892 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { |
||
893 |
bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; |
||
894 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { |
||
895 |
bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; |
||
896 |
} else if (n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
897 |
bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; |
||
898 |
} else if (n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
899 |
bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; |
||
900 |
} else if (n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { |
||
901 |
bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; |
||
902 |
} else if (n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { |
||
903 |
bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; |
||
904 |
} else if (n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { |
||
905 |
bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; |
||
906 |
} else if (n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { |
||
907 |
bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; |
||
908 |
} |
||
909 |
} |
||
910 |
|||
911 |
40 |
void constructBox(const AABB& bv, Box& box, Transform3f& tf) { |
|
912 |
✓✗✓✗ ✓✗ |
40 |
box = Box(bv.max_ - bv.min_); |
913 |
✓✗✓✗ |
40 |
tf = Transform3f(bv.center()); |
914 |
40 |
} |
|
915 |
|||
916 |
void constructBox(const OBB& bv, Box& box, Transform3f& tf) { |
||
917 |
box = Box(bv.extent * 2); |
||
918 |
tf = Transform3f(bv.axes, bv.To); |
||
919 |
} |
||
920 |
|||
921 |
void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) { |
||
922 |
box = Box(bv.obb.extent * 2); |
||
923 |
tf = Transform3f(bv.obb.axes, bv.obb.To); |
||
924 |
} |
||
925 |
|||
926 |
void constructBox(const kIOS& bv, Box& box, Transform3f& tf) { |
||
927 |
box = Box(bv.obb.extent * 2); |
||
928 |
tf = Transform3f(bv.obb.axes, bv.obb.To); |
||
929 |
} |
||
930 |
|||
931 |
void constructBox(const RSS& bv, Box& box, Transform3f& tf) { |
||
932 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
933 |
tf = Transform3f(bv.axes, bv.Tr); |
||
934 |
} |
||
935 |
|||
936 |
void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) { |
||
937 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
938 |
tf = Transform3f(bv.center()); |
||
939 |
} |
||
940 |
|||
941 |
void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) { |
||
942 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
943 |
tf = Transform3f(bv.center()); |
||
944 |
} |
||
945 |
|||
946 |
void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) { |
||
947 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
948 |
tf = Transform3f(bv.center()); |
||
949 |
} |
||
950 |
|||
951 |
1541 |
void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, |
|
952 |
Transform3f& tf) { |
||
953 |
✓✗✓✗ ✓✗ |
1541 |
box = Box(bv.max_ - bv.min_); |
954 |
✓✗✓✗ ✓✗ |
1541 |
tf = tf_bv * Transform3f(bv.center()); |
955 |
1541 |
} |
|
956 |
|||
957 |
void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, |
||
958 |
Transform3f& tf) { |
||
959 |
box = Box(bv.extent * 2); |
||
960 |
tf = tf_bv * Transform3f(bv.axes, bv.To); |
||
961 |
} |
||
962 |
|||
963 |
void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, |
||
964 |
Transform3f& tf) { |
||
965 |
box = Box(bv.obb.extent * 2); |
||
966 |
tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); |
||
967 |
} |
||
968 |
|||
969 |
void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, |
||
970 |
Transform3f& tf) { |
||
971 |
box = Box(bv.obb.extent * 2); |
||
972 |
tf = tf_bv * Transform3f(bv.obb.axes, bv.obb.To); |
||
973 |
} |
||
974 |
|||
975 |
void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, |
||
976 |
Transform3f& tf) { |
||
977 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
978 |
tf = tf_bv * Transform3f(bv.axes, bv.Tr); |
||
979 |
} |
||
980 |
|||
981 |
void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, |
||
982 |
Transform3f& tf) { |
||
983 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
984 |
tf = tf_bv * Transform3f(bv.center()); |
||
985 |
} |
||
986 |
|||
987 |
void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, |
||
988 |
Transform3f& tf) { |
||
989 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
990 |
tf = tf_bv * Transform3f(bv.center()); |
||
991 |
} |
||
992 |
|||
993 |
void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, |
||
994 |
Transform3f& tf) { |
||
995 |
box = Box(bv.width(), bv.height(), bv.depth()); |
||
996 |
tf = tf_bv * Transform3f(bv.center()); |
||
997 |
} |
||
998 |
|||
999 |
} // namespace fcl |
||
1000 |
|||
1001 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |