public bool closestPtPointTetrahedron( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btVector3 d, btSubSimplexClosestResult finalResult )
{
btSubSimplexClosestResult tempResult = new btSubSimplexClosestResult();
// Start out assuming point inside all halfspaces, so closest to itself
finalResult.m_closestPointOnSimplex = p;
finalResult.m_usedVertices = 0;
finalResult.m_usedVertices |= btUsageBitfield.usedVertexA;
finalResult.m_usedVertices |= btUsageBitfield.usedVertexB;
finalResult.m_usedVertices |= btUsageBitfield.usedVertexC;
finalResult.m_usedVertices |= btUsageBitfield.usedVertexD;
int pointOutsideABC = pointOutsideOfPlane( ref p, ref a, ref b, ref c, ref d );
int pointOutsideACD = pointOutsideOfPlane( ref p, ref a, ref c, ref d, ref b );
int pointOutsideADB = pointOutsideOfPlane( ref p, ref a, ref d, ref b, ref c );
int pointOutsideBDC = pointOutsideOfPlane( ref p, ref b, ref d, ref c, ref a );
if( pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0 )
{
finalResult.m_degenerate = true;
return false;
}
if( pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0 )
{
return false;
}
double bestSqDist = btScalar.BT_MAX_FLOAT;
// If point outside face abc then compute closest point on abc
btVector3 tmp;
if( pointOutsideABC != 0 )
{
closestPtPointTriangle( ref p, ref a, ref b, ref c, ref tempResult );
//btVector3 q = tempResult.m_closestPointOnSimplex;
tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
double sqDist = ( tmp ).dot( ref tmp );
// Update best closest point if (squared) distance is less than current best
if( sqDist < bestSqDist )
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
finalResult.m_usedVertices = 0;
finalResult.m_usedVertices = tempResult.m_usedVertices
& ( btUsageBitfield.usedVertexA | btUsageBitfield.usedVertexB | btUsageBitfield.usedVertexC );
finalResult.setBarycentricCoordinates(
tempResult.m_barycentricCoord0,
tempResult.m_barycentricCoord1,
tempResult.m_barycentricCoord2,
0
);
}
}
// Repeat test for face acd
if( pointOutsideACD != 0 )
{
closestPtPointTriangle( ref p, ref a, ref c, ref d, ref tempResult );
//btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
double sqDist = ( tmp ).dot( ref tmp );
//double sqDist = ( q - p ).dot( q - p );
if( sqDist < bestSqDist )
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
finalResult.m_usedVertices = 0;
finalResult.m_usedVertices = tempResult.m_usedVertices & ( btUsageBitfield.usedVertexA );
finalResult.m_usedVertices |= ((tempResult.m_usedVertices & btUsageBitfield.usedVertexB) != 0)? btUsageBitfield.usedVertexC : 0;
finalResult.m_usedVertices |= ((tempResult.m_usedVertices & btUsageBitfield.usedVertexC) != 0)? btUsageBitfield.usedVertexD : 0;
finalResult.setBarycentricCoordinates(
tempResult.m_barycentricCoord0,
0,
tempResult.m_barycentricCoord1,
tempResult.m_barycentricCoord2
);
}
}
// Repeat test for face adb
if( pointOutsideADB != 0 )
{
closestPtPointTriangle( ref p, ref a, ref d, ref b, ref tempResult );
//btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
double sqDist = ( tmp ).dot( ref tmp );
//double sqDist = ( q - p ).dot( q - p );
if( sqDist < bestSqDist )
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
finalResult.m_usedVertices = 0;
finalResult.m_usedVertices = tempResult.m_usedVertices & ( btUsageBitfield.usedVertexA );
finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexC ) != 0 ) ? btUsageBitfield.usedVertexB : 0;
finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexB ) != 0 ) ? btUsageBitfield.usedVertexD : 0;
finalResult.setBarycentricCoordinates(
tempResult.m_barycentricCoord0,
tempResult.m_barycentricCoord2,
0,
tempResult.m_barycentricCoord3
);
}
}
// Repeat test for face bdc
if( pointOutsideBDC != 0 )
{
closestPtPointTriangle( ref p, ref b, ref d, ref c, ref tempResult );
//btVector3 q = tempResult.m_closestPointOnSimplex;
//convert result bitmask!
tempResult.m_closestPointOnSimplex.Sub( ref p, out tmp );
double sqDist = ( tmp ).dot( ref tmp );
//double sqDist = ( q - p ).dot( q - p );
if( sqDist < bestSqDist )
{
bestSqDist = sqDist;
finalResult.m_closestPointOnSimplex = tempResult.m_closestPointOnSimplex;
finalResult.m_usedVertices = 0;
//
finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexA ) != 0 ) ? btUsageBitfield.usedVertexB : 0;
finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexC ) != 0 ) ? btUsageBitfield.usedVertexC : 0;
finalResult.m_usedVertices |= ( ( tempResult.m_usedVertices & btUsageBitfield.usedVertexB ) != 0 ) ? btUsageBitfield.usedVertexD : 0;
finalResult.setBarycentricCoordinates(
0,
tempResult.m_barycentricCoord0,
tempResult.m_barycentricCoord2,
tempResult.m_barycentricCoord2
);
}
}
//help! we ended up full !
if( ( finalResult.m_usedVertices & btUsageBitfield.usedAll ) == btUsageBitfield.usedAll )
{
return true;
}
return true;
}
public bool closestPtPointTriangle( ref btVector3 p, ref btVector3 a, ref btVector3 b, ref btVector3 c, ref btSubSimplexClosestResult result )
{
result.m_usedVertices = 0;//.reset();
// Check if P in vertex region outside A
btVector3 ab; b.Sub( ref a, out ab );
btVector3 ac; c.Sub( ref a, out ac );
btVector3 ap; p.Sub( ref a, out ap );
double d1 = ab.dot( ref ap );
double d2 = ac.dot( ref ap );
if( d1 <= (double)( 0.0 ) && d2 <= (double)( 0.0 ) )
{
result.m_closestPointOnSimplex = a;
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.setBarycentricCoordinates( 1, 0, 0 );
return true;// a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
btVector3 bp; p.Sub( ref b, out bp );
double d3 = ab.dot( ref bp );
double d4 = ac.dot( ref bp );
if( d3 >= (double)( 0.0 ) && d4 <= d3 )
{
result.m_closestPointOnSimplex = b;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.setBarycentricCoordinates( 0, 1, 0 );
return true; // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
double vc = d1 * d4 - d3 * d2;
btVector3 tmp;
if( vc <= (double)( 0.0 ) && d1 >= (double)( 0.0 ) && d3 <= (double)( 0.0 ) )
{
double v = d1 / ( d1 - d3 );
ab.Mult( v, out tmp );
a.Add( ref tmp, out result.m_closestPointOnSimplex );
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.setBarycentricCoordinates( 1 - v, v, 0 );
return true;
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
btVector3 cp; p.Sub( ref c, out cp );
double d5 = ab.dot( ref cp );
double d6 = ac.dot( ref cp );
if( d6 >= (double)( 0.0 ) && d5 <= d6 )
{
result.m_closestPointOnSimplex = c;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 0, 0, 1 );
return true;//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
double vb = d5 * d2 - d1 * d6;
if( vb <= (double)( 0.0 ) && d2 >= (double)( 0.0 ) && d6 <= (double)( 0.0 ) )
{
double w = d2 / ( d2 - d6 );
ac.Mult( w, out tmp );
a.Add( ref tmp, out result.m_closestPointOnSimplex );
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 1 - w, 0, w );
return true;
//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
double va = d3 * d6 - d5 * d4;
if( va <= (double)( 0.0 ) && ( d4 - d3 ) >= (double)( 0.0 ) && ( d5 - d6 ) >= (double)( 0.0 ) )
{
double w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) );
btVector3 tmp2;
c.Sub( ref b, out tmp2 );
tmp2.Mult( w, out tmp );
b.Add( ref tmp, out result.m_closestPointOnSimplex );
//result.m_closestPointOnSimplex = b + w * ( c - b );
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 0, 1 - w, w );
return true;
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
{
double denom = (double)( 1.0 ) / ( va + vb + vc );
double v = vb * denom;
double w = vc * denom;
ab.Mult( v, out ab );
ac.Mult( w, out ac );
a.Add( ref ab, out tmp );
tmp.Add( ref ac, out result.m_closestPointOnSimplex );
//result.m_closestPointOnSimplex = a + ab * v + ac * w;
result.m_usedVertices |= btUsageBitfield.usedVertexA;
result.m_usedVertices |= btUsageBitfield.usedVertexB;
result.m_usedVertices |= btUsageBitfield.usedVertexC;
result.setBarycentricCoordinates( 1 - v - w, v, w );
}
return true;
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = (double)(1.0) - v - w
}