/// 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(); }