38 #ifndef FCL_BV_DETAIL_UTILITY_INL_H
39 #define FCL_BV_DETAIL_UTILITY_INL_H
83 unsigned int* indices,
96 unsigned int* indices,
117 unsigned int* indices,
127 unsigned int* indices,
139 unsigned int* indices,
147 template <
typename S>
153 unsigned int* indices,
157 bool indirect_index =
true;
158 if(!indices) indirect_index =
false;
161 for(
int i = 0; i < n; ++i)
163 unsigned int index = indirect_index ? indices[i] : i;
166 for(
int j = 0; j < 3; ++j)
171 S d = (p - query).squaredNorm();
172 if(d > maxD) maxD = d;
177 for(
int j = 0; j < 3; ++j)
182 S d = (p - query).squaredNorm();
183 if(d > maxD) maxD = d;
188 return std::sqrt(maxD);
192 template <
typename S>
197 unsigned int* indices,
201 bool indirect_index =
true;
202 if(!indices) indirect_index =
false;
205 for(
int i = 0; i < n; ++i)
207 int index = indirect_index ? indices[i] : i;
210 S d = (p - query).squaredNorm();
211 if(d > maxD) maxD = d;
216 S d = (v - query).squaredNorm();
217 if(d > maxD) maxD = d;
221 return std::sqrt(maxD);
227 template <
typename S>
232 unsigned int* indices,
238 bool indirect_index =
true;
239 if(!indices) indirect_index =
false;
246 for(
int i = 0; i < n; ++i)
248 int index = indirect_index ? indices[i] : i;
256 for(
int j = 0; j < 3; ++j)
258 if(proj[j] > max_coord[j])
259 max_coord[j] = proj[j];
261 if(proj[j] < min_coord[j])
262 min_coord[j] = proj[j];
273 for(
int j = 0; j < 3; ++j)
275 if(proj[j] > max_coord[j])
276 max_coord[j] = proj[j];
278 if(proj[j] < min_coord[j])
279 min_coord[j] = proj[j];
284 const Vector3<S> o = (max_coord + min_coord) / 2;
285 center.noalias() = axis * o;
286 extent.noalias() = (max_coord - min_coord) * 0.5;
292 template <
typename S>
298 unsigned int* indices,
304 bool indirect_index =
true;
305 if(!indices) indirect_index =
false;
312 for(
int i = 0; i < n; ++i)
314 unsigned int index = indirect_index? indices[i] : i;
317 for(
int j = 0; j < 3; ++j)
326 for(
int k = 0; k < 3; ++k)
328 if(proj[k] > max_coord[k])
329 max_coord[k] = proj[k];
331 if(proj[k] < min_coord[k])
332 min_coord[k] = proj[k];
338 for(
int j = 0; j < 3; ++j)
347 for(
int k = 0; k < 3; ++k)
349 if(proj[k] > max_coord[k])
350 max_coord[k] = proj[k];
352 if(proj[k] < min_coord[k])
353 min_coord[k] = proj[k];
359 const Vector3<S> o = (max_coord + min_coord) / 2;
360 center.noalias() = axis * o;
361 extent.noalias() = (max_coord - min_coord) / 2;
370 unsigned int* indices,
379 unsigned int* indices,
388 unsigned int* indices,
400 unsigned int* indices,
411 template <
typename S>
415 S sqr_length = v.squaredNorm();
419 v /= std::sqrt(sqr_length);
429 template <
typename Derived>
431 typename Derived::RealScalar
triple(
const Eigen::MatrixBase<Derived>& x,
432 const Eigen::MatrixBase<Derived>& y,
433 const Eigen::MatrixBase<Derived>& z)
435 return x.dot(y.cross(z));
439 template <
typename S,
int M,
int N>
451 template <
typename S>
455 mat << 0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0;
464 Eigen::SelfAdjointEigenSolver<Matrix3<S>> eigensolver(m);
465 if (eigensolver.info() != Eigen::Success)
467 std::cerr <<
"[eigen] Failed to compute eigendecomposition.\n";
470 dout = eigensolver.eigenvalues();
471 vout = eigensolver.eigenvectors();
482 S tresh, theta, tau, t, sm, s, h, g, c;
486 S v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
489 for(ip = 0; ip < n; ++ip)
491 b[ip] = d[ip] = R(ip, ip);
497 for(i = 0; i < 50; ++i)
500 for(ip = 0; ip < n; ++ip)
501 for(iq = ip + 1; iq < n; ++iq)
502 sm += std::abs(R(ip, iq));
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];
512 if(i < 3) tresh = 0.2 * sm / (n * n);
515 for(ip = 0; ip < n; ++ip)
517 for(iq= ip + 1; iq < n; ++iq)
519 g = 100.0 * std::abs(R(ip, iq));
521 std::abs(d[ip]) + g == std::abs(d[ip]) &&
522 std::abs(d[iq]) + g == std::abs(d[iq]))
524 else if(std::abs(R(ip, iq)) > tresh)
527 if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
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;
534 c = 1.0 / std::sqrt(1 + t * t);
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); }
551 for(ip = 0; ip < n; ++ip)
559 std::cerr <<
"eigen: too many iterations in Jacobi transform.\n";
565 template <
typename S>
573 if(eigenS[0] > eigenS[1])
584 if(eigenS[2] < eigenS[
min])
589 else if(eigenS[2] > eigenS[
max])
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));
605 template <
typename S>
613 if(eigenS[0] > eigenS[1])
624 if(eigenS[2] < eigenS[
min])
629 else if(eigenS[2] > eigenS[
max])
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));
645 template <
typename S>
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();
657 template <
typename DerivedA,
typename DerivedB,
typename DerivedC,
typename DerivedD>
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)
665 DerivedA::RowsAtCompileTime == 3
666 && DerivedA::ColsAtCompileTime == 3,
667 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
670 DerivedB::RowsAtCompileTime == 3
671 && DerivedB::ColsAtCompileTime == 1,
672 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
675 DerivedC::RowsAtCompileTime == 3
676 && DerivedC::ColsAtCompileTime == 3,
677 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
680 DerivedD::RowsAtCompileTime == 3
681 && DerivedD::ColsAtCompileTime == 1,
682 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
684 R.noalias() = R1.transpose() * R2;
685 t.noalias() = R1.transpose() * (t2 - t1);
689 template <
typename S,
typename DerivedA,
typename DerivedB>
694 Eigen::MatrixBase<DerivedA>& R, Eigen::MatrixBase<DerivedB>& t)
697 DerivedA::RowsAtCompileTime == 3
698 && DerivedA::ColsAtCompileTime == 3,
699 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
702 DerivedB::RowsAtCompileTime == 3
703 && DerivedB::ColsAtCompileTime == 1,
704 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
706 R.noalias() = T1.linear().transpose() * T2.linear();
707 t.noalias() = T1.linear().transpose() * (T2.translation() - T1.translation());
711 template <
typename S>
717 unsigned int* indices,
724 bool indirect_index =
true;
725 if(!indices) indirect_index =
false;
727 int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
729 std::vector<Vector3<S>> P(size_P);
735 for(
int i = 0; i < n; ++i)
737 int index = indirect_index ? indices[i] : i;
740 for(
int j = 0; j < 3; ++j)
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);
752 for(
int j = 0; j < 3; ++j)
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);
766 for(
int i = 0; i < n; ++i)
768 int index = indirect_index ? indices[i] : i;
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);
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);
787 S minx, maxx, miny, maxy, minz, maxz;
791 minz = maxz = P[0][2];
793 for(
int i = 1; i < size_P; ++i)
796 if(z_value < minz) minz = z_value;
797 else if(z_value > maxz) maxz = z_value;
800 r = (S)0.5 * (maxz - minz);
802 cz = (S)0.5 * (maxz + minz);
808 int minindex, maxindex;
809 minindex = maxindex = 0;
811 mintmp = maxtmp = P[0][0];
813 for(
int i = 1; i < size_P; ++i)
821 else if(x_value > maxtmp)
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));
837 for(
int i = 0; i < size_P; ++i)
842 x = P[i][0] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
843 if(x < minx) minx = x;
849 for(
int i = 0; i < size_P; ++i)
854 x = P[i][0] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
855 if(x > maxx) maxx = x;
863 minindex = maxindex = 0;
864 mintmp = maxtmp = P[0][1];
865 for(
int i = 1; i < size_P; ++i)
873 else if(y_value > maxtmp)
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));
888 for(
int i = 0; i < size_P; ++i)
893 y = P[i][1] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
894 if(y < miny) miny = y;
900 for(
int i = 0; i < size_P; ++i)
905 y = P[i][1] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
906 if(y > maxy) maxy = y;
913 S a = std::sqrt((S)0.5);
914 for(
int i = 0; i < size_P; ++i)
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));
933 else if(P[i][1] < miny)
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));
949 else if(P[i][0] < minx)
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));
966 else if(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));
984 origin = axis.col(0) * minx + axis.col(1) * miny + axis.col(2) * cz;
987 if(l[0] < 0) l[0] = 0;
989 if(l[1] < 0) l[1] = 0;
994 template <
typename S>
1000 unsigned int* indices,
1006 bool indirect_index =
true;
1007 if(!indices) indirect_index =
false;
1009 int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
1011 std::vector<Vector3<S>> P(size_P);
1017 for(
int i = 0; i < n; ++i)
1019 int index = indirect_index ? indices[i] : i;
1022 for(
int j = 0; j < 3; ++j)
1024 int point_id = t[j];
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);
1034 for(
int j = 0; j < 3; ++j)
1036 int point_id = t[j];
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);
1048 for(
int i = 0; i < n; ++i)
1050 int index = indirect_index ? indices[i] : i;
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);
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]);
1068 S minx, maxx, miny, maxy, minz, maxz;
1072 minz = maxz = P[0][2];
1074 for(
int i = 1; i < size_P; ++i)
1076 S z_value = P[i][2];
1077 if(z_value < minz) minz = z_value;
1078 else if(z_value > maxz) maxz = z_value;
1081 r = (S)0.5 * (maxz - minz);
1083 cz = (S)0.5 * (maxz + minz);
1089 int minindex, maxindex;
1090 minindex = maxindex = 0;
1092 mintmp = maxtmp = P[0][0];
1094 for(
int i = 1; i < size_P; ++i)
1096 S x_value = P[i][0];
1097 if(x_value < mintmp)
1102 else if(x_value > maxtmp)
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));
1118 for(
int i = 0; i < size_P; ++i)
1123 x = P[i][0] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1124 if(x < minx) minx = x;
1130 for(
int i = 0; i < size_P; ++i)
1135 x = P[i][0] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1136 if(x > maxx) maxx = x;
1144 minindex = maxindex = 0;
1145 mintmp = maxtmp = P[0][1];
1146 for(
int i = 1; i < size_P; ++i)
1148 S y_value = P[i][1];
1149 if(y_value < mintmp)
1154 else if(y_value > maxtmp)
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));
1169 for(
int i = 0; i < size_P; ++i)
1174 y = P[i][1] + std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1175 if(y < miny) miny = y;
1181 for(
int i = 0; i < size_P; ++i)
1186 y = P[i][1] - std::sqrt(std::max<S>(radsqr - dz * dz, 0));
1187 if(y > maxy) maxy = y;
1194 S a = std::sqrt((S)0.5);
1195 for(
int i = 0; i < size_P; ++i)
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));
1214 else if(P[i][1] < miny)
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));
1230 else if(P[i][0] < minx)
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));
1247 else if(P[i][1] < miny)
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));
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;
1270 if(l[0] < 0) l[0] = 0;
1272 if(l[1] < 0) l[1] = 0;
1276 template <
typename S>
1287 S e1_len2 = e1.squaredNorm();
1288 S e2_len2 = e2.squaredNorm();
1290 S e3_len2 = e3.squaredNorm();
1291 radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2;
1292 radius = std::sqrt(radius) * 0.5;
1295 center.noalias() += (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2);
1301 template <
typename S>
1307 unsigned int* indices,
1318 template <
typename S>
1324 unsigned int* indices,
1337 template <
typename S>
1343 unsigned int* indices,
1353 for(
int i = 0; i < n; ++i)
1355 const Triangle& t = (indices) ? ts[indices[i]] : ts[i];
1361 S1 += (p1 + p2 + p3).eval();
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]);
1376 S1 += (p1 + p2 + p3).eval();
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]);
1389 for(
int i = 0; i < n; ++i)
1391 const Vector3<S>& p = (indices) ? ps[indices[i]] : ps[i];
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]);
1402 const Vector3<S>& p = (indices) ? ps2[indices[i]] : ps2[i];
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]);
1414 int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n;
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;