public virtual bool calcPenDepth( btSimplexSolverInterface simplexSolver, btConvexShape convexA, btConvexShape convexB, ref btTransform transA, ref btTransform transB, ref btVector3 v, ref btVector3 pa, ref btVector3 pb, btIDebugDraw debugDraw ) { //(void)v; bool check2d = convexA.isConvex2d() && convexB.isConvex2d(); //just take fixed number of orientation, and sample the penetration depth in that direction double minProj = btScalar.BT_LARGE_FLOAT; btVector3 minNorm = btVector3.Zero; btVector3 minA, minB; btVector3 seperatingAxisInA, seperatingAxisInB; btVector3 pInA, qInB, pWorld, qWorld, w; #if USE_BATCHED_SUPPORT btVector3[] supportVerticesABatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; btVector3[] supportVerticesBBatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; btVector3[] seperatingAxisInABatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; btVector3[] seperatingAxisInBBatch = new btVector3[NUM_UNITSPHERE_POINTS + btConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; int numSampleDirections = NUM_UNITSPHERE_POINTS; for( i = 0; i < numSampleDirections; i++ ) { btVector3 norm = getPenetrationDirections()[i]; seperatingAxisInABatch[i] = ( -norm ) * transA.getBasis(); seperatingAxisInBBatch[i] = norm * transB.getBasis(); } { int numPDA = convexA.getNumPreferredPenetrationDirections(); if( numPDA ) { for( int i = 0; i < numPDA; i++ ) { btVector3 norm; convexA.getPreferredPenetrationDirection( i, norm ); norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = ( -norm ) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } } { int numPDB = convexB.getNumPreferredPenetrationDirections(); if( numPDB ) { for( int i = 0; i < numPDB; i++ ) { btVector3 norm; convexB.getPreferredPenetrationDirection( i, norm ); norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; seperatingAxisInABatch[numSampleDirections] = ( -norm ) * transA.getBasis(); seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); numSampleDirections++; } } } convexA.batchedUnitVectorGetSupportingVertexWithoutMargin( seperatingAxisInABatch, supportVerticesABatch, numSampleDirections ); convexB.batchedUnitVectorGetSupportingVertexWithoutMargin( seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections ); for( i = 0; i < numSampleDirections; i++ ) { btVector3 norm = getPenetrationDirections()[i]; if( check2d ) { norm[2] = 0; } if( norm.length2() > 0.01 ) { seperatingAxisInA = seperatingAxisInABatch[i]; seperatingAxisInB = seperatingAxisInBBatch[i]; pInA = supportVerticesABatch[i]; qInB = supportVerticesBBatch[i]; pWorld = transA( pInA ); qWorld = transB( qInB ); if( check2d ) { pWorld[2] = 0; qWorld[2] = 0; } w = qWorld - pWorld; double delta = norm.dot( w ); //find smallest delta if( delta < minProj ) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } } } #else int numSampleDirections = NUM_UNITSPHERE_POINTS; #if !__SPU__ { int numPDA = convexA.getNumPreferredPenetrationDirections(); if (numPDA) { for (int i=0;i<numPDA;i++) { btVector3 norm; convexA.getPreferredPenetrationDirection(i,norm); norm = transA.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } } { int numPDB = convexB.getNumPreferredPenetrationDirections(); if (numPDB) { for (int i=0;i<numPDB;i++) { btVector3 norm; convexB.getPreferredPenetrationDirection(i,norm); norm = transB.getBasis() * norm; getPenetrationDirections()[numSampleDirections] = norm; numSampleDirections++; } } } #endif // __SPU__ for (int i=0;i<numSampleDirections;i++) { ref btVector3 norm = getPenetrationDirections()[i]; seperatingAxisInA = (-norm)* transA.getBasis(); seperatingAxisInB = norm* transB.getBasis(); pInA = convexA.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); qInB = convexB.localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); pWorld = transA(pInA); qWorld = transB(qInB); w = qWorld - pWorld; double delta = norm.dot(w); //find smallest delta if (delta < minProj) { minProj = delta; minNorm = norm; minA = pWorld; minB = qWorld; } }