/// sort cached points so most isolated points come first
int sortCachedPoints( btManifoldPoint pt )
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
#if KEEP_DEEPEST_POINT
double maxPenetration = pt.getDistance();
for( int i = 0; i < 4; i++ )
{
if( m_pointCache[i].getDistance() < maxPenetration )
{
maxPenetrationIndex = i;
maxPenetration = m_pointCache[i].getDistance();
}
}
#endif //KEEP_DEEPEST_POINT
double res0 = ( 0 )
, res1 = ( (double)( 0 ) ), res2 = ( (double)( 0 ) )
, res3 = ( (double)( 0 ) );
if( gContactCalcArea3Points )
{
if( maxPenetrationIndex != 0 )
{
btVector3 a0; pt.m_localPointA.Sub( ref m_pointCache[1].m_localPointA, out a0 );
btVector3 b0; m_pointCache[3].m_localPointA.Sub( ref m_pointCache[2].m_localPointA, out b0 );
btVector3 cross; a0.cross( ref b0, out cross );
res0 = cross.length2();
}
if( maxPenetrationIndex != 1 )
{
btVector3 a1; pt.m_localPointA.Sub( ref m_pointCache[0].m_localPointA, out a1 );
btVector3 b1; m_pointCache[3].m_localPointA.Sub( ref m_pointCache[2].m_localPointA, out b1 );
btVector3 cross; a1.cross( ref b1, out cross );
res1 = cross.length2();
}
if( maxPenetrationIndex != 2 )
{
btVector3 a2; pt.m_localPointA.Sub( ref m_pointCache[0].m_localPointA, out a2 );
btVector3 b2; m_pointCache[3].m_localPointA.Sub( ref m_pointCache[1].m_localPointA, out b2 );
btVector3 cross; a2.cross( ref b2, out cross );
res2 = cross.length2();
}
if( maxPenetrationIndex != 3 )
{
btVector3 a3; pt.m_localPointA.Sub( ref m_pointCache[0].m_localPointA, out a3 );
btVector3 b3; m_pointCache[2].m_localPointA.Sub( ref m_pointCache[1].m_localPointA, out b3 );
btVector3 cross; a3.cross( ref b3, out cross );
res3 = cross.length2();
}
}
else
{
if( maxPenetrationIndex != 0 )
{
res0 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[1].m_localPointA, ref m_pointCache[2].m_localPointA, ref m_pointCache[3].m_localPointA );
}
if( maxPenetrationIndex != 1 )
{
res1 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[0].m_localPointA, ref m_pointCache[2].m_localPointA, ref m_pointCache[3].m_localPointA );
}
if( maxPenetrationIndex != 2 )
{
res2 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[0].m_localPointA, ref m_pointCache[1].m_localPointA, ref m_pointCache[3].m_localPointA );
}
if( maxPenetrationIndex != 3 )
{
res3 = calcArea4Points( ref pt.m_localPointA, ref m_pointCache[0].m_localPointA, ref m_pointCache[1].m_localPointA, ref m_pointCache[2].m_localPointA );
}
}
btVector3 maxvec = new btVector3( res0, res1, res2, res3 );
return maxvec.closestAxis4();
}
