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