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;
}
