static Vector3 closestPtPointTetrahedron(ref int size)
{
float eps = (1e-4f);
Vector3 a = Q[0];
Vector3 b = Q[1];
Vector3 c = Q[2];
Vector3 d = Q[3];
//degenerated
Vector3 ab = (b - a);
Vector3 ac = (c - a);
Vector3 n = Vector3.Normalize(Vector3.Cross(ab, ac));
float signDist = Vector3.Dot(n, (d - a));
if (eps > Mathf.Abs(signDist))
{
size = 3;
return(closestPtPointTriangle(ref size));
}
BoolV4 bIsOutside4 = PointOutsideOfPlane4(a, b, c, d);
if (bIsOutside4.BAllEq(false))
{
//All inside
return(Vector3.zero);
}
Index indices = new Index(); indices.Reset();
Vector3 closest = getClosestPtPointTriangle(bIsOutside4, ref indices, ref size);
Vector3 q0 = Q[indices.x]; Vector3 q1 = Q[indices.y]; Vector3 q2 = Q[indices.z];
int b0 = B[indices.x]; int b1 = B[indices.y]; int b2 = B[indices.z];
Q[0] = q0; Q[1] = q1; Q[2] = q2;
B[0] = b0; B[1] = b1; B[2] = b2;
return(closest);
}
static Vector3 getClosestPtPointTriangle(BoolV4 bIsOutside4, ref Index indices, ref int size)
{
float bestSqDist = float.MaxValue;
Index _indices = new Index(); _indices.Reset();
Vector3 result = Vector3.zero;
if (bIsOutside4.x)
{
//use the original indices, size, v and w
result = closestPtPointTriangleBaryCentric(Q[0], Q[1], Q[2], ref indices, ref size);
bestSqDist = Vector3.Dot(result, result);
}
if (bIsOutside4.y)
{
int _size = 3;
_indices.x = 0; _indices.y = 2; _indices.z = 3;
Vector3 q = closestPtPointTriangleBaryCentric(Q[0], Q[2], Q[3], ref _indices, ref _size);
float sqDist = Vector3.Dot(q, q);
bool con = bestSqDist > sqDist;
if (con)
{
result = q;
bestSqDist = sqDist;
indices.x = _indices.x;
indices.y = _indices.y;
indices.z = _indices.z;
size = _size;
}
}
if (bIsOutside4.z)
{
int _size = 3;
_indices.x = 0; _indices.y = 3; _indices.z = 1;
Vector3 q = closestPtPointTriangleBaryCentric(Q[0], Q[3], Q[1], ref _indices, ref _size);
float sqDist = Vector3.Dot(q, q);
bool con = bestSqDist > sqDist;
if (con)
{
result = q;
bestSqDist = sqDist;
indices.x = _indices.x;
indices.y = _indices.y;
indices.z = _indices.z;
size = _size;
}
}
if (bIsOutside4.w)
{
int _size = 3;
_indices.x = 1; _indices.y = 3; _indices.z = 2;
Vector3 q = closestPtPointTriangleBaryCentric(Q[1], Q[3], Q[2], ref _indices, ref _size);
float sqDist = Vector3.Dot(q, q);
bool con = bestSqDist > sqDist;
if (con)
{
result = q;
bestSqDist = sqDist;
indices.x = _indices.x;
indices.y = _indices.y;
indices.z = _indices.z;
size = _size;
}
}
return(result);
}