BV_fitter.cpp
Go to the documentation of this file.
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 
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 static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3],
51  Matrix3f& axes) {
52  int min, mid, max;
53  if (eigenS[0] > eigenS[1]) {
54  max = 0;
55  min = 1;
56  } else {
57  min = 0;
58  max = 1;
59  }
60  if (eigenS[2] < eigenS[min]) {
61  mid = min;
62  min = 2;
63  } else if (eigenS[2] > eigenS[max]) {
64  mid = max;
65  max = 2;
66  } else {
67  mid = 2;
68  }
69 
70  axes.col(0) << eigenV[0][max], eigenV[1][max], eigenV[2][max];
71  axes.col(1) << eigenV[0][mid], eigenV[1][mid], eigenV[2][mid];
72  axes.col(2) << eigenV[1][max] * eigenV[2][mid] -
73  eigenV[1][mid] * eigenV[2][max],
74  eigenV[0][mid] * eigenV[2][max] - eigenV[0][max] * eigenV[2][mid],
75  eigenV[0][max] * eigenV[1][mid] - eigenV[0][mid] * eigenV[1][max];
76 }
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 void fit3(Vec3f* ps, OBB& bv) {
101  const Vec3f& p1 = ps[0];
102  const Vec3f& p2 = ps[1];
103  const Vec3f& p3 = ps[2];
104  Vec3f e[3];
105  e[0] = p1 - p2;
106  e[1] = p2 - p3;
107  e[2] = p3 - p1;
108  FCL_REAL len[3];
109  len[0] = e[0].squaredNorm();
110  len[1] = e[1].squaredNorm();
111  len[2] = e[2].squaredNorm();
112 
113  int imax = 0;
114  if (len[1] > len[0]) imax = 1;
115  if (len[2] > len[imax]) imax = 2;
116 
117  bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized();
118  bv.axes.col(0).noalias() = e[imax].normalized();
119  bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0));
120 
121  getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axes, bv.To, bv.extent);
122 }
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 void fitn(Vec3f* ps, unsigned int n, OBB& bv) {
132  Matrix3f M;
133  Vec3f E[3];
134  Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values
135 
136  getCovariance(ps, NULL, NULL, NULL, n, M);
137  eigen(M, s, E);
138  axisFromEigen(E, s, bv.axes);
139 
140  // set obb centers and extensions
141  getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axes, bv.To, bv.extent);
142 }
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 void fit3(Vec3f* ps, RSS& bv) {
171  const Vec3f& p1 = ps[0];
172  const Vec3f& p2 = ps[1];
173  const Vec3f& p3 = ps[2];
174  Vec3f e[3];
175  e[0] = p1 - p2;
176  e[1] = p2 - p3;
177  e[2] = p3 - p1;
178  FCL_REAL len[3];
179  len[0] = e[0].squaredNorm();
180  len[1] = e[1].squaredNorm();
181  len[2] = e[2].squaredNorm();
182 
183  int imax = 0;
184  if (len[1] > len[0]) imax = 1;
185  if (len[2] > len[imax]) imax = 2;
186 
187  bv.axes.col(2).noalias() = e[0].cross(e[1]).normalized();
188  bv.axes.col(0).noalias() = e[imax].normalized();
189  bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0));
190 
191  getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr,
192  bv.length, bv.radius);
193 }
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 void fitn(Vec3f* ps, unsigned int n, RSS& bv) {
203  Matrix3f M; // row first matrix
204  Vec3f E[3]; // row first eigen-vectors
205  Matrix3f::Scalar s[3] = {0, 0, 0};
206 
207  getCovariance(ps, NULL, NULL, NULL, n, M);
208  eigen(M, s, E);
209  axisFromEigen(E, s, bv.axes);
210 
211  // set rss origin, rectangle size and radius
212  getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr,
213  bv.length, bv.radius);
214 }
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 void fit3(Vec3f* ps, kIOS& bv) {
266  bv.num_spheres = 3;
267 
268  const Vec3f& p1 = ps[0];
269  const Vec3f& p2 = ps[1];
270  const Vec3f& p3 = ps[2];
271  Vec3f e[3];
272  e[0] = p1 - p2;
273  e[1] = p2 - p3;
274  e[2] = p3 - p1;
275  FCL_REAL len[3];
276  len[0] = e[0].squaredNorm();
277  len[1] = e[1].squaredNorm();
278  len[2] = e[2].squaredNorm();
279 
280  int imax = 0;
281  if (len[1] > len[0]) imax = 1;
282  if (len[2] > len[imax]) imax = 2;
283 
284  bv.obb.axes.col(2).noalias() = e[0].cross(e[1]).normalized();
285  bv.obb.axes.col(0).noalias() = e[imax].normalized();
286  bv.obb.axes.col(1).noalias() = bv.obb.axes.col(2).cross(bv.obb.axes.col(0));
287 
288  getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axes, bv.obb.To,
289  bv.obb.extent);
290 
291  // compute radius and center
292  FCL_REAL r0;
293  Vec3f center;
294  circumCircleComputation(p1, p2, p3, center, r0);
295 
296  bv.spheres[0].o = center;
297  bv.spheres[0].r = r0;
298 
299  FCL_REAL r1 = r0 * invSinA;
300  Vec3f delta = bv.obb.axes.col(2) * (r1 * cosA);
301 
302  bv.spheres[1].r = r1;
303  bv.spheres[1].o = center - delta;
304  bv.spheres[2].r = r1;
305  bv.spheres[2].o = center + delta;
306 }
307 
308 void fitn(Vec3f* ps, unsigned int n, kIOS& bv) {
309  Matrix3f M;
310  Vec3f E[3];
311  Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values;
312 
313  getCovariance(ps, NULL, NULL, NULL, n, M);
314  eigen(M, s, E);
315 
316  Matrix3f& axes = bv.obb.axes;
317  axisFromEigen(E, s, axes);
318 
319  getExtentAndCenter(ps, NULL, NULL, NULL, n, axes, bv.obb.To, bv.obb.extent);
320 
321  // get center and extension
322  const Vec3f& center = bv.obb.To;
323  const Vec3f& extent = bv.obb.extent;
324  FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
325 
326  // decide the k in kIOS
327  if (extent[0] > kIOS_RATIO * extent[2]) {
328  if (extent[0] > kIOS_RATIO * extent[1])
329  bv.num_spheres = 5;
330  else
331  bv.num_spheres = 3;
332  } else
333  bv.num_spheres = 1;
334 
335  bv.spheres[0].o = center;
336  bv.spheres[0].r = r0;
337 
338  if (bv.num_spheres >= 3) {
339  FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
340  Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
341  bv.spheres[1].o = center - delta;
342  bv.spheres[2].o = center + delta;
343 
344  FCL_REAL r11 = 0, r12 = 0;
345  r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o);
346  r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o);
347  bv.spheres[1].o += axes.col(2) * (-r10 + r11);
348  bv.spheres[2].o += axes.col(2) * (r10 - r12);
349 
350  bv.spheres[1].r = r10;
351  bv.spheres[2].r = r10;
352  }
353 
354  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 }
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 void fit3(Vec3f* ps, OBBRSS& bv) {
389  OBB_fit_functions::fit3(ps, bv.obb);
390  RSS_fit_functions::fit3(ps, bv.rss);
391 }
392 
393 void fitn(Vec3f* ps, unsigned int n, OBBRSS& bv) {
394  OBB_fit_functions::fitn(ps, n, bv.obb);
395  RSS_fit_functions::fitn(ps, n, bv.rss);
396 }
397 
398 } // namespace OBBRSS_fit_functions
399 
400 template <>
401 void fit(Vec3f* ps, unsigned int n, OBB& bv) {
402  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  case 3:
410  OBB_fit_functions::fit3(ps, bv);
411  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 }
419 
420 template <>
421 void fit(Vec3f* ps, unsigned int n, RSS& bv) {
422  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  case 3:
430  RSS_fit_functions::fit3(ps, bv);
431  break;
432  default:
433  RSS_fit_functions::fitn(ps, n, bv);
434  }
435 }
436 
437 template <>
438 void fit(Vec3f* ps, unsigned int n, kIOS& bv) {
439  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  case 3:
447  kIOS_fit_functions::fit3(ps, bv);
448  break;
449  default:
450  kIOS_fit_functions::fitn(ps, n, bv);
451  }
452 }
453 
454 template <>
455 void fit(Vec3f* ps, unsigned int n, OBBRSS& bv) {
456  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  case 3:
464  OBBRSS_fit_functions::fit3(ps, bv);
465  break;
466  default:
467  OBBRSS_fit_functions::fitn(ps, n, bv);
468  }
469 }
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 OBB BVFitter<OBB>::fit(unsigned int* primitive_indices,
481  unsigned int num_primitives) {
482  OBB bv;
483 
484  Matrix3f M; // row first matrix
485  Vec3f E[3]; // row first eigen-vectors
486  Matrix3f::Scalar s[3]; // three eigen values
487 
488  getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
489  num_primitives, M);
490  eigen(M, s, E);
491 
492  axisFromEigen(E, s, bv.axes);
493 
494  // set obb centers and extensions
495  getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices,
496  num_primitives, bv.axes, bv.To, bv.extent);
497 
498  return bv;
499 }
500 
501 OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices,
502  unsigned int num_primitives) {
503  OBBRSS bv;
504  Matrix3f M;
505  Vec3f E[3];
506  Matrix3f::Scalar s[3];
507 
508  getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
509  num_primitives, M);
510  eigen(M, s, E);
511 
512  axisFromEigen(E, s, bv.obb.axes);
513  bv.rss.axes.noalias() = bv.obb.axes;
514 
515  getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices,
516  num_primitives, bv.obb.axes, bv.obb.To, bv.obb.extent);
517 
518  Vec3f origin;
519  FCL_REAL l[2];
520  FCL_REAL r;
521  getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices,
522  primitive_indices, num_primitives,
523  bv.rss.axes, origin, l, r);
524 
525  bv.rss.Tr = origin;
526  bv.rss.length[0] = l[0];
527  bv.rss.length[1] = l[1];
528  bv.rss.radius = r;
529 
530  return bv;
531 }
532 
533 RSS BVFitter<RSS>::fit(unsigned int* primitive_indices,
534  unsigned int num_primitives) {
535  RSS bv;
536 
537  Matrix3f M; // row first matrix
538  Vec3f E[3]; // row first eigen-vectors
539  Matrix3f::Scalar s[3]; // three eigen values
540  getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
541  num_primitives, M);
542  eigen(M, s, E);
543  axisFromEigen(E, s, bv.axes);
544 
545  // set rss origin, rectangle size and radius
546 
547  Vec3f origin;
548  FCL_REAL l[2];
549  FCL_REAL r;
550  getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices,
551  primitive_indices, num_primitives, bv.axes,
552  origin, l, r);
553 
554  bv.Tr = origin;
555  bv.length[0] = l[0];
556  bv.length[1] = l[1];
557  bv.radius = r;
558 
559  return bv;
560 }
561 
562 kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices,
563  unsigned int num_primitives) {
564  kIOS bv;
565 
566  Matrix3f M; // row first matrix
567  Vec3f E[3]; // row first eigen-vectors
568  Matrix3f::Scalar s[3];
569 
570  getCovariance(vertices, prev_vertices, tri_indices, primitive_indices,
571  num_primitives, M);
572  eigen(M, s, E);
573 
574  Matrix3f& axes = bv.obb.axes;
575  axisFromEigen(E, s, axes);
576 
577  // get centers and extensions
578  getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices,
579  num_primitives, axes, bv.obb.To, bv.obb.extent);
580 
581  const Vec3f& center = bv.obb.To;
582  const Vec3f& extent = bv.obb.extent;
583  FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices,
584  primitive_indices, num_primitives, center);
585 
586  // decide k in kIOS
587  if (extent[0] > kIOS_RATIO * extent[2]) {
588  if (extent[0] > kIOS_RATIO * extent[1])
589  bv.num_spheres = 5;
590  else
591  bv.num_spheres = 3;
592  } else
593  bv.num_spheres = 1;
594 
595  bv.spheres[0].o = center;
596  bv.spheres[0].r = r0;
597 
598  if (bv.num_spheres >= 3) {
599  FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
600  Vec3f delta = axes.col(2) * (r10 * cosA - extent[2]);
601  bv.spheres[1].o = center - delta;
602  bv.spheres[2].o = center + delta;
603 
604  FCL_REAL r11 =
605  maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices,
606  num_primitives, bv.spheres[1].o);
607  FCL_REAL r12 =
608  maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices,
609  num_primitives, bv.spheres[2].o);
610 
611  bv.spheres[1].o += axes.col(2) * (-r10 + r11);
612  bv.spheres[2].o += axes.col(2) * (r10 - r12);
613 
614  bv.spheres[1].r = r10;
615  bv.spheres[2].r = r10;
616  }
617 
618  if (bv.num_spheres >= 5) {
619  FCL_REAL r10 = bv.spheres[1].r;
620  Vec3f delta =
621  axes.col(1) *
622  (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) -
623  extent[1]);
624  bv.spheres[3].o = bv.spheres[0].o - delta;
625  bv.spheres[4].o = bv.spheres[0].o + delta;
626 
627  FCL_REAL r21 = 0, r22 = 0;
628  r21 = maximumDistance(vertices, prev_vertices, tri_indices,
629  primitive_indices, num_primitives, bv.spheres[3].o);
630  r22 = maximumDistance(vertices, prev_vertices, tri_indices,
631  primitive_indices, num_primitives, bv.spheres[4].o);
632 
633  bv.spheres[3].o += axes.col(1) * (-r10 + r21);
634  bv.spheres[4].o += axes.col(1) * (r10 - r22);
635 
636  bv.spheres[3].r = r10;
637  bv.spheres[4].r = r10;
638  }
639 
640  return bv;
641 }
642 
643 AABB BVFitter<AABB>::fit(unsigned int* primitive_indices,
644  unsigned int num_primitives) {
645  AABB bv;
646  if (num_primitives == 0) return bv;
647 
648  if (type == BVH_MODEL_TRIANGLES)
649  {
650  Triangle t0 = tri_indices[primitive_indices[0]];
651  bv = AABB(vertices[t0[0]]);
652 
653  for (unsigned int i = 0; i < num_primitives; ++i) {
654  Triangle t = tri_indices[primitive_indices[i]];
655  bv += vertices[t[0]];
656  bv += vertices[t[1]];
657  bv += vertices[t[2]];
658 
659  if (prev_vertices)
660  {
661  bv += prev_vertices[t[0]];
662  bv += prev_vertices[t[1]];
663  bv += prev_vertices[t[2]];
664  }
665  }
666  return bv;
667  } else if (type == BVH_MODEL_POINTCLOUD)
668  {
669  bv = AABB(vertices[primitive_indices[0]]);
670  for (unsigned int i = 0; i < num_primitives; ++i) {
671  bv += vertices[primitive_indices[i]];
672 
673  if (prev_vertices)
674  {
675  bv += prev_vertices[primitive_indices[i]];
676  }
677  }
678  }
679  return bv;
680 }
681 
682 } // namespace fcl
683 
684 } // namespace hpp
hpp::fcl::OBB::axes
Matrix3f axes
Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i...
Definition: include/hpp/fcl/BV/OBB.h:58
hpp::fcl::OBB::extent
Vec3f extent
Half dimensions of OBB.
Definition: include/hpp/fcl/BV/OBB.h:64
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::maximumDistance
HPP_FCL_DLLAPI FCL_REAL maximumDistance(Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, unsigned int n, const Vec3f &query)
Compute the maximum distance from a given center point to a point cloud.
Definition: BVH_utility.cpp:644
doxygen_xml_parser.type
type
Definition: doxygen_xml_parser.py:882
hpp::fcl::kIOS::kIOS_Sphere::o
Vec3f o
Definition: kIOS.h:56
hpp::fcl::OBB_fit_functions::fit3
void fit3(Vec3f *ps, OBB &bv)
Definition: BV_fitter.cpp:100
hpp::fcl::RSS::axes
Matrix3f axes
Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i...
Definition: BV/RSS.h:59
hpp::fcl::OBBRSS_fit_functions::fit1
void fit1(Vec3f *ps, OBBRSS &bv)
Definition: BV_fitter.cpp:378
hpp::fcl::axisFromEigen
static void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], Matrix3f &axes)
Definition: BV_fitter.cpp:50
hpp::fcl::OBBRSS_fit_functions::fit3
void fit3(Vec3f *ps, OBBRSS &bv)
Definition: BV_fitter.cpp:388
hpp::fcl::OBBRSS::obb
OBB obb
OBB member, for rotation.
Definition: BV/OBBRSS.h:56
hpp::fcl::kIOS_RATIO
static const double kIOS_RATIO
Definition: BV_fitter.cpp:46
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
hpp::fcl::kIOS_fit_functions::fitn
void fitn(Vec3f *ps, unsigned int n, kIOS &bv)
Definition: BV_fitter.cpp:308
hpp::fcl::OBBRSS_fit_functions::fitn
void fitn(Vec3f *ps, unsigned int n, OBBRSS &bv)
Definition: BV_fitter.cpp:393
hpp::fcl::getCovariance
HPP_FCL_DLLAPI void getCovariance(Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, unsigned int n, Matrix3f &M)
Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to poin...
Definition: BVH_utility.cpp:168
hpp::fcl::eigen
void eigen(const Eigen::MatrixBase< Derived > &m, typename Derived::Scalar dout[3], Vector *vout)
compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen ve...
Definition: tools.h:104
octree.r
r
Definition: octree.py:9
hpp::fcl::fit
void fit(Vec3f *ps, unsigned int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: BV_fitter.h:52
hpp::fcl::kIOS_fit_functions::fit2
void fit2(Vec3f *ps, kIOS &bv)
Definition: BV_fitter.cpp:230
hpp::fcl::RSS::Tr
Vec3f Tr
Origin of the rectangle in RSS.
Definition: BV/RSS.h:62
hpp::fcl::OBB_fit_functions::fit1
void fit1(Vec3f *ps, OBB &bv)
Definition: BV_fitter.cpp:80
hpp::fcl::kIOS_fit_functions::fit1
void fit1(Vec3f *ps, kIOS &bv)
Definition: BV_fitter.cpp:220
hpp::fcl::kIOS::kIOS_Sphere::r
FCL_REAL r
Definition: kIOS.h:57
hpp::fcl::getExtentAndCenter
HPP_FCL_DLLAPI void getExtentAndCenter(Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, unsigned int n, Matrix3f &axes, Vec3f &center, Vec3f &extent)
Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
Definition: BVH_utility.cpp:562
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::RSS_fit_functions::fitn
void fitn(Vec3f *ps, unsigned int n, RSS &bv)
Definition: BV_fitter.cpp:202
hpp::fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:82
hpp::fcl::kIOS::obb
OBB obb
@ OBB related with kIOS
Definition: kIOS.h:117
hpp::fcl::getRadiusAndOriginAndRectangleSize
HPP_FCL_DLLAPI void getRadiusAndOriginAndRectangleSize(Vec3f *ps, Vec3f *ps2, Triangle *ts, unsigned int *indices, unsigned int n, const Matrix3f &axes, Vec3f &origin, FCL_REAL l[2], FCL_REAL &r)
Compute the RSS bounding volume parameters: radius, rectangle size and the origin,...
Definition: BVH_utility.cpp:249
hpp::fcl::OBB_fit_functions::fit6
void fit6(Vec3f *ps, OBB &bv)
Definition: BV_fitter.cpp:124
hpp::fcl::cosA
static const double cosA
Definition: BV_fitter.cpp:48
hpp::fcl::RSS_fit_functions::fit3
void fit3(Vec3f *ps, RSS &bv)
Definition: BV_fitter.cpp:170
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::OBB
Oriented bounding box class.
Definition: include/hpp/fcl/BV/OBB.h:52
hpp::fcl::RSS_fit_functions::fit6
void fit6(Vec3f *ps, RSS &bv)
Definition: BV_fitter.cpp:195
t
tuple t
M
M
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
hpp::fcl::circumCircleComputation
HPP_FCL_DLLAPI void circumCircleComputation(const Vec3f &a, const Vec3f &b, const Vec3f &c, Vec3f &center, FCL_REAL &radius)
Compute the center and radius for a triangle's circumcircle.
Definition: BVH_utility.cpp:571
octree.p1
tuple p1
Definition: octree.py:54
hpp::fcl::OBB::To
Vec3f To
Center of OBB.
Definition: include/hpp/fcl/BV/OBB.h:61
hpp::fcl::BVFitter::fit
BV fit(unsigned int *primitive_indices, unsigned int num_primitives)
Compute a bounding volume that fits a set of primitives (points or triangles). The primitive data was...
Definition: BV_fitter.h:129
hpp::fcl::kIOS::num_spheres
unsigned int num_spheres
The number of spheres, no larger than 5.
Definition: kIOS.h:114
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
hpp::fcl::RSS_fit_functions::fit1
void fit1(Vec3f *ps, RSS &bv)
Definition: BV_fitter.cpp:147
hpp::fcl::RSS_fit_functions::fit2
void fit2(Vec3f *ps, RSS &bv)
Definition: BV_fitter.cpp:155
hpp::fcl::kIOS_fit_functions::fit3
void fit3(Vec3f *ps, kIOS &bv)
Definition: BV_fitter.cpp:265
hpp::fcl::OBB_fit_functions::fitn
void fitn(Vec3f *ps, unsigned int n, OBB &bv)
Definition: BV_fitter.cpp:131
hpp::fcl::OBB_fit_functions::fit2
void fit2(Vec3f *ps, OBB &bv)
Definition: BV_fitter.cpp:86
hpp::fcl::OBBRSS_fit_functions::fit2
void fit2(Vec3f *ps, OBBRSS &bv)
Definition: BV_fitter.cpp:383
hpp::fcl::OBBRSS::rss
RSS rss
RSS member, for distance.
Definition: BV/OBBRSS.h:59
hpp::fcl::invSinA
static const double invSinA
Definition: BV_fitter.cpp:47
tools.h
hpp::fcl::Triangle
Triangle with 3 indices for points.
Definition: data_types.h:96
BVH_utility.h
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
hpp::fcl::RSS::length
FCL_REAL length[2]
Side lengths of rectangle.
Definition: BV/RSS.h:65
hpp::fcl::BVH_MODEL_POINTCLOUD
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:83
hpp::fcl::RSS::radius
FCL_REAL radius
Radius of sphere summed with rectangle to form RSS.
Definition: BV/RSS.h:68
BV_fitter.h
hpp::fcl::kIOS::spheres
kIOS_Sphere spheres[5]
The (at most) five spheres for intersection.
Definition: kIOS.h:111
hpp::fcl::generateCoordinateSystem
void generateCoordinateSystem(const Eigen::MatrixBase< Derived1 > &_w, const Eigen::MatrixBase< Derived2 > &_u, const Eigen::MatrixBase< Derived3 > &_v)
Definition: tools.h:60

hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:13