geometry-inl.h
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-2016, 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 #ifndef FCL_BV_DETAIL_UTILITY_INL_H
39 #define FCL_BV_DETAIL_UTILITY_INL_H
40 
41 #include "fcl/math/geometry.h"
42 #include "fcl/math/constants.h"
43 
44 namespace fcl {
45 
46 //==============================================================================
47 extern template
48 void normalize(Vector3d& v, bool* signal);
49 
50 //==============================================================================
51 extern template
52 void hat(Matrix3d& mat, const Vector3d& vec);
53 
54 //==============================================================================
55 extern template
56 void eigen(const Matrix3d& m, Vector3d& dout, Matrix3d& vout);
57 
58 //==============================================================================
59 extern template
60 void eigen_old(const Matrix3d& m, Vector3d& dout, Matrix3d& vout);
61 
62 //==============================================================================
63 extern template
64 void axisFromEigen(
65  const Matrix3d& eigenV, const Vector3d& eigenS, Matrix3d& axis);
66 
67 //==============================================================================
68 extern template
69 void axisFromEigen(const Matrix3d& eigenV,
70  const Vector3d& eigenS,
71  Transform3d& tf);
72 
73 //==============================================================================
74 extern template
75 Matrix3d generateCoordinateSystem(const Vector3d& x_axis);
76 
77 //==============================================================================
78 extern template
79 void getRadiusAndOriginAndRectangleSize(
80  const Vector3d* const ps,
81  const Vector3d* const ps2,
82  Triangle* ts,
83  unsigned int* indices,
84  int n,
85  const Matrix3d& axis,
86  Vector3d& origin,
87  double l[2],
88  double& r);
89 
90 //==============================================================================
91 extern template
92 void getRadiusAndOriginAndRectangleSize(
93  const Vector3d* const ps,
94  const Vector3d* const ps2,
95  Triangle* ts,
96  unsigned int* indices,
97  int n,
98  Transform3d& tf,
99  double l[2],
100  double& r);
101 
102 //==============================================================================
103 extern template
104 void circumCircleComputation(
105  const Vector3d& a,
106  const Vector3d& b,
107  const Vector3d& c,
108  Vector3d& center,
109  double& radius);
110 
111 //==============================================================================
112 extern template
113 double maximumDistance(
114  const Vector3d* const ps,
115  const Vector3d* const ps2,
116  Triangle* ts,
117  unsigned int* indices,
118  int n,
119  const Vector3d& query);
120 
121 //==============================================================================
122 extern template
123 void getExtentAndCenter(
124  const Vector3d* const ps,
125  const Vector3d* const ps2,
126  Triangle* ts,
127  unsigned int* indices,
128  int n,
129  const Matrix3d& axis,
130  Vector3d& center,
131  Vector3d& extent);
132 
133 //==============================================================================
134 extern template
135 void getCovariance(
136  const Vector3d* const ps,
137  const Vector3d* const ps2,
138  Triangle* ts,
139  unsigned int* indices,
140  int n, Matrix3d& M);
141 
142 //==============================================================================
143 namespace detail {
144 //==============================================================================
145 
146 //==============================================================================
147 template <typename S>
148 FCL_EXPORT
149 S maximumDistance_mesh(
150  const Vector3<S>* const ps,
151  const Vector3<S>* const ps2,
152  Triangle* ts,
153  unsigned int* indices,
154  int n,
155  const Vector3<S>& query)
156 {
157  bool indirect_index = true;
158  if(!indices) indirect_index = false;
159 
160  S maxD = 0;
161  for(int i = 0; i < n; ++i)
162  {
163  unsigned int index = indirect_index ? indices[i] : i;
164  const Triangle& t = ts[index];
165 
166  for(int j = 0; j < 3; ++j)
167  {
168  int point_id = t[j];
169  const Vector3<S>& p = ps[point_id];
170 
171  S d = (p - query).squaredNorm();
172  if(d > maxD) maxD = d;
173  }
174 
175  if(ps2)
176  {
177  for(int j = 0; j < 3; ++j)
178  {
179  int point_id = t[j];
180  const Vector3<S>& p = ps2[point_id];
181 
182  S d = (p - query).squaredNorm();
183  if(d > maxD) maxD = d;
184  }
185  }
186  }
187 
188  return std::sqrt(maxD);
189 }
190 
191 //==============================================================================
192 template <typename S>
193 FCL_EXPORT
194 S maximumDistance_pointcloud(
195  const Vector3<S>* const ps,
196  const Vector3<S>* const ps2,
197  unsigned int* indices,
198  int n,
199  const Vector3<S>& query)
200 {
201  bool indirect_index = true;
202  if(!indices) indirect_index = false;
203 
204  S maxD = 0;
205  for(int i = 0; i < n; ++i)
206  {
207  int index = indirect_index ? indices[i] : i;
208 
209  const Vector3<S>& p = ps[index];
210  S d = (p - query).squaredNorm();
211  if(d > maxD) maxD = d;
212 
213  if(ps2)
214  {
215  const Vector3<S>& v = ps2[index];
216  S d = (v - query).squaredNorm();
217  if(d > maxD) maxD = d;
218  }
219  }
220 
221  return std::sqrt(maxD);
222 }
223 
224 //==============================================================================
227 template <typename S>
228 FCL_EXPORT
229 void getExtentAndCenter_pointcloud(
230  const Vector3<S>* const ps,
231  const Vector3<S>* const ps2,
232  unsigned int* indices,
233  int n,
234  const Matrix3<S>& axis,
235  Vector3<S>& center,
236  Vector3<S>& extent)
237 {
238  bool indirect_index = true;
239  if(!indices) indirect_index = false;
240 
241  auto real_max = std::numeric_limits<S>::max();
242 
243  Vector3<S> min_coord = Vector3<S>::Constant(real_max);
244  Vector3<S> max_coord = Vector3<S>::Constant(-real_max);
245 
246  for(int i = 0; i < n; ++i)
247  {
248  int index = indirect_index ? indices[i] : i;
249 
250  const Vector3<S>& p = ps[index];
251  Vector3<S> proj(
252  axis.col(0).dot(p),
253  axis.col(1).dot(p),
254  axis.col(2).dot(p));
255 
256  for(int j = 0; j < 3; ++j)
257  {
258  if(proj[j] > max_coord[j])
259  max_coord[j] = proj[j];
260 
261  if(proj[j] < min_coord[j])
262  min_coord[j] = proj[j];
263  }
264 
265  if(ps2)
266  {
267  const Vector3<S>& v = ps2[index];
268  Vector3<S> proj(
269  axis.col(0).dot(v),
270  axis.col(1).dot(v),
271  axis.col(2).dot(v));
272 
273  for(int j = 0; j < 3; ++j)
274  {
275  if(proj[j] > max_coord[j])
276  max_coord[j] = proj[j];
277 
278  if(proj[j] < min_coord[j])
279  min_coord[j] = proj[j];
280  }
281  }
282  }
283 
284  const Vector3<S> o = (max_coord + min_coord) / 2;
285  center.noalias() = axis * o;
286  extent.noalias() = (max_coord - min_coord) * 0.5;
287 }
288 
289 //==============================================================================
292 template <typename S>
293 FCL_EXPORT
294 void getExtentAndCenter_mesh(
295  const Vector3<S>* const ps,
296  const Vector3<S>* const ps2,
297  Triangle* ts,
298  unsigned int* indices,
299  int n,
300  const Matrix3<S>& axis,
301  Vector3<S>& center,
302  Vector3<S>& extent)
303 {
304  bool indirect_index = true;
305  if(!indices) indirect_index = false;
306 
307  auto real_max = std::numeric_limits<S>::max();
308 
309  Vector3<S> min_coord = Vector3<S>::Constant(real_max);
310  Vector3<S> max_coord = Vector3<S>::Constant(-real_max);
311 
312  for(int i = 0; i < n; ++i)
313  {
314  unsigned int index = indirect_index? indices[i] : i;
315  const Triangle& t = ts[index];
316 
317  for(int j = 0; j < 3; ++j)
318  {
319  int point_id = t[j];
320  const Vector3<S>& p = ps[point_id];
321  Vector3<S> proj(
322  axis.col(0).dot(p),
323  axis.col(1).dot(p),
324  axis.col(2).dot(p));
325 
326  for(int k = 0; k < 3; ++k)
327  {
328  if(proj[k] > max_coord[k])
329  max_coord[k] = proj[k];
330 
331  if(proj[k] < min_coord[k])
332  min_coord[k] = proj[k];
333  }
334  }
335 
336  if(ps2)
337  {
338  for(int j = 0; j < 3; ++j)
339  {
340  int point_id = t[j];
341  const Vector3<S>& p = ps2[point_id];
342  Vector3<S> proj(
343  axis.col(0).dot(p),
344  axis.col(1).dot(p),
345  axis.col(2).dot(p));
346 
347  for(int k = 0; k < 3; ++k)
348  {
349  if(proj[k] > max_coord[k])
350  max_coord[k] = proj[k];
351 
352  if(proj[k] < min_coord[k])
353  min_coord[k] = proj[k];
354  }
355  }
356  }
357  }
358 
359  const Vector3<S> o = (max_coord + min_coord) / 2;
360  center.noalias() = axis * o;
361  extent.noalias() = (max_coord - min_coord) / 2;
362 }
363 
364 //==============================================================================
365 extern template
366 double maximumDistance_mesh(
367  const Vector3d* const ps,
368  const Vector3d* const ps2,
369  Triangle* ts,
370  unsigned int* indices,
371  int n,
372  const Vector3d& query);
373 
374 //==============================================================================
375 extern template
376 double maximumDistance_pointcloud(
377  const Vector3d* const ps,
378  const Vector3d* const ps2,
379  unsigned int* indices,
380  int n,
381  const Vector3d& query);
382 
383 //==============================================================================
384 extern template
385 void getExtentAndCenter_pointcloud(
386  const Vector3d* const ps,
387  const Vector3d* const ps2,
388  unsigned int* indices,
389  int n,
390  const Matrix3d& axis,
391  Vector3d& center,
392  Vector3d& extent);
393 
394 //==============================================================================
395 extern template
396 void getExtentAndCenter_mesh(
397  const Vector3d* const ps,
398  const Vector3d* const ps2,
399  Triangle* ts,
400  unsigned int* indices,
401  int n,
402  const Matrix3d& axis,
403  Vector3d& center,
404  Vector3d& extent);
405 
406 //==============================================================================
407 } // namespace detail
408 //==============================================================================
409 
410 //==============================================================================
411 template <typename S>
412 FCL_EXPORT
413 void normalize(Vector3<S>& v, bool* signal)
414 {
415  S sqr_length = v.squaredNorm();
416 
417  if (sqr_length > 0)
418  {
419  v /= std::sqrt(sqr_length);
420  *signal = true;
421  }
422  else
423  {
424  *signal = false;
425  }
426 }
427 
428 //==============================================================================
429 template <typename Derived>
430 FCL_EXPORT
431 typename Derived::RealScalar triple(const Eigen::MatrixBase<Derived>& x,
432  const Eigen::MatrixBase<Derived>& y,
433  const Eigen::MatrixBase<Derived>& z)
434 {
435  return x.dot(y.cross(z));
436 }
437 
438 //==============================================================================
439 template <typename S, int M, int N>
440 FCL_EXPORT
441 VectorN<S, M+N> combine(
442  const VectorN<S, M>& v1, const VectorN<S, N>& v2)
443 {
444  VectorN<S, M+N> v;
445  v << v1, v2;
446 
447  return v;
448 }
449 
450 //==============================================================================
451 template <typename S>
452 FCL_EXPORT
453 void hat(Matrix3<S>& mat, const Vector3<S>& vec)
454 {
455  mat << 0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0;
456 }
457 
458 //==============================================================================
459 template<typename S>
460 FCL_EXPORT
461 void eigen(const Matrix3<S>& m, Vector3<S>& dout, Matrix3<S>& vout)
462 {
463  // We assume that m is symmetric matrix.
464  Eigen::SelfAdjointEigenSolver<Matrix3<S>> eigensolver(m);
465  if (eigensolver.info() != Eigen::Success)
466  {
467  std::cerr << "[eigen] Failed to compute eigendecomposition.\n";
468  return;
469  }
470  dout = eigensolver.eigenvalues();
471  vout = eigensolver.eigenvectors();
472 }
473 
474 //==============================================================================
475 template<typename S>
476 FCL_EXPORT
477 void eigen_old(const Matrix3<S>& m, Vector3<S>& dout, Matrix3<S>& vout)
478 {
479  Matrix3<S> R(m);
480  int n = 3;
481  int j, iq, ip, i;
482  S tresh, theta, tau, t, sm, s, h, g, c;
483  int nrot;
484  S b[3];
485  S z[3];
486  S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
487  S d[3];
488 
489  for(ip = 0; ip < n; ++ip)
490  {
491  b[ip] = d[ip] = R(ip, ip);
492  z[ip] = 0;
493  }
494 
495  nrot = 0;
496 
497  for(i = 0; i < 50; ++i)
498  {
499  sm = 0;
500  for(ip = 0; ip < n; ++ip)
501  for(iq = ip + 1; iq < n; ++iq)
502  sm += std::abs(R(ip, iq));
503  if(sm == 0.0)
504  {
505  vout.col(0) << v[0][0], v[0][1], v[0][2];
506  vout.col(1) << v[1][0], v[1][1], v[1][2];
507  vout.col(2) << v[2][0], v[2][1], v[2][2];
508  dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
509  return;
510  }
511 
512  if(i < 3) tresh = 0.2 * sm / (n * n);
513  else tresh = 0.0;
514 
515  for(ip = 0; ip < n; ++ip)
516  {
517  for(iq= ip + 1; iq < n; ++iq)
518  {
519  g = 100.0 * std::abs(R(ip, iq));
520  if(i > 3 &&
521  std::abs(d[ip]) + g == std::abs(d[ip]) &&
522  std::abs(d[iq]) + g == std::abs(d[iq]))
523  R(ip, iq) = 0.0;
524  else if(std::abs(R(ip, iq)) > tresh)
525  {
526  h = d[iq] - d[ip];
527  if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
528  else
529  {
530  theta = 0.5 * h / (R(ip, iq));
531  t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
532  if(theta < 0.0) t = -t;
533  }
534  c = 1.0 / std::sqrt(1 + t * t);
535  s = t * c;
536  tau = s / (1.0 + c);
537  h = t * R(ip, iq);
538  z[ip] -= h;
539  z[iq] += h;
540  d[ip] -= h;
541  d[iq] += h;
542  R(ip, iq) = 0.0;
543  for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
544  for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
545  for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
546  for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
547  nrot++;
548  }
549  }
550  }
551  for(ip = 0; ip < n; ++ip)
552  {
553  b[ip] += z[ip];
554  d[ip] = b[ip];
555  z[ip] = 0.0;
556  }
557  }
558 
559  std::cerr << "eigen: too many iterations in Jacobi transform.\n";
560 
561  return;
562 }
563 
564 //==============================================================================
565 template <typename S>
566 FCL_EXPORT
567 void axisFromEigen(const Matrix3<S>& eigenV,
568  const Vector3<S>& eigenS,
569  Matrix3<S>& axis)
570 {
571  int min, mid, max;
572 
573  if(eigenS[0] > eigenS[1])
574  {
575  max = 0;
576  min = 1;
577  }
578  else
579  {
580  min = 0;
581  max = 1;
582  }
583 
584  if(eigenS[2] < eigenS[min])
585  {
586  mid = min;
587  min = 2;
588  }
589  else if(eigenS[2] > eigenS[max])
590  {
591  mid = max;
592  max = 2;
593  }
594  else
595  {
596  mid = 2;
597  }
598 
599  axis.col(0) = eigenV.row(max);
600  axis.col(1) = eigenV.row(mid);
601  axis.col(2).noalias() = axis.col(0).cross(axis.col(1));
602 }
603 
604 //==============================================================================
605 template <typename S>
606 FCL_EXPORT
607 void axisFromEigen(const Matrix3<S>& eigenV,
608  const Vector3<S>& eigenS,
609  Transform3<S>& tf)
610 {
611  int min, mid, max;
612 
613  if(eigenS[0] > eigenS[1])
614  {
615  max = 0;
616  min = 1;
617  }
618  else
619  {
620  min = 0;
621  max = 1;
622  }
623 
624  if(eigenS[2] < eigenS[min])
625  {
626  mid = min;
627  min = 2;
628  }
629  else if(eigenS[2] > eigenS[max])
630  {
631  mid = max;
632  max = 2;
633  }
634  else
635  {
636  mid = 2;
637  }
638 
639  tf.linear().col(0) = eigenV.col(max);
640  tf.linear().col(1) = eigenV.col(mid);
641  tf.linear().col(2).noalias() = tf.linear().col(0).cross(tf.linear().col(1));
642 }
643 
644 //==============================================================================
645 template <typename S>
646 FCL_EXPORT
647 Matrix3<S> generateCoordinateSystem(const Vector3<S>& x_axis)
648 {
649  Matrix3<S> axis;
650  axis.col(0).noalias() = x_axis.normalized();
651  axis.col(1).noalias() = axis.col(0).unitOrthogonal();
652  axis.col(2).noalias() = axis.col(0).cross(axis.col(1)).normalized();
653  return axis;
654 }
655 
656 //==============================================================================
657 template <typename DerivedA, typename DerivedB, typename DerivedC, typename DerivedD>
658 FCL_EXPORT
659 void relativeTransform(
660  const Eigen::MatrixBase<DerivedA>& R1, const Eigen::MatrixBase<DerivedB>& t1,
661  const Eigen::MatrixBase<DerivedA>& R2, const Eigen::MatrixBase<DerivedB>& t2,
662  Eigen::MatrixBase<DerivedC>& R, Eigen::MatrixBase<DerivedD>& t)
663 {
664  EIGEN_STATIC_ASSERT(
665  DerivedA::RowsAtCompileTime == 3
666  && DerivedA::ColsAtCompileTime == 3,
667  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
668 
669  EIGEN_STATIC_ASSERT(
670  DerivedB::RowsAtCompileTime == 3
671  && DerivedB::ColsAtCompileTime == 1,
672  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
673 
674  EIGEN_STATIC_ASSERT(
675  DerivedC::RowsAtCompileTime == 3
676  && DerivedC::ColsAtCompileTime == 3,
677  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
678 
679  EIGEN_STATIC_ASSERT(
680  DerivedD::RowsAtCompileTime == 3
681  && DerivedD::ColsAtCompileTime == 1,
682  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
683 
684  R.noalias() = R1.transpose() * R2;
685  t.noalias() = R1.transpose() * (t2 - t1);
686 }
687 
688 //==============================================================================
689 template <typename S, typename DerivedA, typename DerivedB>
690 FCL_EXPORT
691 void relativeTransform(
692  const Transform3<S>& T1,
693  const Transform3<S>& T2,
694  Eigen::MatrixBase<DerivedA>& R, Eigen::MatrixBase<DerivedB>& t)
695 {
696  EIGEN_STATIC_ASSERT(
697  DerivedA::RowsAtCompileTime == 3
698  && DerivedA::ColsAtCompileTime == 3,
699  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
700 
701  EIGEN_STATIC_ASSERT(
702  DerivedB::RowsAtCompileTime == 3
703  && DerivedB::ColsAtCompileTime == 1,
704  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
705 
706  R.noalias() = T1.linear().transpose() * T2.linear();
707  t.noalias() = T1.linear().transpose() * (T2.translation() - T1.translation());
708 }
709 
710 //==============================================================================
711 template <typename S>
712 FCL_EXPORT
713 void getRadiusAndOriginAndRectangleSize(
714  const Vector3<S>* const ps,
715  const Vector3<S>* const ps2,
716  Triangle* ts,
717  unsigned int* indices,
718  int n,
719  const Matrix3<S>& axis,
720  Vector3<S>& origin,
721  S l[2],
722  S& r)
723 {
724  bool indirect_index = true;
725  if(!indices) indirect_index = false;
726 
727  int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
728 
729  std::vector<Vector3<S>> P(size_P);
730 
731  int P_id = 0;
732 
733  if(ts)
734  {
735  for(int i = 0; i < n; ++i)
736  {
737  int index = indirect_index ? indices[i] : i;
738  const Triangle& t = ts[index];
739 
740  for(int j = 0; j < 3; ++j)
741  {
742  int point_id = t[j];
743  const Vector3<S>& p = ps[point_id];
744  P[P_id][0] = axis.col(0).dot(p);
745  P[P_id][1] = axis.col(1).dot(p);
746  P[P_id][2] = axis.col(2).dot(p);
747  P_id++;
748  }
749 
750  if(ps2)
751  {
752  for(int j = 0; j < 3; ++j)
753  {
754  int point_id = t[j];
755  const Vector3<S>& p = ps2[point_id];
756  P[P_id][0] = axis.col(0).dot(p);
757  P[P_id][1] = axis.col(1).dot(p);
758  P[P_id][2] = axis.col(2).dot(p);
759  P_id++;
760  }
761  }
762  }
763  }
764  else
765  {
766  for(int i = 0; i < n; ++i)
767  {
768  int index = indirect_index ? indices[i] : i;
769 
770  const Vector3<S>& p = ps[index];
771  P[P_id][0] = axis.col(0).dot(p);
772  P[P_id][1] = axis.col(1).dot(p);
773  P[P_id][2] = axis.col(2).dot(p);
774  P_id++;
775 
776  if(ps2)
777  {
778  const Vector3<S>& v = ps2[index];
779  P[P_id][0] = axis.col(0).dot(v);
780  P[P_id][1] = axis.col(1).dot(v);
781  P[P_id][2] = axis.col(2).dot(v);
782  P_id++;
783  }
784  }
785  }
786 
787  S minx, maxx, miny, maxy, minz, maxz;
788 
789  S cz, radsqr;
790 
791  minz = maxz = P[0][2];
792 
793  for(int i = 1; i < size_P; ++i)
794  {
795  S z_value = P[i][2];
796  if(z_value < minz) minz = z_value;
797  else if(z_value > maxz) maxz = z_value;
798  }
799 
800  r = (S)0.5 * (maxz - minz);
801  radsqr = r * r;
802  cz = (S)0.5 * (maxz + minz);
803 
804  // compute an initial length of rectangle along x direction
805 
806  // find minx and maxx as starting points
807 
808  int minindex, maxindex;
809  minindex = maxindex = 0;
810  S mintmp, maxtmp;
811  mintmp = maxtmp = P[0][0];
812 
813  for(int i = 1; i < size_P; ++i)
814  {
815  S x_value = P[i][0];
816  if(x_value < mintmp)
817  {
818  minindex = i;
819  mintmp = x_value;
820  }
821  else if(x_value > maxtmp)
822  {
823  maxindex = i;
824  maxtmp = x_value;
825  }
826  }
827 
828  S x, dz;
829  dz = P[minindex][2] - cz;
830  minx = P[minindex][0] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
831  dz = P[maxindex][2] - cz;
832  maxx = P[maxindex][0] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
833 
834 
835  // grow minx
836 
837  for(int i = 0; i < size_P; ++i)
838  {
839  if(P[i][0] < minx)
840  {
841  dz = P[i][2] - cz;
842  x = P[i][0] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
843  if(x < minx) minx = x;
844  }
845  }
846 
847  // grow maxx
848 
849  for(int i = 0; i < size_P; ++i)
850  {
851  if(P[i][0] > maxx)
852  {
853  dz = P[i][2] - cz;
854  x = P[i][0] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
855  if(x > maxx) maxx = x;
856  }
857  }
858 
859  // compute an initial length of rectangle along y direction
860 
861  // find miny and maxy as starting points
862 
863  minindex = maxindex = 0;
864  mintmp = maxtmp = P[0][1];
865  for(int i = 1; i < size_P; ++i)
866  {
867  S y_value = P[i][1];
868  if(y_value < mintmp)
869  {
870  minindex = i;
871  mintmp = y_value;
872  }
873  else if(y_value > maxtmp)
874  {
875  maxindex = i;
876  maxtmp = y_value;
877  }
878  }
879 
880  S y;
881  dz = P[minindex][2] - cz;
882  miny = P[minindex][1] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
883  dz = P[maxindex][2] - cz;
884  maxy = P[maxindex][1] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
885 
886  // grow miny
887 
888  for(int i = 0; i < size_P; ++i)
889  {
890  if(P[i][1] < miny)
891  {
892  dz = P[i][2] - cz;
893  y = P[i][1] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
894  if(y < miny) miny = y;
895  }
896  }
897 
898  // grow maxy
899 
900  for(int i = 0; i < size_P; ++i)
901  {
902  if(P[i][1] > maxy)
903  {
904  dz = P[i][2] - cz;
905  y = P[i][1] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
906  if(y > maxy) maxy = y;
907  }
908  }
909 
910  // corners may have some points which are not covered - grow lengths if necessary
911  // quite conservative (can be improved)
912  S dx, dy, u, t;
913  S a = std::sqrt((S)0.5);
914  for(int i = 0; i < size_P; ++i)
915  {
916  if(P[i][0] > maxx)
917  {
918  if(P[i][1] > maxy)
919  {
920  dx = P[i][0] - maxx;
921  dy = P[i][1] - maxy;
922  u = dx * a + dy * a;
923  t = (a*u - dx)*(a*u - dx) +
924  (a*u - dy)*(a*u - dy) +
925  (cz - P[i][2])*(cz - P[i][2]);
926  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
927  if(u > 0)
928  {
929  maxx += u*a;
930  maxy += u*a;
931  }
932  }
933  else if(P[i][1] < miny)
934  {
935  dx = P[i][0] - maxx;
936  dy = P[i][1] - miny;
937  u = dx * a - dy * a;
938  t = (a*u - dx)*(a*u - dx) +
939  (-a*u - dy)*(-a*u - dy) +
940  (cz - P[i][2])*(cz - P[i][2]);
941  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
942  if(u > 0)
943  {
944  maxx += u*a;
945  miny -= u*a;
946  }
947  }
948  }
949  else if(P[i][0] < minx)
950  {
951  if(P[i][1] > maxy)
952  {
953  dx = P[i][0] - minx;
954  dy = P[i][1] - maxy;
955  u = dy * a - dx * a;
956  t = (-a*u - dx)*(-a*u - dx) +
957  (a*u - dy)*(a*u - dy) +
958  (cz - P[i][2])*(cz - P[i][2]);
959  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
960  if(u > 0)
961  {
962  minx -= u*a;
963  maxy += u*a;
964  }
965  }
966  else if(P[i][1] < miny)
967  {
968  dx = P[i][0] - minx;
969  dy = P[i][1] - miny;
970  u = -dx * a - dy * a;
971  t = (-a*u - dx)*(-a*u - dx) +
972  (-a*u - dy)*(-a*u - dy) +
973  (cz - P[i][2])*(cz - P[i][2]);
974  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
975  if (u > 0)
976  {
977  minx -= u*a;
978  miny -= u*a;
979  }
980  }
981  }
982  }
983 
984  origin = axis.col(0) * minx + axis.col(1) * miny + axis.col(2) * cz;
985 
986  l[0] = maxx - minx;
987  if(l[0] < 0) l[0] = 0;
988  l[1] = maxy - miny;
989  if(l[1] < 0) l[1] = 0;
990 
991 }
992 
993 //==============================================================================
994 template <typename S>
995 FCL_EXPORT
996 void getRadiusAndOriginAndRectangleSize(
997  const Vector3<S>* const ps,
998  const Vector3<S>* const ps2,
999  Triangle* ts,
1000  unsigned int* indices,
1001  int n,
1002  Transform3<S>& tf,
1003  S l[2],
1004  S& r)
1005 {
1006  bool indirect_index = true;
1007  if(!indices) indirect_index = false;
1008 
1009  int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
1010 
1011  std::vector<Vector3<S>> P(size_P);
1012 
1013  int P_id = 0;
1014 
1015  if(ts)
1016  {
1017  for(int i = 0; i < n; ++i)
1018  {
1019  int index = indirect_index ? indices[i] : i;
1020  const Triangle& t = ts[index];
1021 
1022  for(int j = 0; j < 3; ++j)
1023  {
1024  int point_id = t[j];
1025  const Vector3<S>& p = ps[point_id];
1026  P[P_id][0] = tf.linear().col(0).dot(p);
1027  P[P_id][1] = tf.linear().col(1).dot(p);
1028  P[P_id][2] = tf.linear().col(2).dot(p);
1029  P_id++;
1030  }
1031 
1032  if(ps2)
1033  {
1034  for(int j = 0; j < 3; ++j)
1035  {
1036  int point_id = t[j];
1037  const Vector3<S>& p = ps2[point_id];
1038  P[P_id][0] = tf.linear().col(0).dot(p);
1039  P[P_id][1] = tf.linear().col(1).dot(p);
1040  P[P_id][2] = tf.linear().col(2).dot(p);
1041  P_id++;
1042  }
1043  }
1044  }
1045  }
1046  else
1047  {
1048  for(int i = 0; i < n; ++i)
1049  {
1050  int index = indirect_index ? indices[i] : i;
1051 
1052  const Vector3<S>& p = ps[index];
1053  P[P_id][0] = tf.linear().col(0).dot(p);
1054  P[P_id][1] = tf.linear().col(1).dot(p);
1055  P[P_id][2] = tf.linear().col(2).dot(p);
1056  P_id++;
1057 
1058  if(ps2)
1059  {
1060  P[P_id][0] = tf.linear().col(0).dot(ps2[index]);
1061  P[P_id][1] = tf.linear().col(1).dot(ps2[index]);
1062  P[P_id][2] = tf.linear().col(2).dot(ps2[index]);
1063  P_id++;
1064  }
1065  }
1066  }
1067 
1068  S minx, maxx, miny, maxy, minz, maxz;
1069 
1070  S cz, radsqr;
1071 
1072  minz = maxz = P[0][2];
1073 
1074  for(int i = 1; i < size_P; ++i)
1075  {
1076  S z_value = P[i][2];
1077  if(z_value < minz) minz = z_value;
1078  else if(z_value > maxz) maxz = z_value;
1079  }
1080 
1081  r = (S)0.5 * (maxz - minz);
1082  radsqr = r * r;
1083  cz = (S)0.5 * (maxz + minz);
1084 
1085  // compute an initial length of rectangle along x direction
1086 
1087  // find minx and maxx as starting points
1088 
1089  int minindex, maxindex;
1090  minindex = maxindex = 0;
1091  S mintmp, maxtmp;
1092  mintmp = maxtmp = P[0][0];
1093 
1094  for(int i = 1; i < size_P; ++i)
1095  {
1096  S x_value = P[i][0];
1097  if(x_value < mintmp)
1098  {
1099  minindex = i;
1100  mintmp = x_value;
1101  }
1102  else if(x_value > maxtmp)
1103  {
1104  maxindex = i;
1105  maxtmp = x_value;
1106  }
1107  }
1108 
1109  S x, dz;
1110  dz = P[minindex][2] - cz;
1111  minx = P[minindex][0] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1112  dz = P[maxindex][2] - cz;
1113  maxx = P[maxindex][0] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1114 
1115 
1116  // grow minx
1117 
1118  for(int i = 0; i < size_P; ++i)
1119  {
1120  if(P[i][0] < minx)
1121  {
1122  dz = P[i][2] - cz;
1123  x = P[i][0] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1124  if(x < minx) minx = x;
1125  }
1126  }
1127 
1128  // grow maxx
1129 
1130  for(int i = 0; i < size_P; ++i)
1131  {
1132  if(P[i][0] > maxx)
1133  {
1134  dz = P[i][2] - cz;
1135  x = P[i][0] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1136  if(x > maxx) maxx = x;
1137  }
1138  }
1139 
1140  // compute an initial length of rectangle along y direction
1141 
1142  // find miny and maxy as starting points
1143 
1144  minindex = maxindex = 0;
1145  mintmp = maxtmp = P[0][1];
1146  for(int i = 1; i < size_P; ++i)
1147  {
1148  S y_value = P[i][1];
1149  if(y_value < mintmp)
1150  {
1151  minindex = i;
1152  mintmp = y_value;
1153  }
1154  else if(y_value > maxtmp)
1155  {
1156  maxindex = i;
1157  maxtmp = y_value;
1158  }
1159  }
1160 
1161  S y;
1162  dz = P[minindex][2] - cz;
1163  miny = P[minindex][1] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1164  dz = P[maxindex][2] - cz;
1165  maxy = P[maxindex][1] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1166 
1167  // grow miny
1168 
1169  for(int i = 0; i < size_P; ++i)
1170  {
1171  if(P[i][1] < miny)
1172  {
1173  dz = P[i][2] - cz;
1174  y = P[i][1] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1175  if(y < miny) miny = y;
1176  }
1177  }
1178 
1179  // grow maxy
1180 
1181  for(int i = 0; i < size_P; ++i)
1182  {
1183  if(P[i][1] > maxy)
1184  {
1185  dz = P[i][2] - cz;
1186  y = P[i][1] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1187  if(y > maxy) maxy = y;
1188  }
1189  }
1190 
1191  // corners may have some points which are not covered - grow lengths if necessary
1192  // quite conservative (can be improved)
1193  S dx, dy, u, t;
1194  S a = std::sqrt((S)0.5);
1195  for(int i = 0; i < size_P; ++i)
1196  {
1197  if(P[i][0] > maxx)
1198  {
1199  if(P[i][1] > maxy)
1200  {
1201  dx = P[i][0] - maxx;
1202  dy = P[i][1] - maxy;
1203  u = dx * a + dy * a;
1204  t = (a*u - dx)*(a*u - dx) +
1205  (a*u - dy)*(a*u - dy) +
1206  (cz - P[i][2])*(cz - P[i][2]);
1207  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
1208  if(u > 0)
1209  {
1210  maxx += u*a;
1211  maxy += u*a;
1212  }
1213  }
1214  else if(P[i][1] < miny)
1215  {
1216  dx = P[i][0] - maxx;
1217  dy = P[i][1] - miny;
1218  u = dx * a - dy * a;
1219  t = (a*u - dx)*(a*u - dx) +
1220  (-a*u - dy)*(-a*u - dy) +
1221  (cz - P[i][2])*(cz - P[i][2]);
1222  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
1223  if(u > 0)
1224  {
1225  maxx += u*a;
1226  miny -= u*a;
1227  }
1228  }
1229  }
1230  else if(P[i][0] < minx)
1231  {
1232  if(P[i][1] > maxy)
1233  {
1234  dx = P[i][0] - minx;
1235  dy = P[i][1] - maxy;
1236  u = dy * a - dx * a;
1237  t = (-a*u - dx)*(-a*u - dx) +
1238  (a*u - dy)*(a*u - dy) +
1239  (cz - P[i][2])*(cz - P[i][2]);
1240  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
1241  if(u > 0)
1242  {
1243  minx -= u*a;
1244  maxy += u*a;
1245  }
1246  }
1247  else if(P[i][1] < miny)
1248  {
1249  dx = P[i][0] - minx;
1250  dy = P[i][1] - miny;
1251  u = -dx * a - dy * a;
1252  t = (-a*u - dx)*(-a*u - dx) +
1253  (-a*u - dy)*(-a*u - dy) +
1254  (cz - P[i][2])*(cz - P[i][2]);
1255  u = u - std::sqrt(std::max<S>(radsqr - t, 0));
1256  if (u > 0)
1257  {
1258  minx -= u*a;
1259  miny -= u*a;
1260  }
1261  }
1262  }
1263  }
1264 
1265  tf.translation().noalias() = tf.linear().col(0) * minx;
1266  tf.translation().noalias() += tf.linear().col(1) * miny;
1267  tf.translation().noalias() += tf.linear().col(2) * cz;
1268 
1269  l[0] = maxx - minx;
1270  if(l[0] < 0) l[0] = 0;
1271  l[1] = maxy - miny;
1272  if(l[1] < 0) l[1] = 0;
1273 }
1274 
1275 //==============================================================================
1276 template <typename S>
1277 FCL_EXPORT
1278 void circumCircleComputation(
1279  const Vector3<S>& a,
1280  const Vector3<S>& b,
1281  const Vector3<S>& c,
1282  Vector3<S>& center,
1283  S& radius)
1284 {
1285  Vector3<S> e1 = a - c;
1286  Vector3<S> e2 = b - c;
1287  S e1_len2 = e1.squaredNorm();
1288  S e2_len2 = e2.squaredNorm();
1289  Vector3<S> e3 = e1.cross(e2);
1290  S e3_len2 = e3.squaredNorm();
1291  radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2;
1292  radius = std::sqrt(radius) * 0.5;
1293 
1294  center = c;
1295  center.noalias() += (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2);
1296 }
1297 
1298 
1299 
1300 //==============================================================================
1301 template <typename S>
1302 FCL_EXPORT
1303 S maximumDistance(
1304  const Vector3<S>* const ps,
1305  const Vector3<S>* const ps2,
1306  Triangle* ts,
1307  unsigned int* indices,
1308  int n,
1309  const Vector3<S>& query)
1310 {
1311  if(ts)
1312  return detail::maximumDistance_mesh(ps, ps2, ts, indices, n, query);
1313  else
1314  return detail::maximumDistance_pointcloud(ps, ps2, indices, n, query);
1315 }
1316 
1317 //==============================================================================
1318 template <typename S>
1319 FCL_EXPORT
1320 void getExtentAndCenter(
1321  const Vector3<S>* const ps,
1322  const Vector3<S>* const ps2,
1323  Triangle* ts,
1324  unsigned int* indices,
1325  int n,
1326  const Matrix3<S>& axis,
1327  Vector3<S>& center,
1328  Vector3<S>& extent)
1329 {
1330  if(ts)
1331  detail::getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axis, center, extent);
1332  else
1333  detail::getExtentAndCenter_pointcloud(ps, ps2, indices, n, axis, center, extent);
1334 }
1335 
1336 //==============================================================================
1337 template <typename S>
1338 FCL_EXPORT
1339 void getCovariance(
1340  const Vector3<S>* const ps,
1341  const Vector3<S>* const ps2,
1342  Triangle* ts,
1343  unsigned int* indices,
1344  int n, Matrix3<S>& M)
1345 {
1346  Vector3<S> S1 = Vector3<S>::Zero();
1347  Vector3<S> S2[3] = {
1348  Vector3<S>::Zero(), Vector3<S>::Zero(), Vector3<S>::Zero()
1349  };
1350 
1351  if(ts)
1352  {
1353  for(int i = 0; i < n; ++i)
1354  {
1355  const Triangle& t = (indices) ? ts[indices[i]] : ts[i];
1356 
1357  const Vector3<S>& p1 = ps[t[0]];
1358  const Vector3<S>& p2 = ps[t[1]];
1359  const Vector3<S>& p3 = ps[t[2]];
1360 
1361  S1 += (p1 + p2 + p3).eval();
1362 
1363  S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
1364  S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
1365  S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
1366  S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
1367  S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
1368  S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
1369 
1370  if(ps2)
1371  {
1372  const Vector3<S>& p1 = ps2[t[0]];
1373  const Vector3<S>& p2 = ps2[t[1]];
1374  const Vector3<S>& p3 = ps2[t[2]];
1375 
1376  S1 += (p1 + p2 + p3).eval();
1377 
1378  S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]);
1379  S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]);
1380  S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]);
1381  S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]);
1382  S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]);
1383  S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]);
1384  }
1385  }
1386  }
1387  else
1388  {
1389  for(int i = 0; i < n; ++i)
1390  {
1391  const Vector3<S>& p = (indices) ? ps[indices[i]] : ps[i];
1392  S1 += p;
1393  S2[0][0] += (p[0] * p[0]);
1394  S2[1][1] += (p[1] * p[1]);
1395  S2[2][2] += (p[2] * p[2]);
1396  S2[0][1] += (p[0] * p[1]);
1397  S2[0][2] += (p[0] * p[2]);
1398  S2[1][2] += (p[1] * p[2]);
1399 
1400  if(ps2) // another frame
1401  {
1402  const Vector3<S>& p = (indices) ? ps2[indices[i]] : ps2[i];
1403  S1 += p;
1404  S2[0][0] += (p[0] * p[0]);
1405  S2[1][1] += (p[1] * p[1]);
1406  S2[2][2] += (p[2] * p[2]);
1407  S2[0][1] += (p[0] * p[1]);
1408  S2[0][2] += (p[0] * p[2]);
1409  S2[1][2] += (p[1] * p[2]);
1410  }
1411  }
1412  }
1413 
1414  int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
1415 
1416  M(0, 0) = S2[0][0] - S1[0]*S1[0] / n_points;
1417  M(1, 1) = S2[1][1] - S1[1]*S1[1] / n_points;
1418  M(2, 2) = S2[2][2] - S1[2]*S1[2] / n_points;
1419  M(0, 1) = S2[0][1] - S1[0]*S1[1] / n_points;
1420  M(1, 2) = S2[1][2] - S1[1]*S1[2] / n_points;
1421  M(0, 2) = S2[0][2] - S1[0]*S1[2] / n_points;
1422  M(1, 0) = M(0, 1);
1423  M(2, 0) = M(0, 2);
1424  M(2, 1) = M(1, 2);
1425 }
1426 
1427 } // namespace fcl
1428 
1429 #endif
fcl::circumCircleComputation
template void circumCircleComputation(const Vector3d &a, const Vector3d &b, const Vector3d &c, Vector3d &center, double &radius)
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:92
fcl::axisFromEigen
template void axisFromEigen(const Matrix3d &eigenV, const Vector3d &eigenS, Matrix3d &axis)
fcl::VectorN
Eigen::Matrix< S, N, 1 > VectorN
Definition: types.h:80
fcl::detail::maximumDistance_mesh
template double maximumDistance_mesh(const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, const Vector3d &query)
fcl::detail::getExtentAndCenter_pointcloud
template void getExtentAndCenter_pointcloud(const Vector3d *const ps, const Vector3d *const ps2, unsigned int *indices, int n, const Matrix3d &axis, Vector3d &center, Vector3d &extent)
geometry.h
fcl::detail::maximumDistance_mesh
FCL_EXPORT S maximumDistance_mesh(const Vector3< S > *const ps, const Vector3< S > *const ps2, Triangle *ts, unsigned int *indices, int n, const Vector3< S > &query)
Definition: geometry-inl.h:149
fcl::Transform3d
Transform3< double > Transform3d
Definition: types.h:118
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::getExtentAndCenter_mesh
template void getExtentAndCenter_mesh(const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3d &axis, Vector3d &center, Vector3d &extent)
fcl::combine
FCL_EXPORT VectorN< S, M+N > combine(const VectorN< S, M > &v1, const VectorN< S, N > &v2)
Definition: geometry-inl.h:441
fcl::normalize
template void normalize(Vector3d &v, bool *signal)
fcl::Triangle
Triangle with 3 indices for points.
Definition: triangle.h:48
fcl::Vector3d
Vector3< double > Vector3d
Definition: types.h:112
fcl::detail::maximumDistance_pointcloud
template double maximumDistance_pointcloud(const Vector3d *const ps, const Vector3d *const ps2, unsigned int *indices, int n, const Vector3d &query)
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:71
fcl::detail::getExtentAndCenter_mesh
FCL_EXPORT void getExtentAndCenter_mesh(const Vector3< S > *const ps, const Vector3< S > *const ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3< S > &axis, Vector3< S > &center, Vector3< S > &extent)
Compute the bounding volume extent and center for a set or subset of points. The bounding volume axes...
Definition: geometry-inl.h:294
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:86
fcl::detail::maximumDistance_pointcloud
FCL_EXPORT S maximumDistance_pointcloud(const Vector3< S > *const ps, const Vector3< S > *const ps2, unsigned int *indices, int n, const Vector3< S > &query)
Definition: geometry-inl.h:194
fcl::getCovariance
template void getCovariance(const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, Matrix3d &M)
fcl::maximumDistance
template double maximumDistance(const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, const Vector3d &query)
fcl::detail::getExtentAndCenter_pointcloud
FCL_EXPORT void getExtentAndCenter_pointcloud(const Vector3< S > *const ps, const Vector3< S > *const ps2, unsigned int *indices, int n, const Matrix3< S > &axis, Vector3< S > &center, Vector3< S > &extent)
Compute the bounding volume extent and center for a set or subset of points. The bounding volume axes...
Definition: geometry-inl.h:229
r
S r
Definition: test_sphere_box.cpp:171
Vector3d
fcl::Vector3d Vector3d
Definition: test_broadphase_dynamic_AABB_tree.cpp:48
fcl::eigen_old
template void eigen_old(const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
fcl::generateCoordinateSystem
template Matrix3d generateCoordinateSystem(const Vector3d &x_axis)
fcl::getExtentAndCenter
template void getExtentAndCenter(const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3d &axis, Vector3d &center, Vector3d &extent)
fcl::getRadiusAndOriginAndRectangleSize
template void getRadiusAndOriginAndRectangleSize(const Vector3d *const ps, const Vector3d *const ps2, Triangle *ts, unsigned int *indices, int n, const Matrix3d &axis, Vector3d &origin, double l[2], double &r)
constants.h
fcl::hat
template void hat(Matrix3d &mat, const Vector3d &vec)
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::Matrix3d
Matrix3< double > Matrix3d
Definition: types.h:116
fcl::eigen
template void eigen(const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
fcl::triple
FCL_EXPORT Derived::RealScalar triple(const Eigen::MatrixBase< Derived > &x, const Eigen::MatrixBase< Derived > &y, const Eigen::MatrixBase< Derived > &z)
Definition: geometry-inl.h:431
fcl::relativeTransform
FCL_EXPORT void relativeTransform(const Eigen::MatrixBase< DerivedA > &R1, const Eigen::MatrixBase< DerivedB > &t1, const Eigen::MatrixBase< DerivedA > &R2, const Eigen::MatrixBase< DerivedB > &t2, Eigen::MatrixBase< DerivedC > &R, Eigen::MatrixBase< DerivedD > &t)
Definition: geometry-inl.h:659
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45

fcl
Author(s):
autogenerated on Fri Mar 14 2025 02:38:17