|
@@ -12,6 +12,8 @@
|
|
|
#include <limits>
|
|
|
#include <cassert>
|
|
|
|
|
|
+
|
|
|
+
|
|
|
template <
|
|
|
int DIM,
|
|
|
typename Derivedp,
|
|
@@ -20,129 +22,85 @@ template <
|
|
|
typename Derivedsqr_d,
|
|
|
typename Derivedc>
|
|
|
IGL_INLINE void igl::point_simplex_squared_distance(
|
|
|
- const Eigen::PlainObjectBase<Derivedp> & p,
|
|
|
- const Eigen::PlainObjectBase<DerivedV> & V,
|
|
|
- const Eigen::PlainObjectBase<DerivedEle> & Ele,
|
|
|
+ const Eigen::MatrixBase<Derivedp> & p,
|
|
|
+ const Eigen::MatrixBase<DerivedV> & V,
|
|
|
+ const Eigen::MatrixBase<DerivedEle> & Ele,
|
|
|
const typename DerivedEle::Index primitive,
|
|
|
Derivedsqr_d & sqr_d,
|
|
|
Eigen::PlainObjectBase<Derivedc> & c)
|
|
|
{
|
|
|
- assert(p.size() == DIM);
|
|
|
- assert(V.cols() == DIM);
|
|
|
- assert(Ele.cols() <= DIM+1);
|
|
|
- assert(Ele.cols() <= 3 && "Only simplices up to triangles are considered");
|
|
|
+ typedef typename Derivedp::Scalar Scalar;
|
|
|
+ typedef typename Eigen::Matrix<Scalar,1,DIM> Vector;
|
|
|
+ typedef Vector Point;
|
|
|
|
|
|
- typedef Derivedsqr_d Scalar;
|
|
|
- typedef typename Eigen::Matrix<Scalar,1,DIM> RowVectorDIMS;
|
|
|
- sqr_d = std::numeric_limits<Scalar>::infinity();
|
|
|
- const auto & set_min =
|
|
|
- [&sqr_d,&c](const Scalar & sqr_d_candidate, const RowVectorDIMS & c_candidate)
|
|
|
+ const auto & Dot = [](const Point & a, const Point & b)->Scalar
|
|
|
{
|
|
|
- if(sqr_d_candidate < sqr_d)
|
|
|
- {
|
|
|
- sqr_d = sqr_d_candidate;
|
|
|
- c = c_candidate;
|
|
|
- }
|
|
|
+ return a.dot(b);
|
|
|
};
|
|
|
-
|
|
|
- // Simplex size
|
|
|
- const size_t ss = Ele.cols();
|
|
|
- // Only one element per node
|
|
|
- // plane unit normal
|
|
|
- bool inside_triangle = false;
|
|
|
- Scalar d_j = std::numeric_limits<Scalar>::infinity();
|
|
|
- RowVectorDIMS pp;
|
|
|
- // Only consider triangles, and non-degenerate triangles at that
|
|
|
- if(ss == 3 &&
|
|
|
- Ele(primitive,0) != Ele(primitive,1) &&
|
|
|
- Ele(primitive,1) != Ele(primitive,2) &&
|
|
|
- Ele(primitive,2) != Ele(primitive,0))
|
|
|
+ // Real-time collision detection, Ericson, Chapter 5
|
|
|
+ const auto & ClosestPtPointTriangle =
|
|
|
+ [&Dot](Point p, Point a, Point b, Point c)->Point
|
|
|
{
|
|
|
- assert(DIM == 3 && "Only implemented for 3D triangles");
|
|
|
- typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
|
|
|
- // can't be const because of annoying DIM template
|
|
|
- RowVector3S v10(0,0,0);
|
|
|
- v10.head(DIM) = (V.row(Ele(primitive,1))- V.row(Ele(primitive,0)));
|
|
|
- RowVector3S v20(0,0,0);
|
|
|
- v20.head(DIM) = (V.row(Ele(primitive,2))- V.row(Ele(primitive,0)));
|
|
|
- const RowVectorDIMS n = (v10.cross(v20)).head(DIM);
|
|
|
- Scalar n_norm = n.norm();
|
|
|
- if(n_norm > 0)
|
|
|
+ // Check if P in vertex region outside A
|
|
|
+ Vector ab = b - a;
|
|
|
+ Vector ac = c - a;
|
|
|
+ Vector ap = p - a;
|
|
|
+ Scalar d1 = Dot(ab, ap);
|
|
|
+ Scalar d2 = Dot(ac, ap);
|
|
|
+ if (d1 <= 0.0 && d2 <= 0.0) return a; // barycentric coordinates (1,0,0)
|
|
|
+ // Check if P in vertex region outside B
|
|
|
+ Vector bp = p - b;
|
|
|
+ Scalar d3 = Dot(ab, bp);
|
|
|
+ Scalar d4 = Dot(ac, bp);
|
|
|
+ if (d3 >= 0.0 && d4 <= d3) return b; // barycentric coordinates (0,1,0)
|
|
|
+ // Check if P in edge region of AB, if so return projection of P onto AB
|
|
|
+ Scalar vc = d1*d4 - d3*d2;
|
|
|
+ if( a != b)
|
|
|
{
|
|
|
- const RowVectorDIMS un = n/n.norm();
|
|
|
- // vector to plane
|
|
|
- const RowVectorDIMS bc =
|
|
|
- 1./3.*
|
|
|
- ( V.row(Ele(primitive,0))+
|
|
|
- V.row(Ele(primitive,1))+
|
|
|
- V.row(Ele(primitive,2)));
|
|
|
- const auto & v = p-bc;
|
|
|
- // projected point on plane
|
|
|
- d_j = v.dot(un);
|
|
|
- pp = p - d_j*un;
|
|
|
- // determine if pp is inside triangle
|
|
|
- Eigen::Matrix<Scalar,1,3> b;
|
|
|
- barycentric_coordinates(
|
|
|
- pp,
|
|
|
- V.row(Ele(primitive,0)),
|
|
|
- V.row(Ele(primitive,1)),
|
|
|
- V.row(Ele(primitive,2)),
|
|
|
- b);
|
|
|
- inside_triangle = fabs(fabs(b(0)) + fabs(b(1)) + fabs(b(2)) - 1.) <= 1e-10;
|
|
|
- }
|
|
|
- }
|
|
|
- const auto & point_point_squared_distance = [&](const RowVectorDIMS & s)
|
|
|
- {
|
|
|
- const Scalar sqr_d_s = (p-s).squaredNorm();
|
|
|
- set_min(sqr_d_s,s);
|
|
|
- };
|
|
|
- if(inside_triangle)
|
|
|
- {
|
|
|
- // point-triangle squared distance
|
|
|
- const Scalar sqr_d_j = d_j*d_j;
|
|
|
- //cout<<"point-triangle..."<<endl;
|
|
|
- set_min(sqr_d_j,pp);
|
|
|
- }else
|
|
|
- {
|
|
|
- if(ss >= 2)
|
|
|
- {
|
|
|
- // point-segment distance
|
|
|
- // number of edges
|
|
|
- size_t ne = ss==3?3:1;
|
|
|
- for(size_t x = 0;x<ne;x++)
|
|
|
- {
|
|
|
- const size_t e1 = Ele(primitive,(x+1)%ss);
|
|
|
- const size_t e2 = Ele(primitive,(x+2)%ss);
|
|
|
- const RowVectorDIMS & s = V.row(e1);
|
|
|
- const RowVectorDIMS & d = V.row(e2);
|
|
|
- // Degenerate edge
|
|
|
- if(e1 == e2 || (s-d).squaredNorm()==0)
|
|
|
- {
|
|
|
- // only consider once
|
|
|
- if(e1 < e2)
|
|
|
- {
|
|
|
- point_point_squared_distance(s);
|
|
|
- }
|
|
|
- continue;
|
|
|
- }
|
|
|
- Eigen::Matrix<Scalar,1,1> sqr_d_j_x(1,1);
|
|
|
- Eigen::Matrix<Scalar,1,1> t(1,1);
|
|
|
- project_to_line_segment(p,s,d,t,sqr_d_j_x);
|
|
|
- const RowVectorDIMS q = s+t(0)*(d-s);
|
|
|
- set_min(sqr_d_j_x(0),q);
|
|
|
+ if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0) {
|
|
|
+ Scalar v = d1 / (d1 - d3);
|
|
|
+ return a + v * ab; // barycentric coordinates (1-v,v,0)
|
|
|
}
|
|
|
- }else
|
|
|
- {
|
|
|
- // So then Ele is just a list of points...
|
|
|
- assert(ss == 1);
|
|
|
- const RowVectorDIMS & s = V.row(Ele(primitive,0));
|
|
|
- point_point_squared_distance(s);
|
|
|
}
|
|
|
- }
|
|
|
+ // Check if P in vertex region outside C
|
|
|
+ Vector cp = p - c;
|
|
|
+ Scalar d5 = Dot(ab, cp);
|
|
|
+ Scalar d6 = Dot(ac, cp);
|
|
|
+ if (d6 >= 0.0 && d5 <= d6) return c; // barycentric coordinates (0,0,1)
|
|
|
+ // Check if P in edge region of AC, if so return projection of P onto AC
|
|
|
+ Scalar vb = d5*d2 - d1*d6;
|
|
|
+ if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0) {
|
|
|
+ Scalar w = d2 / (d2 - d6);
|
|
|
+ return a + w * ac; // barycentric coordinates (1-w,0,w)
|
|
|
+ }
|
|
|
+ // Check if P in edge region of BC, if so return projection of P onto BC
|
|
|
+ Scalar va = d3*d6 - d5*d4;
|
|
|
+ if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0) {
|
|
|
+ Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
|
|
+ return b + w * (c - b); // barycentric coordinates (0,1-w,w)
|
|
|
+ }
|
|
|
+ // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
|
|
|
+ Scalar denom = 1.0 / (va + vb + vc);
|
|
|
+ Scalar v = vb * denom;
|
|
|
+ Scalar w = vc * denom;
|
|
|
+ return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w
|
|
|
+ };
|
|
|
+
|
|
|
+ assert(p.size() == DIM);
|
|
|
+ assert(V.cols() == DIM);
|
|
|
+ assert(Ele.cols() <= DIM+1);
|
|
|
+ assert(Ele.cols() <= 3 && "Only simplices up to triangles are considered");
|
|
|
+
|
|
|
+ c = ClosestPtPointTriangle(
|
|
|
+ p,
|
|
|
+ V.row(Ele(primitive,0)),
|
|
|
+ V.row(Ele(primitive,1%Ele.cols())),
|
|
|
+ V.row(Ele(primitive,2%Ele.cols())));
|
|
|
+ sqr_d = (p-c).squaredNorm();
|
|
|
}
|
|
|
|
|
|
#ifdef IGL_STATIC_LIBRARY
|
|
|
// Explicit template instanciation
|
|
|
-template void igl::point_simplex_squared_distance<2, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&);
|
|
|
-template void igl::point_simplex_squared_distance<3, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
|
|
|
+template void igl::point_simplex_squared_distance<3, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 3, 1, 1, 3> >&);
|
|
|
+template void igl::point_simplex_squared_distance<2, Eigen::Matrix<double, 1, 2, 1, 1, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double, Eigen::Matrix<double, 1, 2, 1, 1, 2> >(Eigen::MatrixBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::Matrix<int, -1, -1, 0, -1, -1>::Index, double&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, 2, 1, 1, 2> >&);
|
|
|
#endif
|