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/internal/BV_fitter.h> |
||
39 |
#include <hpp/fcl/BVH/BVH_utility.h> |
||
40 |
#include <limits> |
||
41 |
#include <hpp/fcl/internal/tools.h> |
||
42 |
|||
43 |
namespace hpp { |
||
44 |
namespace fcl { |
||
45 |
|||
46 |
static const double kIOS_RATIO = 1.5; |
||
47 |
static const double invSinA = 2; |
||
48 |
static const double cosA = sqrt(3.0) / 2.0; |
||
49 |
|||
50 |
1764185 |
static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], |
|
51 |
Matrix3f& axes) { |
||
52 |
int min, mid, max; |
||
53 |
✓✓ | 1764185 |
if (eigenS[0] > eigenS[1]) { |
54 |
863207 |
max = 0; |
|
55 |
863207 |
min = 1; |
|
56 |
} else { |
||
57 |
900978 |
min = 0; |
|
58 |
900978 |
max = 1; |
|
59 |
} |
||
60 |
✓✓ | 1764185 |
if (eigenS[2] < eigenS[min]) { |
61 |
391695 |
mid = min; |
|
62 |
391695 |
min = 2; |
|
63 |
✓✓ | 1372490 |
} else if (eigenS[2] > eigenS[max]) { |
64 |
594752 |
mid = max; |
|
65 |
594752 |
max = 2; |
|
66 |
} else { |
||
67 |
777738 |
mid = 2; |
|
68 |
} |
||
69 |
|||
70 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1764185 |
axes.col(0) << eigenV[0][max], eigenV[1][max], eigenV[2][max]; |
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1764185 |
axes.col(1) << eigenV[0][mid], eigenV[1][mid], eigenV[2][mid]; |
72 |
✓✗✓✗ |
1764185 |
axes.col(2) << eigenV[1][max] * eigenV[2][mid] - |
73 |
✓✗✓✗ ✓✗ |
1764185 |
eigenV[1][mid] * eigenV[2][max], |
74 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1764185 |
eigenV[0][mid] * eigenV[2][max] - eigenV[0][max] * eigenV[2][mid], |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1764185 |
eigenV[0][max] * eigenV[1][mid] - eigenV[0][mid] * eigenV[1][max]; |
76 |
1764185 |
} |
|
77 |
|||
78 |
namespace OBB_fit_functions { |
||
79 |
|||
80 |
void fit1(Vec3f* ps, OBB& bv) { |
||
81 |
bv.To.noalias() = ps[0]; |
||
82 |
bv.axes.setIdentity(); |
||
83 |
bv.extent.setZero(); |
||
84 |
} |
||
85 |
|||
86 |
void fit2(Vec3f* ps, OBB& bv) { |
||
87 |
const Vec3f& p1 = ps[0]; |
||
88 |
const Vec3f& p2 = ps[1]; |
||
89 |
Vec3f p1p2 = p1 - p2; |
||
90 |
FCL_REAL len_p1p2 = p1p2.norm(); |
||
91 |
p1p2.normalize(); |
||
92 |
|||
93 |
bv.axes.col(0).noalias() = p1p2; |
||
94 |
generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2)); |
||
95 |
|||
96 |
bv.extent << len_p1p2 * 0.5, 0, 0; |
||
97 |
bv.To.noalias() = (p1 + p2) / 2; |
||
98 |
} |
||
99 |
|||
100 |
13080 |
void fit3(Vec3f* ps, OBB& bv) { |
|
101 |
13080 |
const Vec3f& p1 = ps[0]; |
|
102 |
13080 |
const Vec3f& p2 = ps[1]; |
|
103 |
13080 |
const Vec3f& p3 = ps[2]; |
|
104 |
✓✓✓✗ |
52320 |
Vec3f e[3]; |
105 |
✓✗✓✗ |
13080 |
e[0] = p1 - p2; |
106 |
✓✗✓✗ |
13080 |
e[1] = p2 - p3; |
107 |
✓✗✓✗ |
13080 |
e[2] = p3 - p1; |
108 |
FCL_REAL len[3]; |
||
109 |
✓✗ | 13080 |
len[0] = e[0].squaredNorm(); |
110 |
✓✗ | 13080 |
len[1] = e[1].squaredNorm(); |
111 |
✓✗ | 13080 |
len[2] = e[2].squaredNorm(); |
112 |
|||
113 |
13080 |
int imax = 0; |
|
114 |
✓✓ | 13080 |
if (len[1] > len[0]) imax = 1; |
115 |
✗✓ | 13080 |
if (len[2] > len[imax]) imax = 2; |
116 |
|||
117 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13080 |
bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); |
118 |
✓✗✓✗ ✓✗✓✗ |
13080 |
bv.axes.col(0).noalias() = e[imax].normalized(); |
119 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
13080 |
bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0)); |
120 |
|||
121 |
✓✗ | 13080 |
getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent); |
122 |
13080 |
} |
|
123 |
|||
124 |
void fit6(Vec3f* ps, OBB& bv) { |
||
125 |
OBB bv1, bv2; |
||
126 |
fit3(ps, bv1); |
||
127 |
fit3(ps + 3, bv2); |
||
128 |
bv = bv1 + bv2; |
||
129 |
} |
||
130 |
|||
131 |
226 |
void fitn(Vec3f* ps, unsigned int n, OBB& bv) { |
|
132 |
✓✗ | 226 |
Matrix3f M; |
133 |
✓✓✓✗ |
904 |
Vec3f E[3]; |
134 |
226 |
Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values |
|
135 |
|||
136 |
✓✗ | 226 |
getCovariance(ps, NULL, NULL, NULL, n, M); |
137 |
✓✗ | 226 |
eigen(M, s, E); |
138 |
✓✗ | 226 |
axisFromEigen(E, s, bv.axes); |
139 |
|||
140 |
// set obb centers and extensions |
||
141 |
✓✗ | 226 |
getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axes, bv.To, bv.extent); |
142 |
226 |
} |
|
143 |
|||
144 |
} // namespace OBB_fit_functions |
||
145 |
|||
146 |
namespace RSS_fit_functions { |
||
147 |
void fit1(Vec3f* ps, RSS& bv) { |
||
148 |
bv.Tr.noalias() = ps[0]; |
||
149 |
bv.axes.setIdentity(); |
||
150 |
bv.length[0] = 0; |
||
151 |
bv.length[1] = 0; |
||
152 |
bv.radius = 0; |
||
153 |
} |
||
154 |
|||
155 |
void fit2(Vec3f* ps, RSS& bv) { |
||
156 |
const Vec3f& p1 = ps[0]; |
||
157 |
const Vec3f& p2 = ps[1]; |
||
158 |
bv.axes.col(0).noalias() = p1 - p2; |
||
159 |
FCL_REAL len_p1p2 = bv.axes.col(0).norm(); |
||
160 |
bv.axes.col(0) /= len_p1p2; |
||
161 |
|||
162 |
generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2)); |
||
163 |
bv.length[0] = len_p1p2; |
||
164 |
bv.length[1] = 0; |
||
165 |
|||
166 |
bv.Tr = p2; |
||
167 |
bv.radius = 0; |
||
168 |
} |
||
169 |
|||
170 |
13080 |
void fit3(Vec3f* ps, RSS& bv) { |
|
171 |
13080 |
const Vec3f& p1 = ps[0]; |
|
172 |
13080 |
const Vec3f& p2 = ps[1]; |
|
173 |
13080 |
const Vec3f& p3 = ps[2]; |
|
174 |
✓✓✓✗ |
52320 |
Vec3f e[3]; |
175 |
✓✗✓✗ |
13080 |
e[0] = p1 - p2; |
176 |
✓✗✓✗ |
13080 |
e[1] = p2 - p3; |
177 |
✓✗✓✗ |
13080 |
e[2] = p3 - p1; |
178 |
FCL_REAL len[3]; |
||
179 |
✓✗ | 13080 |
len[0] = e[0].squaredNorm(); |
180 |
✓✗ | 13080 |
len[1] = e[1].squaredNorm(); |
181 |
✓✗ | 13080 |
len[2] = e[2].squaredNorm(); |
182 |
|||
183 |
13080 |
int imax = 0; |
|
184 |
✓✓ | 13080 |
if (len[1] > len[0]) imax = 1; |
185 |
✗✓ | 13080 |
if (len[2] > len[imax]) imax = 2; |
186 |
|||
187 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13080 |
bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); |
188 |
✓✗✓✗ ✓✗✓✗ |
13080 |
bv.axes.col(0).noalias() = e[imax].normalized(); |
189 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
13080 |
bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0)); |
190 |
|||
191 |
13080 |
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, |
|
192 |
✓✗ | 13080 |
bv.length, bv.radius); |
193 |
13080 |
} |
|
194 |
|||
195 |
void fit6(Vec3f* ps, RSS& bv) { |
||
196 |
RSS bv1, bv2; |
||
197 |
fit3(ps, bv1); |
||
198 |
fit3(ps + 3, bv2); |
||
199 |
bv = bv1 + bv2; |
||
200 |
} |
||
201 |
|||
202 |
227 |
void fitn(Vec3f* ps, unsigned int n, RSS& bv) { |
|
203 |
✓✗ | 227 |
Matrix3f M; // row first matrix |
204 |
✓✓✓✗ |
908 |
Vec3f E[3]; // row first eigen-vectors |
205 |
227 |
Matrix3f::Scalar s[3] = {0, 0, 0}; |
|
206 |
|||
207 |
✓✗ | 227 |
getCovariance(ps, NULL, NULL, NULL, n, M); |
208 |
✓✗ | 227 |
eigen(M, s, E); |
209 |
✓✗ | 227 |
axisFromEigen(E, s, bv.axes); |
210 |
|||
211 |
// set rss origin, rectangle size and radius |
||
212 |
227 |
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, |
|
213 |
✓✗ | 227 |
bv.length, bv.radius); |
214 |
227 |
} |
|
215 |
|||
216 |
} // namespace RSS_fit_functions |
||
217 |
|||
218 |
namespace kIOS_fit_functions { |
||
219 |
|||
220 |
void fit1(Vec3f* ps, kIOS& bv) { |
||
221 |
bv.num_spheres = 1; |
||
222 |
bv.spheres[0].o.noalias() = ps[0]; |
||
223 |
bv.spheres[0].r = 0; |
||
224 |
|||
225 |
bv.obb.axes.setIdentity(); |
||
226 |
bv.obb.extent.setZero(); |
||
227 |
bv.obb.To.noalias() = ps[0]; |
||
228 |
} |
||
229 |
|||
230 |
void fit2(Vec3f* ps, kIOS& bv) { |
||
231 |
bv.num_spheres = 5; |
||
232 |
|||
233 |
const Vec3f& p1 = ps[0]; |
||
234 |
const Vec3f& p2 = ps[1]; |
||
235 |
Vec3f p1p2 = p1 - p2; |
||
236 |
FCL_REAL len_p1p2 = p1p2.norm(); |
||
237 |
p1p2.normalize(); |
||
238 |
|||
239 |
Matrix3f& axes = bv.obb.axes; |
||
240 |
axes.col(0).noalias() = p1p2; |
||
241 |
generateCoordinateSystem(axes.col(0), axes.col(1), axes.col(2)); |
||
242 |
|||
243 |
FCL_REAL r0 = len_p1p2 * 0.5; |
||
244 |
bv.obb.extent << r0, 0, 0; |
||
245 |
bv.obb.To = (p1 + p2) * 0.5; |
||
246 |
|||
247 |
bv.spheres[0].o = bv.obb.To; |
||
248 |
bv.spheres[0].r = r0; |
||
249 |
|||
250 |
FCL_REAL r1 = r0 * invSinA; |
||
251 |
FCL_REAL r1cosA = r1 * cosA; |
||
252 |
bv.spheres[1].r = r1; |
||
253 |
bv.spheres[2].r = r1; |
||
254 |
Vec3f delta = axes.col(1) * r1cosA; |
||
255 |
bv.spheres[1].o = bv.spheres[0].o - delta; |
||
256 |
bv.spheres[2].o = bv.spheres[0].o + delta; |
||
257 |
|||
258 |
bv.spheres[3].r = r1; |
||
259 |
bv.spheres[4].r = r1; |
||
260 |
delta = axes.col(2) * r1cosA; |
||
261 |
bv.spheres[3].o = bv.spheres[0].o - delta; |
||
262 |
bv.spheres[4].o = bv.spheres[0].o + delta; |
||
263 |
} |
||
264 |
|||
265 |
6540 |
void fit3(Vec3f* ps, kIOS& bv) { |
|
266 |
6540 |
bv.num_spheres = 3; |
|
267 |
|||
268 |
6540 |
const Vec3f& p1 = ps[0]; |
|
269 |
6540 |
const Vec3f& p2 = ps[1]; |
|
270 |
6540 |
const Vec3f& p3 = ps[2]; |
|
271 |
✓✓✓✗ |
26160 |
Vec3f e[3]; |
272 |
✓✗✓✗ |
6540 |
e[0] = p1 - p2; |
273 |
✓✗✓✗ |
6540 |
e[1] = p2 - p3; |
274 |
✓✗✓✗ |
6540 |
e[2] = p3 - p1; |
275 |
FCL_REAL len[3]; |
||
276 |
✓✗ | 6540 |
len[0] = e[0].squaredNorm(); |
277 |
✓✗ | 6540 |
len[1] = e[1].squaredNorm(); |
278 |
✓✗ | 6540 |
len[2] = e[2].squaredNorm(); |
279 |
|||
280 |
6540 |
int imax = 0; |
|
281 |
✓✓ | 6540 |
if (len[1] > len[0]) imax = 1; |
282 |
✗✓ | 6540 |
if (len[2] > len[imax]) imax = 2; |
283 |
|||
284 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
6540 |
bv.obb.axes.col(2).noalias() = e[0].cross(e[1]).normalized(); |
285 |
✓✗✓✗ ✓✗✓✗ |
6540 |
bv.obb.axes.col(0).noalias() = e[imax].normalized(); |
286 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
6540 |
bv.obb.axes.col(1).noalias() = bv.obb.axes.col(2).cross(bv.obb.axes.col(0)); |
287 |
|||
288 |
6540 |
getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axes, bv.obb.To, |
|
289 |
✓✗ | 6540 |
bv.obb.extent); |
290 |
|||
291 |
// compute radius and center |
||
292 |
FCL_REAL r0; |
||
293 |
✓✗ | 6540 |
Vec3f center; |
294 |
✓✗ | 6540 |
circumCircleComputation(p1, p2, p3, center, r0); |
295 |
|||
296 |
✓✗ | 6540 |
bv.spheres[0].o = center; |
297 |
6540 |
bv.spheres[0].r = r0; |
|
298 |
|||
299 |
6540 |
FCL_REAL r1 = r0 * invSinA; |
|
300 |
✓✗✓✗ ✓✗ |
6540 |
Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA); |
301 |
|||
302 |
6540 |
bv.spheres[1].r = r1; |
|
303 |
✓✗✓✗ |
6540 |
bv.spheres[1].o = center - delta; |
304 |
6540 |
bv.spheres[2].r = r1; |
|
305 |
✓✗✓✗ |
6540 |
bv.spheres[2].o = center + delta; |
306 |
6540 |
} |
|
307 |
|||
308 |
1 |
void fitn(Vec3f* ps, unsigned int n, kIOS& bv) { |
|
309 |
✓✗ | 1 |
Matrix3f M; |
310 |
✓✓✓✗ |
4 |
Vec3f E[3]; |
311 |
1 |
Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values; |
|
312 |
|||
313 |
✓✗ | 1 |
getCovariance(ps, NULL, NULL, NULL, n, M); |
314 |
✓✗ | 1 |
eigen(M, s, E); |
315 |
|||
316 |
1 |
Matrix3f& axes = bv.obb.axes; |
|
317 |
✓✗ | 1 |
axisFromEigen(E, s, axes); |
318 |
|||
319 |
✓✗ | 1 |
getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent); |
320 |
|||
321 |
// get center and extension |
||
322 |
1 |
const Vec3f& center = bv.obb.To; |
|
323 |
1 |
const Vec3f& extent = bv.obb.extent; |
|
324 |
✓✗ | 1 |
FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); |
325 |
|||
326 |
// decide the k in kIOS |
||
327 |
✓✗✓✗ ✓✗ |
1 |
if (extent[0] > kIOS_RATIO * extent[2]) { |
328 |
✓✗✓✗ ✗✓ |
1 |
if (extent[0] > kIOS_RATIO * extent[1]) |
329 |
bv.num_spheres = 5; |
||
330 |
else |
||
331 |
1 |
bv.num_spheres = 3; |
|
332 |
} else |
||
333 |
bv.num_spheres = 1; |
||
334 |
|||
335 |
✓✗ | 1 |
bv.spheres[0].o = center; |
336 |
1 |
bv.spheres[0].r = r0; |
|
337 |
|||
338 |
✓✗ | 1 |
if (bv.num_spheres >= 3) { |
339 |
✓✗✓✗ |
1 |
FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; |
340 |
✓✗✓✗ ✓✗✓✗ |
1 |
Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); |
341 |
✓✗✓✗ |
1 |
bv.spheres[1].o = center - delta; |
342 |
✓✗✓✗ |
1 |
bv.spheres[2].o = center + delta; |
343 |
|||
344 |
1 |
FCL_REAL r11 = 0, r12 = 0; |
|
345 |
✓✗ | 1 |
r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o); |
346 |
✓✗ | 1 |
r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o); |
347 |
✓✗✓✗ ✓✗ |
1 |
bv.spheres[1].o += axes.col(2) * (-r10 + r11); |
348 |
✓✗✓✗ ✓✗ |
1 |
bv.spheres[2].o += axes.col(2) * (r10 - r12); |
349 |
|||
350 |
1 |
bv.spheres[1].r = r10; |
|
351 |
1 |
bv.spheres[2].r = r10; |
|
352 |
} |
||
353 |
|||
354 |
✗✓ | 1 |
if (bv.num_spheres >= 5) { |
355 |
FCL_REAL r10 = bv.spheres[1].r; |
||
356 |
Vec3f delta = |
||
357 |
axes.col(1) * |
||
358 |
(sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - |
||
359 |
extent[1]); |
||
360 |
bv.spheres[3].o = bv.spheres[0].o - delta; |
||
361 |
bv.spheres[4].o = bv.spheres[0].o + delta; |
||
362 |
|||
363 |
FCL_REAL r21 = 0, r22 = 0; |
||
364 |
r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o); |
||
365 |
r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o); |
||
366 |
|||
367 |
bv.spheres[3].o += axes.col(1) * (-r10 + r21); |
||
368 |
bv.spheres[4].o += axes.col(1) * (r10 - r22); |
||
369 |
|||
370 |
bv.spheres[3].r = r10; |
||
371 |
bv.spheres[4].r = r10; |
||
372 |
} |
||
373 |
1 |
} |
|
374 |
|||
375 |
} // namespace kIOS_fit_functions |
||
376 |
|||
377 |
namespace OBBRSS_fit_functions { |
||
378 |
void fit1(Vec3f* ps, OBBRSS& bv) { |
||
379 |
OBB_fit_functions::fit1(ps, bv.obb); |
||
380 |
RSS_fit_functions::fit1(ps, bv.rss); |
||
381 |
} |
||
382 |
|||
383 |
void fit2(Vec3f* ps, OBBRSS& bv) { |
||
384 |
OBB_fit_functions::fit2(ps, bv.obb); |
||
385 |
RSS_fit_functions::fit2(ps, bv.rss); |
||
386 |
} |
||
387 |
|||
388 |
6540 |
void fit3(Vec3f* ps, OBBRSS& bv) { |
|
389 |
6540 |
OBB_fit_functions::fit3(ps, bv.obb); |
|
390 |
6540 |
RSS_fit_functions::fit3(ps, bv.rss); |
|
391 |
6540 |
} |
|
392 |
|||
393 |
226 |
void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) { |
|
394 |
226 |
OBB_fit_functions::fitn(ps, n, bv.obb); |
|
395 |
226 |
RSS_fit_functions::fitn(ps, n, bv.rss); |
|
396 |
226 |
} |
|
397 |
|||
398 |
} // namespace OBBRSS_fit_functions |
||
399 |
|||
400 |
template <> |
||
401 |
6540 |
void fit(Vec3f* ps, unsigned int n, OBB& bv) { |
|
402 |
✗✗✓✗ ✗ |
6540 |
switch (n) { |
403 |
case 1: |
||
404 |
OBB_fit_functions::fit1(ps, bv); |
||
405 |
break; |
||
406 |
case 2: |
||
407 |
OBB_fit_functions::fit2(ps, bv); |
||
408 |
break; |
||
409 |
6540 |
case 3: |
|
410 |
6540 |
OBB_fit_functions::fit3(ps, bv); |
|
411 |
6540 |
break; |
|
412 |
case 6: |
||
413 |
OBB_fit_functions::fit6(ps, bv); |
||
414 |
break; |
||
415 |
default: |
||
416 |
OBB_fit_functions::fitn(ps, n, bv); |
||
417 |
} |
||
418 |
6540 |
} |
|
419 |
|||
420 |
template <> |
||
421 |
6541 |
void fit(Vec3f* ps, unsigned int n, RSS& bv) { |
|
422 |
✗✗✓✓ |
6541 |
switch (n) { |
423 |
case 1: |
||
424 |
RSS_fit_functions::fit1(ps, bv); |
||
425 |
break; |
||
426 |
case 2: |
||
427 |
RSS_fit_functions::fit2(ps, bv); |
||
428 |
break; |
||
429 |
6540 |
case 3: |
|
430 |
6540 |
RSS_fit_functions::fit3(ps, bv); |
|
431 |
6540 |
break; |
|
432 |
1 |
default: |
|
433 |
1 |
RSS_fit_functions::fitn(ps, n, bv); |
|
434 |
} |
||
435 |
6541 |
} |
|
436 |
|||
437 |
template <> |
||
438 |
6541 |
void fit(Vec3f* ps, unsigned int n, kIOS& bv) { |
|
439 |
✗✗✓✓ |
6541 |
switch (n) { |
440 |
case 1: |
||
441 |
kIOS_fit_functions::fit1(ps, bv); |
||
442 |
break; |
||
443 |
case 2: |
||
444 |
kIOS_fit_functions::fit2(ps, bv); |
||
445 |
break; |
||
446 |
6540 |
case 3: |
|
447 |
6540 |
kIOS_fit_functions::fit3(ps, bv); |
|
448 |
6540 |
break; |
|
449 |
1 |
default: |
|
450 |
1 |
kIOS_fit_functions::fitn(ps, n, bv); |
|
451 |
} |
||
452 |
6541 |
} |
|
453 |
|||
454 |
template <> |
||
455 |
6766 |
void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) { |
|
456 |
✗✗✓✓ |
6766 |
switch (n) { |
457 |
case 1: |
||
458 |
OBBRSS_fit_functions::fit1(ps, bv); |
||
459 |
break; |
||
460 |
case 2: |
||
461 |
OBBRSS_fit_functions::fit2(ps, bv); |
||
462 |
break; |
||
463 |
6540 |
case 3: |
|
464 |
6540 |
OBBRSS_fit_functions::fit3(ps, bv); |
|
465 |
6540 |
break; |
|
466 |
226 |
default: |
|
467 |
226 |
OBBRSS_fit_functions::fitn(ps, n, bv); |
|
468 |
} |
||
469 |
6766 |
} |
|
470 |
|||
471 |
template <> |
||
472 |
void fit(Vec3f* ps, unsigned int n, AABB& bv) { |
||
473 |
if (n <= 0) return; |
||
474 |
bv = AABB(ps[0]); |
||
475 |
for (unsigned int i = 1; i < n; ++i) { |
||
476 |
bv += ps[i]; |
||
477 |
} |
||
478 |
} |
||
479 |
|||
480 |
316227 |
OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, |
|
481 |
unsigned int num_primitives) { |
||
482 |
✓✗ | 316227 |
OBB bv; |
483 |
|||
484 |
✓✗ | 316227 |
Matrix3f M; // row first matrix |
485 |
✓✓✓✗ |
1264908 |
Vec3f E[3]; // row first eigen-vectors |
486 |
Matrix3f::Scalar s[3]; // three eigen values |
||
487 |
|||
488 |
✓✗ | 316227 |
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, |
489 |
num_primitives, M); |
||
490 |
✓✗ | 316227 |
eigen(M, s, E); |
491 |
|||
492 |
✓✗ | 316227 |
axisFromEigen(E, s, bv.axes); |
493 |
|||
494 |
// set obb centers and extensions |
||
495 |
316227 |
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, |
|
496 |
✓✗ | 316227 |
num_primitives, bv.axes, bv.To, bv.extent); |
497 |
|||
498 |
632454 |
return bv; |
|
499 |
} |
||
500 |
|||
501 |
1012716 |
OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, |
|
502 |
unsigned int num_primitives) { |
||
503 |
✓✗ | 1012716 |
OBBRSS bv; |
504 |
✓✗ | 1012716 |
Matrix3f M; |
505 |
✓✓✓✗ |
4050864 |
Vec3f E[3]; |
506 |
Matrix3f::Scalar s[3]; |
||
507 |
|||
508 |
✓✗ | 1012716 |
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, |
509 |
num_primitives, M); |
||
510 |
✓✗ | 1012716 |
eigen(M, s, E); |
511 |
|||
512 |
✓✗ | 1012716 |
axisFromEigen(E, s, bv.obb.axes); |
513 |
✓✗✓✗ |
1012716 |
bv.rss.axes.noalias() = bv.obb.axes; |
514 |
|||
515 |
1012716 |
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, |
|
516 |
✓✗ | 1012716 |
num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent); |
517 |
|||
518 |
✓✗ | 1012716 |
Vec3f origin; |
519 |
FCL_REAL l[2]; |
||
520 |
FCL_REAL r; |
||
521 |
1012716 |
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, |
|
522 |
primitive_indices, num_primitives, |
||
523 |
✓✗ | 1012716 |
bv.rss.axes, origin, l, r); |
524 |
|||
525 |
✓✗ | 1012716 |
bv.rss.Tr = origin; |
526 |
1012716 |
bv.rss.length[0] = l[0]; |
|
527 |
1012716 |
bv.rss.length[1] = l[1]; |
|
528 |
1012716 |
bv.rss.radius = r; |
|
529 |
|||
530 |
2025432 |
return bv; |
|
531 |
} |
||
532 |
|||
533 |
296669 |
RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, |
|
534 |
unsigned int num_primitives) { |
||
535 |
✓✗ | 296669 |
RSS bv; |
536 |
|||
537 |
✓✗ | 296669 |
Matrix3f M; // row first matrix |
538 |
✓✓✓✗ |
1186676 |
Vec3f E[3]; // row first eigen-vectors |
539 |
Matrix3f::Scalar s[3]; // three eigen values |
||
540 |
✓✗ | 296669 |
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, |
541 |
num_primitives, M); |
||
542 |
✓✗ | 296669 |
eigen(M, s, E); |
543 |
✓✗ | 296669 |
axisFromEigen(E, s, bv.axes); |
544 |
|||
545 |
// set rss origin, rectangle size and radius |
||
546 |
|||
547 |
✓✗ | 296669 |
Vec3f origin; |
548 |
FCL_REAL l[2]; |
||
549 |
FCL_REAL r; |
||
550 |
296669 |
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, |
|
551 |
✓✗ | 296669 |
primitive_indices, num_primitives, bv.axes, |
552 |
origin, l, r); |
||
553 |
|||
554 |
✓✗ | 296669 |
bv.Tr = origin; |
555 |
296669 |
bv.length[0] = l[0]; |
|
556 |
296669 |
bv.length[1] = l[1]; |
|
557 |
296669 |
bv.radius = r; |
|
558 |
|||
559 |
593338 |
return bv; |
|
560 |
} |
||
561 |
|||
562 |
138119 |
kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, |
|
563 |
unsigned int num_primitives) { |
||
564 |
✓✗ | 138119 |
kIOS bv; |
565 |
|||
566 |
✓✗ | 138119 |
Matrix3f M; // row first matrix |
567 |
✓✓✓✗ |
552476 |
Vec3f E[3]; // row first eigen-vectors |
568 |
Matrix3f::Scalar s[3]; |
||
569 |
|||
570 |
✓✗ | 138119 |
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, |
571 |
num_primitives, M); |
||
572 |
✓✗ | 138119 |
eigen(M, s, E); |
573 |
|||
574 |
138119 |
Matrix3f& axes = bv.obb.axes; |
|
575 |
✓✗ | 138119 |
axisFromEigen(E, s, axes); |
576 |
|||
577 |
// get centers and extensions |
||
578 |
138119 |
getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, |
|
579 |
✓✗ | 138119 |
num_primitives, axes, bv.obb.To, bv.obb.extent); |
580 |
|||
581 |
138119 |
const Vec3f& center = bv.obb.To; |
|
582 |
138119 |
const Vec3f& extent = bv.obb.extent; |
|
583 |
✓✗ | 138119 |
FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, |
584 |
primitive_indices, num_primitives, center); |
||
585 |
|||
586 |
// decide k in kIOS |
||
587 |
✓✗✓✗ ✓✓ |
138119 |
if (extent[0] > kIOS_RATIO * extent[2]) { |
588 |
✓✗✓✗ ✓✓ |
136587 |
if (extent[0] > kIOS_RATIO * extent[1]) |
589 |
128321 |
bv.num_spheres = 5; |
|
590 |
else |
||
591 |
8266 |
bv.num_spheres = 3; |
|
592 |
} else |
||
593 |
1532 |
bv.num_spheres = 1; |
|
594 |
|||
595 |
✓✗ | 138119 |
bv.spheres[0].o = center; |
596 |
138119 |
bv.spheres[0].r = r0; |
|
597 |
|||
598 |
✓✓ | 138119 |
if (bv.num_spheres >= 3) { |
599 |
✓✗✓✗ |
136587 |
FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; |
600 |
✓✗✓✗ ✓✗✓✗ |
136587 |
Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]); |
601 |
✓✗✓✗ |
136587 |
bv.spheres[1].o = center - delta; |
602 |
✓✗✓✗ |
136587 |
bv.spheres[2].o = center + delta; |
603 |
|||
604 |
FCL_REAL r11 = |
||
605 |
273174 |
maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, |
|
606 |
✓✗ | 136587 |
num_primitives, bv.spheres[1].o); |
607 |
FCL_REAL r12 = |
||
608 |
273174 |
maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, |
|
609 |
✓✗ | 136587 |
num_primitives, bv.spheres[2].o); |
610 |
|||
611 |
✓✗✓✗ ✓✗ |
136587 |
bv.spheres[1].o += axes.col(2) * (-r10 + r11); |
612 |
✓✗✓✗ ✓✗ |
136587 |
bv.spheres[2].o += axes.col(2) * (r10 - r12); |
613 |
|||
614 |
136587 |
bv.spheres[1].r = r10; |
|
615 |
136587 |
bv.spheres[2].r = r10; |
|
616 |
} |
||
617 |
|||
618 |
✓✓ | 138119 |
if (bv.num_spheres >= 5) { |
619 |
128321 |
FCL_REAL r10 = bv.spheres[1].r; |
|
620 |
Vec3f delta = |
||
621 |
✓✗ | 128321 |
axes.col(1) * |
622 |
✓✗✓✗ ✓✗✓✗ |
128321 |
(sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - |
623 |
✓✗✓✗ ✓✗ |
256642 |
extent[1]); |
624 |
✓✗✓✗ |
128321 |
bv.spheres[3].o = bv.spheres[0].o - delta; |
625 |
✓✗✓✗ |
128321 |
bv.spheres[4].o = bv.spheres[0].o + delta; |
626 |
|||
627 |
128321 |
FCL_REAL r21 = 0, r22 = 0; |
|
628 |
256642 |
r21 = maximumDistance(vertices, prev_vertices, tri_indices, |
|
629 |
✓✗ | 128321 |
primitive_indices, num_primitives, bv.spheres[3].o); |
630 |
256642 |
r22 = maximumDistance(vertices, prev_vertices, tri_indices, |
|
631 |
✓✗ | 128321 |
primitive_indices, num_primitives, bv.spheres[4].o); |
632 |
|||
633 |
✓✗✓✗ ✓✗ |
128321 |
bv.spheres[3].o += axes.col(1) * (-r10 + r21); |
634 |
✓✗✓✗ ✓✗ |
128321 |
bv.spheres[4].o += axes.col(1) * (r10 - r22); |
635 |
|||
636 |
128321 |
bv.spheres[3].r = r10; |
|
637 |
128321 |
bv.spheres[4].r = r10; |
|
638 |
} |
||
639 |
|||
640 |
276238 |
return bv; |
|
641 |
} |
||
642 |
|||
643 |
149968 |
AABB BVFitter<AABB>::fit(unsigned int* primitive_indices, |
|
644 |
unsigned int num_primitives) { |
||
645 |
149968 |
AABB bv; |
|
646 |
✗✓ | 149968 |
if (num_primitives == 0) return bv; |
647 |
|||
648 |
✓✓ | 149968 |
if (type == BVH_MODEL_TRIANGLES) /// The primitive is triangle |
649 |
{ |
||
650 |
149938 |
Triangle t0 = tri_indices[primitive_indices[0]]; |
|
651 |
✓✗✓✗ |
149938 |
bv = AABB(vertices[t0[0]]); |
652 |
|||
653 |
✓✓ | 1095765 |
for (unsigned int i = 0; i < num_primitives; ++i) { |
654 |
945827 |
Triangle t = tri_indices[primitive_indices[i]]; |
|
655 |
✓✗ | 945827 |
bv += vertices[t[0]]; |
656 |
✓✗ | 945827 |
bv += vertices[t[1]]; |
657 |
✓✗ | 945827 |
bv += vertices[t[2]]; |
658 |
|||
659 |
✗✓ | 945827 |
if (prev_vertices) /// can fitting both current and previous frame |
660 |
{ |
||
661 |
bv += prev_vertices[t[0]]; |
||
662 |
bv += prev_vertices[t[1]]; |
||
663 |
bv += prev_vertices[t[2]]; |
||
664 |
} |
||
665 |
} |
||
666 |
149938 |
return bv; |
|
667 |
✓✗ | 30 |
} else if (type == BVH_MODEL_POINTCLOUD) /// The primitive is point |
668 |
{ |
||
669 |
✓✗ | 30 |
bv = AABB(vertices[primitive_indices[0]]); |
670 |
✓✓ | 94 |
for (unsigned int i = 0; i < num_primitives; ++i) { |
671 |
64 |
bv += vertices[primitive_indices[i]]; |
|
672 |
|||
673 |
✗✓ | 64 |
if (prev_vertices) /// can fitting both current and previous frame |
674 |
{ |
||
675 |
bv += prev_vertices[primitive_indices[i]]; |
||
676 |
} |
||
677 |
} |
||
678 |
} |
||
679 |
30 |
return bv; |
|
680 |
} |
||
681 |
|||
682 |
} // namespace fcl |
||
683 |
|||
684 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |