// this is a
internal override bool calcPenDepth( btSimplexSolverInterface simplexSolver,
btConvexShape pConvexA, btConvexShape pConvexB,
ref btTransform transformA, ref btTransform transformB,
ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB,
btIDebugDraw debugDraw )
{
//(void)debugDraw;
//(void)v;
//(void)simplexSolver;
// double radialmargin(btScalar.BT_ZERO);
btVector3 guessVector; transformB.m_origin.Sub( ref transformA.m_origin, out guessVector );
btGjkEpaSolver2.sResults results;
if( btGjkEpaSolver2.Penetration( pConvexA, ref transformA,
pConvexB, ref transformB,
ref guessVector, out results ) )
{
// debugDraw.drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut.addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
v = results.normal;
return true;
}
else
{
if( btGjkEpaSolver2.Distance( pConvexA, ref transformA, pConvexB, ref transformB, ref guessVector, out results ) )
{
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
v = results.normal;
return false;
}
}
wWitnessOnA = results.witness0;
wWitnessOnB = results.witness1;
return false;
}
public void Initialize(btConvexShape convexA, btConvexShape convexB, btSimplexSolverInterface simplexSolver)
{
m_simplexSolver = ( simplexSolver );
m_convexA = ( convexA );
m_convexB = ( convexB );
}
///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. internal abstract bool calcPenDepth( btSimplexSolverInterface simplexSolver, btConvexShape convexA, btConvexShape convexB, ref btTransform transA, ref btTransform transB, ref btVector3 v, out btVector3 pa, out btVector3 pb, btIDebugDraw debugDraw );
public btContinuousConvexCollision( btConvexShape convexA, btStaticPlaneShape plane )
{
m_simplexSolver = ( null );
m_penetrationDepthSolver = ( null );
m_convexA = ( convexA ); m_convexB1 = ( null ); m_planeShape = ( plane );
}
internal void Initialize( btConvexShape convexA, btConvexShape convexB, btSimplexSolverInterface simplexSolver, btConvexPenetrationDepthSolver penetrationDepthSolver )
{
m_simplexSolver = ( simplexSolver );
m_penetrationDepthSolver = ( penetrationDepthSolver );
m_convexA = ( convexA ); m_convexB1 = ( convexB ); m_planeShape = null;
}
internal void Initialize( btConvexShape convexA, btStaticPlaneShape plane )
{
m_simplexSolver = ( null );
m_penetrationDepthSolver = ( null );
m_convexA = ( convexA ); m_convexB1 = ( null ); m_planeShape = ( plane );
}
public void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, btSimplexSolverInterface simplexSolver, btConvexPenetrationDepthSolver pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold )
{
base.Initialize( ci, body0Wrap, body1Wrap );
m_simplexSolver = ( simplexSolver );
m_pdSolver = ( pdSolver );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
m_lowLevelOfDetail = ( false );
#if USE_SEPDISTANCE_UTIL2
m_sepDistance((static_cast<btConvexShape*>(body0.getCollisionShape())).getAngularMotionDisc(),
(static_cast<btConvexShape*>(body1.getCollisionShape())).getAngularMotionDisc()),
#endif
m_numPerturbationIterations = ( numPerturbationIterations );
m_minimumPointsPerturbationThreshold = ( minimumPointsPerturbationThreshold );
}
internal CreateFunc( btSimplexSolverInterface simplexSolver, btConvexPenetrationDepthSolver pdSolver )
{
m_numPerturbationIterations = 0;
m_minimumPointsPerturbationThreshold = 3;
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
}
///cache separating vector to speedup collision detection
internal override void Cleanup()
{
if( m_ownManifold )
m_dispatcher.releaseManifold( m_manifoldPtr );
m_simplexSolver = null;
m_pdSolver = null;
m_manifoldPtr = null;
}
internal void Initialize( btConvexShape objectA, btConvexShape objectB, btSimplexSolverInterface simplexSolver
, btConvexPenetrationDepthSolver penetrationDepthSolver )
{
m_cachedSeparatingAxis = btVector3.yAxis;
m_penetrationDepthSolver = ( penetrationDepthSolver );
m_simplexSolver = ( simplexSolver );
m_minkowskiA = ( objectA );
m_minkowskiB = ( objectB );
m_shapeTypeA = ( objectA.getShapeType() );
m_shapeTypeB = ( objectB.getShapeType() );
m_marginA = ( objectA.getMargin() );
m_marginB = ( objectB.getMargin() );
m_ignoreMargin = ( false );
m_lastUsedMethod = ( -1 );
m_catchDegeneracies = ( 1 );
m_fixContactNormalDirection = ( 1 );
}
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;
}
}
