bool VoronoiSimplexSolver::UpdateClosestVectorAndPoints() { if (m_needsUpdate) { m_cachedBC.Reset(); m_needsUpdate = false; switch (numVertices()) { case 0: m_cachedValidClosest = false; break; case 1: { m_cachedP1 = m_simplexPointsP[0]; m_cachedP2 = m_simplexPointsQ[0]; m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] m_cachedBC.Reset(); m_cachedBC.SetBarycentricCoordinates(1.f,0.f,0.f,0.f); m_cachedValidClosest = m_cachedBC.IsValid(); break; }; case 2: { //closest point origin from line segment const SimdVector3& from = m_simplexVectorW[0]; const SimdVector3& to = m_simplexVectorW[1]; SimdVector3 nearest; SimdVector3 p (0.f,0.f,0.f); SimdVector3 diff = p - from; SimdVector3 v = to - from; float t = v.dot(diff); if (t > 0) { float dotVV = v.dot(v); if (t < dotVV) { t /= dotVV; diff -= t*v; m_cachedBC.m_usedVertices.usedVertexA = true; m_cachedBC.m_usedVertices.usedVertexB = true; } else { t = 1; diff -= v; //reduce to 1 point m_cachedBC.m_usedVertices.usedVertexB = true; } } else { t = 0; //reduce to 1 point m_cachedBC.m_usedVertices.usedVertexA = true; } m_cachedBC.SetBarycentricCoordinates(1-t,t); nearest = from + t*v; m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); m_cachedV = m_cachedP1 - m_cachedP2; ReduceVertices(m_cachedBC.m_usedVertices); m_cachedValidClosest = m_cachedBC.IsValid(); break; } case 3: { //closest point origin from triangle SimdVector3 p (0.f,0.f,0.f); const SimdVector3& a = m_simplexVectorW[0]; const SimdVector3& b = m_simplexVectorW[1]; const SimdVector3& c = m_simplexVectorW[2]; ClosestPtPointTriangle(p,a,b,c,m_cachedBC); m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedV = m_cachedP1-m_cachedP2; ReduceVertices (m_cachedBC.m_usedVertices); m_cachedValidClosest = m_cachedBC.IsValid(); break; } case 4: { SimdVector3 p (0.f,0.f,0.f); const SimdVector3& a = m_simplexVectorW[0]; const SimdVector3& b = m_simplexVectorW[1]; const SimdVector3& c = m_simplexVectorW[2]; const SimdVector3& d = m_simplexVectorW[3]; bool hasSeperation = ClosestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); if (hasSeperation) { m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; m_cachedV = m_cachedP1-m_cachedP2; ReduceVertices (m_cachedBC.m_usedVertices); } else { // printf("sub distance got penetration\n"); if (m_cachedBC.m_degenerate) { m_cachedValidClosest = false; } else { m_cachedValidClosest = true; //degenerate case == false, penetration = true + zero m_cachedV.setValue(0.f,0.f,0.f); } break; } m_cachedValidClosest = m_cachedBC.IsValid(); //closest point origin from tetrahedron break; } default: { m_cachedValidClosest = false; } }; } return m_cachedValidClosest; }
IGL_INLINE void igl::point_simplex_squared_distance( 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) { typedef typename Derivedp::Scalar Scalar; typedef typename Eigen::Matrix<Scalar,1,DIM> Vector; typedef Vector Point; const auto & Dot = [](const Point & a, const Point & b)->Scalar { return a.dot(b); }; // Real-time collision detection, Ericson, Chapter 5 const auto & ClosestPtPointTriangle = [&Dot](Point p, Point a, Point b, Point c)->Point { // 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) { 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) } } // 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(); }
bool VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult) { SubSimplexClosestResult tempResult; // Start out assuming point inside all halfspaces, so closest to itself finalResult.m_closestPointOnSimplex = p; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = true; finalResult.m_usedVertices.usedVertexB = true; finalResult.m_usedVertices.usedVertexC = true; finalResult.m_usedVertices.usedVertexD = true; int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d); int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b); int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c); int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a); if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) { finalResult.m_degenerate = true; return false; } if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) { return false; } float bestSqDist = FLT_MAX; // If point outside face abc then compute closest point on abc if (pointOutsideABC) { ClosestPtPointTriangle(p, a, b, c,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; float sqDist = (q - p).dot( q - p); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; //convert result bitmask! finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTB], tempResult.m_barycentricCoords[VERTC], 0 ); } } // Repeat test for face acd if (pointOutsideACD) { ClosestPtPointTriangle(p, a, c, d,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! float sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], 0, tempResult.m_barycentricCoords[VERTB], tempResult.m_barycentricCoords[VERTC] ); } } // Repeat test for face adb if (pointOutsideADB) { ClosestPtPointTriangle(p, a, d, b,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! float sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTC], 0, tempResult.m_barycentricCoords[VERTB] ); } } // Repeat test for face bdc if (pointOutsideBDC) { ClosestPtPointTriangle(p, b, d, c,tempResult); SimdPoint3 q = tempResult.m_closestPointOnSimplex; //convert result bitmask! float sqDist = (q - p).dot( q - p); if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.m_closestPointOnSimplex = q; finalResult.m_usedVertices.reset(); finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; finalResult.SetBarycentricCoordinates( 0, tempResult.m_barycentricCoords[VERTA], tempResult.m_barycentricCoords[VERTC], tempResult.m_barycentricCoords[VERTB] ); } } //help! we ended up full ! if (finalResult.m_usedVertices.usedVertexA && finalResult.m_usedVertices.usedVertexB && finalResult.m_usedVertices.usedVertexC && finalResult.m_usedVertices.usedVertexD) { return true; } return true; }