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;
}
}
