static void RunDisplay()
{
Display = new Debug.OpenGL2.OpenGLDisplay();
Drawer = Display.GetDrawer();
Display.Run( 0, 30 );
Environment.Exit( 0 );
}
// 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;
}
internal override void getClosestPoints( ClosestPointInput input, Result output, btIDebugDraw debugDraw, bool swapResults = false )
{
//(void)debugDraw;
//btTransform transformA = input.m_transformA;
//btTransform transformB = input.m_transformB;
btVector3 point, normal;
double timeOfImpact = btScalar.BT_ONE;
double depth = btScalar.BT_ZERO;
// output.m_distance = (double)(BT_LARGE_FLOAT);
//move sphere into triangle space
btTransform sphereInTr; input.m_transformB.inverseTimes( ref input.m_transformA, out sphereInTr );
if( collide( ref sphereInTr.m_origin, out point, out normal, out depth, timeOfImpact, m_contactBreakingThreshold ) )
{
if( swapResults )
{
btVector3 normalOnB; input.m_transformB.m_basis.Apply( ref normal, out normalOnB );
btVector3 normalOnA; normalOnB.Invert( out normalOnA );
btVector3 tmp;
input.m_transformB.Apply( ref point, out tmp );
btVector3 pointOnA; tmp.AddScale( ref normalOnB, depth, out pointOnA );
output.addContactPoint( ref normalOnA, ref pointOnA, depth );
}
else
{
btVector3 tmp, tmp2;
input.m_transformB.m_basis.Apply( ref normal, out tmp );
input.m_transformB.Apply( ref point, out tmp2 );
output.addContactPoint( ref tmp, ref tmp2, depth );
}
}
}
protected virtual double solveGroupCacheFriendlyIterations( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlyIterations" );
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for( int iteration = 0; iteration < maxIterations; iteration++ )
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
{
solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
}
}
return 0;
}
protected virtual void solveGroupCacheFriendlySplitImpulseIterations( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
int iteration;
if( infoGlobal.m_splitImpulse )
{
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 )
{
for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ )
{
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int j;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
}
}
else
{
for( iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ )
{
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int j;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationImpulseCacheFriendly( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
}
}
}
}
protected virtual double solveSingleIteration( int iteration, btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_RANDMIZE_ORDER ) != 0 )
{
if( true ) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for( int j = 0; j < numNonContactPool; ++j )
{
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraintPool; ++j )
{
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for( int j = 0; j < numFrictionPool; ++j )
{
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
}
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 )
{
///solve all joint constraints, using SIMD, if available
for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ )
{
btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if( iteration < constraint.m_overrideNumSolverIterations )
resolveSingleConstraintRowGenericSIMD( constraint.m_solverBodyA
, constraint.m_solverBodyB, constraint );
}
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
if( constraint.isEnabled() )
{
btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep );
btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep );
constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
}
}
///solve all contact constraints using SIMD, if available
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS ) != 0 )
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int multiplier = ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) ? 2 : 1;
for( int c = 0; c < numPoolConstraints; c++ )
{
double totalImpulse = 0;
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if( applyFriction )
{
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int j;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
gResolveSingleConstraintRowLowerLimit_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count;
for( j = 0; j < numFrictionPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
btScalar.Dbg( "PreFriction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) );
gResolveSingleConstraintRowGeneric_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
//resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
btScalar.Dbg( "Friction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) );
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count;
for( j = 0; j < numRollingFrictionPoolConstraints; j++ )
{
btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
gResolveSingleConstraintRowGeneric_scalar_reference( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
//resolveSingleConstraintRowGenericSIMD( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
}
}
}
}
}
else
{
//non-SIMD version
///solve all joint constraints
for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ )
{
btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if( iteration < constraint.m_overrideNumSolverIterations )
resolveSingleConstraintRowGeneric( constraint.m_solverBodyA, constraint.m_solverBodyB, constraint );
}
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
if( constraint.isEnabled() )
{
//int bodyAid = ;
//int bodyBid = ;
btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep );
btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep );
constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
for( int j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimit( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count;
for( int j = 0; j < numFrictionPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGeneric( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count;
for( int j = 0; j < numRollingFrictionPoolConstraints; j++ )
{
btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
resolveSingleConstraintRowGeneric( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
}
}
}
}
return 0;
}
protected virtual double solveGroupCacheFriendlySetup( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
if( m_fixedBody != null )
{
BulletGlobals.SolverBodyPool.Free( m_fixedBody );
m_fixedBody = null;
}
//m_fixedBodyId = -1;
CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlySetup" );
//(void)debugDrawer;
m_maxOverrideNumSolverIterations = 0;
#if BT_ADDITIONAL_DEBUG
//make sure that dynamic bodies exist for all (enabled)raints
for (int i=0;i<numConstraints;i++)
{
btTypedConstraint* constraint = constraints[i];
if (constraint.isEnabled())
{
if (!constraint.getRigidBodyA().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint.getRigidBodyA()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
if (!constraint.getRigidBodyB().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint.getRigidBodyB()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
}
}
//make sure that dynamic bodies exist for all contact manifolds
for (int i=0;i<numManifolds;i++)
{
if (!manifoldPtr[i].getBody0().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (manifoldPtr[i].getBody0()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
if (!manifoldPtr[i].getBody1().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (manifoldPtr[i].getBody1()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
}
#endif //BT_ADDITIONAL_DEBUG
for( int i = 0; i < numBodies; i++ )
{
bodies[i].setCompanionBody( null );
}
m_tmpSolverBodyPool.Capacity = ( numBodies + 1 );
m_tmpSolverBodyPool.Count = ( 0 );
//btSolverBody fixedBody = m_tmpSolverBodyPool.expand();
//initSolverBody(&fixedBody,0);
//convert all bodies
for( int i = 0; i < numBodies; i++ )
{
//int bodyId = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep );
btRigidBody body = btRigidBody.upcast( bodies[i] );
if( body != null && body.getInvMass() != 0 )
{
btSolverBody solverBody = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep );
btVector3 gyroForce = btVector3.Zero;
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT ) != 0 )
{
body.computeGyroscopicForceExplicit( infoGlobal.m_maxGyroscopicForce, out gyroForce );
btVector3 tmp;
body.m_invInertiaTensorWorld.ApplyInverse( ref gyroForce, out tmp );
//solverBody.m_externalTorqueImpulse -= gyroForce * body.m_invInertiaTensorWorld * infoGlobal.m_timeStep;
solverBody.m_externalTorqueImpulse.SubScale( ref tmp, infoGlobal.m_timeStep, out solverBody.m_externalTorqueImpulse );
}
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD ) != 0 )
{
body.computeGyroscopicImpulseImplicit_World( infoGlobal.m_timeStep, out gyroForce );
solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse );
}
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ) != 0 )
{
body.computeGyroscopicImpulseImplicit_Body( infoGlobal.m_timeStep, out gyroForce );
btScalar.Dbg( "Gyroforce " + gyroForce );
solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse );
}
}
}
if( true )
{
int j;
for( j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
constraint.buildJacobian();
constraint.internalSetAppliedImpulse( 0.0f );
}
}
//btRigidBody rb0=0,*rb1=0;
//if (1)
{
{
int totalNumRows = 0;
int i;
m_tmpConstraintSizesPool.Capacity = ( numConstraints );
//calculate the total number of contraint rows
for( i = 0; i < numConstraints; i++ )
{
int infoNumConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows;
//btTypedConstraint.btConstraintInfo1 info1 = m_tmpConstraintSizesPool[i];
btTypedConstraint.btJointFeedback fb = constraints[i + startConstraint].getJointFeedback();
if( fb != null )
{
fb.m_appliedForceBodyA.setZero();
fb.m_appliedTorqueBodyA.setZero();
fb.m_appliedForceBodyB.setZero();
fb.m_appliedTorqueBodyB.setZero();
}
if( constraints[i + startConstraint].isEnabled() )
{
constraints[i + startConstraint].getInfo1( ref m_tmpConstraintSizesPool.InternalArray[i] );
}
else
{
m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows = 0;
m_tmpConstraintSizesPool.InternalArray[i].nub = 0;
}
totalNumRows += m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows;
}
m_tmpSolverNonContactConstraintPool.Count = ( totalNumRows );
for( i = 0; i < totalNumRows; i++ )
m_tmpSolverNonContactConstraintPool[i] = BulletGlobals.SolverConstraintPool.Get();
///setup the btSolverConstraints
int currentRow = 0;
for( i = 0; i < numConstraints; i++ )
{
int infoConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows;
if( infoConstraintRows != 0 )
{
Debug.Assert( currentRow < totalNumRows );
btSolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow];
btTypedConstraint constraint = constraints[i + startConstraint];
btRigidBody rbA = constraint.getRigidBodyA();
btRigidBody rbB = constraint.getRigidBodyB();
//int solverBodyIdA = ;
//int solverBodyIdB = ;
btSolverBody bodyAPtr = getOrInitSolverBody( rbA, infoGlobal.m_timeStep );
btSolverBody bodyBPtr = getOrInitSolverBody( rbB, infoGlobal.m_timeStep );
int overrideNumSolverIterations = constraint.getOverrideNumSolverIterations() > 0 ? constraint.getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
if( overrideNumSolverIterations > m_maxOverrideNumSolverIterations )
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
int j;
for( j = 0; j < infoConstraintRows; j++ )
{
btSolverConstraint current = m_tmpSolverNonContactConstraintPool[currentRow + j];
current.Clear();
//memset( ¤tConstraintRow[j], 0, sizeof( btSolverConstraint ) );
current.m_lowerLimit = btScalar.SIMD_NEG_INFINITY;
current.m_upperLimit = btScalar.SIMD_INFINITY;
current.m_appliedImpulse = 0;
current.m_appliedPushImpulse = 0;
current.m_solverBodyA = bodyAPtr;
current.m_solverBodyB = bodyBPtr;
current.m_overrideNumSolverIterations = overrideNumSolverIterations;
}
bodyAPtr.Clear();
btTypedConstraint.btConstraintInfo2 info2 = m_tmpConstraintInfo2Pool.Get();// new btTypedConstraint.btConstraintInfo2();
info2.m_numRows = infoConstraintRows;
for( j = 0; j < infoConstraintRows; ++j )
{
info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j];
}
info2.fps = 1 / infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
#if OLD_CONSTRAINT_INFO_INIT
info2.m_J1linearAxis = currentConstraintRow.m_contactNormal1;
info2.m_J1angularAxis = currentConstraintRow.m_relpos1CrossNormal;
info2.m_J2linearAxis = currentConstraintRow.m_contactNormal2;
info2.m_J2angularAxis = currentConstraintRow.m_relpos2CrossNormal;
info2.rowskip = 0;// sizeof( btSolverConstraint ) / sizeof( double );//check this
///the size of btSolverConstraint needs be a multiple of double
//Debug.Assert( info2.rowskip * sizeof( double ) == sizeof( btSolverConstraint ) );
info2.m_constraintError = currentConstraintRow.m_rhs;
info2.cfm = currentConstraintRow.m_cfm;
info2.m_lowerLimit = currentConstraintRow.m_lowerLimit;
info2.m_upperLimit = currentConstraintRow.m_upperLimit;
#endif
currentConstraintRow.m_cfm = infoGlobal.m_globalCfm;
info2.m_damping = infoGlobal.m_damping;
info2.m_numIterations = infoGlobal.m_numIterations;
constraint.getInfo2( info2 );
///finalize the constraint setup
for( j = 0; j < infoConstraintRows; j++ )
{
btSolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j];
if( solverConstraint.m_upperLimit >= constraint.getBreakingImpulseThreshold() )
{
solverConstraint.m_upperLimit = constraint.getBreakingImpulseThreshold();
}
if( solverConstraint.m_lowerLimit <= -constraint.getBreakingImpulseThreshold() )
{
solverConstraint.m_lowerLimit = -constraint.getBreakingImpulseThreshold();
}
solverConstraint.m_originalContactPoint = constraint;
btVector3 tmp;
{
//solverConstraint.m_angularComponentA = constraint.getRigidBodyA().m_invInertiaTensorWorld
// *solverConstraint.m_relpos1CrossNormal * constraint.getRigidBodyA().getAngularFactor();
constraint.m_rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
tmp.Mult( ref constraint.m_rbA.m_angularFactor, out solverConstraint.m_angularComponentA );
}
{
//solverConstraint.m_angularComponentB = constraint.getRigidBodyB().m_invInertiaTensorWorld
// * solverConstraint.m_relpos2CrossNormal * constraint.getRigidBodyB().getAngularFactor();
constraint.m_rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
tmp.Mult( ref constraint.m_rbB.m_angularFactor, out solverConstraint.m_angularComponentB );
}
{
btVector3 iMJlA; solverConstraint.m_contactNormal1.Mult( rbA.m_inverseMass, out iMJlA );
btVector3 iMJaA; rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out iMJaA );
btVector3 iMJlB; solverConstraint.m_contactNormal2.Mult( rbB.m_inverseMass, out iMJlB );//sign of normal?
btVector3 iMJaB; rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out iMJaB );
double sum = iMJlA.dot( ref solverConstraint.m_contactNormal1 );
sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal );
sum += iMJlB.dot( ref solverConstraint.m_contactNormal2 );
sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal );
double fsum = btScalar.btFabs( sum );
Debug.Assert( fsum > btScalar.SIMD_EPSILON );
btScalar.Dbg( "m_jacDiagABInv 4 set to " + ( fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0 ).ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0;
}
{
double rel_vel;
btVector3 externalForceImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalTorqueImpulse : btVector3.Zero;
btVector3 externalForceImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalTorqueImpulse : btVector3.Zero;
btScalar.Dbg( "external torque2 impulses " + externalTorqueImpulseA + externalTorqueImpulseB );
double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref rbA.m_linearVelocity, ref externalForceImpulseA )
+ solverConstraint.m_relpos1CrossNormal.dotAdded( ref rbA.m_angularVelocity, ref externalTorqueImpulseA );
double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref rbB.m_linearVelocity, ref externalForceImpulseB )
+ solverConstraint.m_relpos2CrossNormal.dotAdded( ref rbB.m_angularVelocity, ref externalTorqueImpulseB );
rel_vel = vel1Dotn + vel2Dotn;
double restitution = 0;
double positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
double velocityError = restitution - rel_vel * info2.m_damping;
double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
btScalar.Dbg( "Constraint 5 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_appliedImpulse = 0;
}
}
}
currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows;
}
}
btScalar.Dbg( "About to convert contacts " + start_manifold + " " + numManifolds );
convertContacts( manifoldPtr, start_manifold, numManifolds, infoGlobal );
}
// btContactSolverInfo info = infoGlobal;
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderNonContactConstraintPool.Capacity = ( numNonContactPool );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool * 2 );
else
m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool );
m_orderFrictionConstraintPool.Count = m_orderFrictionConstraintPool.Capacity = ( numFrictionPool );
{
int i;
for( i = 0; i < numNonContactPool; i++ )
{
m_orderNonContactConstraintPool[i] = i;
}
for( i = 0; i < numConstraintPool; i++ )
{
m_orderTmpConstraintPool[i] = i;
}
for( i = 0; i < numFrictionPool; i++ )
{
m_orderFrictionConstraintPool[i] = i;
}
}
return 0;
}
internal InplaceSolverIslandCallback(
btConstraintSolver solver,
btDispatcher dispatcher )
{
m_solverInfo = null;
m_solver = ( solver );
m_sortedConstraints = null;
m_numConstraints = ( 0 );
m_debugDrawer = null;
m_dispatcher = ( dispatcher );
}
internal static void getClosestPoints( btBoxShape m_box1, btBoxShape m_box2
, btDiscreteCollisionDetectorInterface.ClosestPointInput input
, btDiscreteCollisionDetectorInterface.Result output
, btIDebugDraw debugDraw, bool swapResults = false )
{
//btTransform transformA; input.m_transformA.Get( out transformA );
//btTransform transformB; input.m_transformB.Get( out transformB );
int skip = 0;
//dContactGeom* contact = 0;
double[] R1 = new double[12];
double[] R2 = new double[12];
//dMatrix3 R1;
//dMatrix3 R2;
for( int j = 0; j < 3; j++ )
{
R1[0 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformA.m_basis, j,0 );
R2[0 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformB.m_basis, j, 0 );
R1[1 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformA.m_basis,j,1);
R2[1 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformB.m_basis,j,1 );
R1[2 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformA.m_basis,j,2 );
R2[2 + 4 * j] = btMatrix3x3.getValue( ref input.m_transformB.m_basis,j,2 );
}
//btVector3 normal;
//double depth;
//int return_code;
int maxc = 4;
btVector3 half1, half2;
m_box1.getHalfExtentsWithMargin( out half1 );
half1.Mult( btScalar.BT_TWO, out half1 );
m_box2.getHalfExtentsWithMargin( out half2 );
half2.Mult( btScalar.BT_TWO, out half2 );
dBoxBox2( ref input.m_transformA.m_origin,
R1,
ref half1,
ref input.m_transformB.m_origin,
R2,
ref half2,
//out normal, out depth, out return_code,
maxc, skip,
output
);
}
///solve a group of constraints internal abstract double solveGroup( btCollisionObject[] bodies, int numBodies , btPersistentManifold[] manifold, int first_manifold, int numManifolds , btTypedConstraint[] constraints, int startConstraint, int numConstraints , btContactSolverInfo info , btIDebugDraw debugDrawer , btDispatcher dispatcher );
public DebugDrawcallback( btIDebugDraw debugDrawer, ref btTransform worldTrans, ref btVector3 color )
{
m_debugDrawer = ( debugDrawer );
m_color = ( color );
m_worldTrans = ( worldTrans );
}
//void updateSingleAabb( btCollisionObject colObj );
public virtual void setDebugDrawer( btIDebugDraw debugDrawer )
{
m_debugDrawer = debugDrawer;
}
internal void getClosestPointsNonVirtual( btDiscreteCollisionDetectorInterface.ClosestPointInput input
, btDiscreteCollisionDetectorInterface.Result output, btIDebugDraw debugDraw )
#endif
{
m_cachedSeparatingDistance = 0;
double distance = btScalar.BT_ZERO;
btVector3 normalInB = btVector3.Zero;
btVector3 pointOnA, pointOnB = btVector3.Zero;
btTransform localTransA; input.m_transformA.Get( out localTransA );
btTransform localTransB; input.m_transformB.Get( out localTransB );
btVector3 positionOffset; localTransA.m_origin.Add( ref localTransB.m_origin, out positionOffset );
positionOffset.Mult( (double)( 0.5 ), out positionOffset );
localTransA.m_origin.Sub( ref positionOffset, out localTransA.m_origin );
localTransB.m_origin.Sub( ref positionOffset, out localTransB.m_origin );
bool check2d = m_minkowskiA.isConvex2d() && m_minkowskiB.isConvex2d();
double marginA = m_marginA;
double marginB = m_marginB;
gNumGjkChecks++;
//for CCD we don't use margins
if( m_ignoreMargin )
{
marginA = btScalar.BT_ZERO;
marginB = btScalar.BT_ZERO;
}
m_curIter = 0;
int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN?
m_cachedSeparatingAxis.setValue( 0, 1, 0 );
bool isValid = false;
bool checkSimplex = false;
bool checkPenetration = true;
m_degenerateSimplex = 0;
m_lastUsedMethod = -1;
{
double squaredDistance = btScalar.BT_LARGE_FLOAT;
double delta = btScalar.BT_ZERO;
double margin = marginA + marginB;
m_simplexSolver.reset();
for( ;;)
//while (true)
{
btVector3 tmp;
m_cachedSeparatingAxis.Invert( out tmp );
btVector3 seperatingAxisInA; localTransA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
btVector3 seperatingAxisInB; localTransA.m_basis.ApplyInverse( ref m_cachedSeparatingAxis, out seperatingAxisInB );
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btScalar.Dbg( "pWorld is " + pWorld + " qWorld is " + qWorld );
if( check2d )
{
pWorld[2] = 0;
qWorld[2] = 0;
}
btVector3 w; pWorld.Sub( ref qWorld, out w );
delta = m_cachedSeparatingAxis.dot( ref w );
// potential exit, they don't overlap
if( ( delta > (double)( 0.0 ) ) && ( delta * delta > squaredDistance * input.m_maximumDistanceSquared ) )
{
m_degenerateSimplex = 10;
checkSimplex = true;
//checkPenetration = false;
break;
}
//exit 0: the new point is already in the simplex, or we didn't come any closer
if( m_simplexSolver.inSimplex( ref w ) )
{
m_degenerateSimplex = 1;
checkSimplex = true;
break;
}
// are we getting any closer ?
double f0 = squaredDistance - delta;
double f1 = squaredDistance * REL_ERROR2;
btScalar.Dbg( "f0 is " + f0.ToString( "g17" ) + " f1 is " + f1.ToString( "g17" ) );
if( f0 <= f1 )
{
if( f0 <= btScalar.BT_ZERO )
{
m_degenerateSimplex = 2;
}
else
{
m_degenerateSimplex = 11;
}
checkSimplex = true;
break;
}
//add current vertex to simplex
m_simplexSolver.addVertex( ref w, ref pWorld, ref qWorld );
btVector3 newCachedSeparatingAxis;
//calculate the closest point to the origin (update vector v)
if( !m_simplexSolver.closest( out newCachedSeparatingAxis ) )
{
m_degenerateSimplex = 3;
checkSimplex = true;
break;
}
if( newCachedSeparatingAxis.length2() < REL_ERROR2 )
{
m_cachedSeparatingAxis = newCachedSeparatingAxis;
m_degenerateSimplex = 6;
checkSimplex = true;
break;
}
double previousSquaredDistance = squaredDistance;
squaredDistance = newCachedSeparatingAxis.length2();
#if asdfasdf
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
if (squaredDistance>previousSquaredDistance)
{
m_degenerateSimplex = 7;
squaredDistance = previousSquaredDistance;
checkSimplex = false;
break;
}
#endif //
//redundant m_simplexSolver.compute_points(pointOnA, pointOnB);
//are we getting any closer ?
if( previousSquaredDistance - squaredDistance <= btScalar.SIMD_EPSILON * previousSquaredDistance )
{
// m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
checkSimplex = true;
m_degenerateSimplex = 12;
break;
}
m_cachedSeparatingAxis = newCachedSeparatingAxis;
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
if( m_curIter++ > gGjkMaxIter )
{
#if DEBUG
Console.WriteLine( "btGjkPairDetector maxIter exceeded:{0}", m_curIter );
Console.WriteLine( "sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4},shapeTypeB={5}\n",
m_cachedSeparatingAxis.x,
m_cachedSeparatingAxis.y,
m_cachedSeparatingAxis.z,
squaredDistance,
m_minkowskiA.getShapeType(),
m_minkowskiB.getShapeType() );
#endif
break;
}
bool check = ( !m_simplexSolver.fullSimplex() );
//bool check = (!m_simplexSolver.fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver.maxVertex());
if( !check )
{
//do we need this backup_closest here ?
// m_simplexSolver.backup_closest(m_cachedSeparatingAxis);
m_degenerateSimplex = 13;
break;
}
}
if( checkSimplex )
{
m_simplexSolver.compute_points( out pointOnA, out pointOnB );
btScalar.Dbg( "new simplex points " + pointOnA.ToString() + " and " + pointOnB );
normalInB = m_cachedSeparatingAxis;
double lenSqr = m_cachedSeparatingAxis.length2();
//valid normal
if( lenSqr < 0.0001 )
{
m_degenerateSimplex = 5;
}
if( lenSqr > btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON )
{
double rlen = btScalar.BT_ONE / btScalar.btSqrt( lenSqr );
normalInB.Mult( rlen, out normalInB );
//normalInB *= rlen; //normalize
double s = btScalar.btSqrt( squaredDistance );
Debug.Assert( s > (double)( 0.0 ) );
pointOnA.SubScale( ref m_cachedSeparatingAxis, ( marginA / s ), out pointOnA );
pointOnB.AddScale( ref m_cachedSeparatingAxis, ( marginB / s ), out pointOnB );
//pointOnA -= m_cachedSeparatingAxis * ( marginA / s );
//pointOnB += m_cachedSeparatingAxis * ( marginB / s );
distance = ( ( btScalar.BT_ONE / rlen ) - margin );
isValid = true;
m_lastUsedMethod = 1;
}
else
{
m_lastUsedMethod = 2;
}
}
bool catchDegeneratePenetrationCase =
( m_catchDegeneracies != 0
&& m_penetrationDepthSolver != null
&& m_degenerateSimplex != 0
&& ( ( distance + margin ) < 0.01 ) );
//if (checkPenetration && !isValid)
if( checkPenetration && ( !isValid || catchDegeneratePenetrationCase ) )
{
//penetration case
//if there is no way to handle penetrations, bail out
if( m_penetrationDepthSolver != null )
{
// Penetration depth case.
btVector3 tmpPointOnA, tmpPointOnB;
gNumDeepPenetrationChecks++;
m_cachedSeparatingAxis.setZero();
bool isValid2 = m_penetrationDepthSolver.calcPenDepth(
m_simplexSolver,
m_minkowskiA, m_minkowskiB,
ref localTransA, ref localTransB,
ref m_cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
debugDraw
);
btScalar.Dbg( "points are " + tmpPointOnA.ToString() + " and " + tmpPointOnB.ToString() );
if( isValid2 )
{
btVector3 tmpNormalInB; tmpPointOnB.Sub( ref tmpPointOnA, out tmpNormalInB );
double lenSqr = tmpNormalInB.length2();
if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
tmpNormalInB = m_cachedSeparatingAxis;
lenSqr = m_cachedSeparatingAxis.length2();
}
if( lenSqr > ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
tmpNormalInB.Mult( btScalar.btSqrt( lenSqr ), out tmpNormalInB );
btVector3 tmp;
tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
double distance2 = -tmp.length();
m_lastUsedMethod = 3;
//only replace valid penetrations when the result is deeper (check)
if( !isValid || ( distance2 < distance ) )
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
normalInB = tmpNormalInB;
///todo: need to track down this EPA penetration solver degeneracy
///the penetration solver reports penetration but the contact normal
///connecting the contact points is pointing in the opposite direction
///until then, detect the issue and revert the normal
{
btScalar d1 = 0;
{
normalInB.Invert( out tmp );
btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInA );
btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref tmp, out seperatingAxisInB );
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btVector3 w; pWorld.Sub( ref qWorld, out w );
d1 = ( tmp ).dot( w );
}
btScalar d0 = btScalar.BT_ZERO;
{
normalInB.Invert( out tmp );
btVector3 seperatingAxisInA; input.m_transformA.m_basis.ApplyInverse( ref tmp, out seperatingAxisInA );
btVector3 seperatingAxisInB; input.m_transformB.m_basis.ApplyInverse( ref normalInB, out seperatingAxisInB ) ;
btVector3 pInA; m_minkowskiA.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInA, out pInA );
btVector3 qInB; m_minkowskiB.localGetSupportVertexWithoutMarginNonVirtual( ref seperatingAxisInB, out qInB );
btVector3 pWorld; localTransA.Apply( ref pInA, out pWorld );
btVector3 qWorld; localTransB.Apply( ref qInB, out qWorld );
btVector3 w; pWorld.Sub( ref qWorld, out w );
d0 = normalInB.dot( w );
}
if( d1 > d0 )
{
m_lastUsedMethod = 10;
normalInB *= -1;
}
}
isValid = true;
}
else
{
m_lastUsedMethod = 8;
}
}
else
{
m_lastUsedMethod = 9;
}
}
else
{
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
///reports a valid positive distance. Use the results of the second GJK instead of failing.
///thanks to Jacob.Langford for the reproduction case
///http://code.google.com/p/bullet/issues/detail?id=250
if( m_cachedSeparatingAxis.length2() > btScalar.BT_ZERO )
{
btVector3 tmp;
tmpPointOnA.Sub( ref tmpPointOnB, out tmp );
double distance2 = tmp.length() - margin;
//only replace valid distances when the distance is less
btScalar.Dbg( "old distance " + distance2.ToString( "g17" ) + " new distance " + distance.ToString( "g17" ) );
if( !isValid || ( distance2 < distance ) )
{
distance = distance2;
pointOnA = tmpPointOnA;
pointOnB = tmpPointOnB;
pointOnA.SubScale( ref m_cachedSeparatingAxis, marginA, out pointOnA );
pointOnB.AddScale( ref m_cachedSeparatingAxis, marginB, out pointOnA );
//pointOnA -= m_cachedSeparatingAxis * marginA;
//pointOnB += m_cachedSeparatingAxis * marginB;
normalInB = m_cachedSeparatingAxis;
normalInB.normalize();
isValid = true;
m_lastUsedMethod = 6;
}
else
{
m_lastUsedMethod = 5;
}
}
}
}
}
}
btScalar.Dbg( "Pair detector : valid=" + (isValid?"1":"0" )+ " distance=" + distance.ToString( "g17" ) + " maxDistance=" + input.m_maximumDistanceSquared.ToString( "g17" ) );
if( isValid && ( ( distance < 0 ) || ( distance * distance < input.m_maximumDistanceSquared ) ) )
{
m_cachedSeparatingAxis = normalInB;
m_cachedSeparatingDistance = distance;
btVector3 tmp;
pointOnB.Add( ref positionOffset, out tmp );
output.addContactPoint(
ref normalInB,
ref tmp,
distance );
}
}
internal override void getClosestPoints( btDiscreteCollisionDetectorInterface.ClosestPointInput input
, btDiscreteCollisionDetectorInterface.Result output
, btIDebugDraw debugDraw, bool swapResults = false )
{
//(void)swapResults;
getClosestPointsNonVirtual( input, output, debugDraw );
}
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;
}
}
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
internal override double solveGroup( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal
, btIDebugDraw debugDrawer, btDispatcher dispatcher )
{
CProfileSample sample = new CProfileSample( "solveGroup" );
//you need to provide at least some bodies
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
solveGroupCacheFriendlyIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
solveGroupCacheFriendlyFinish( bodies, numBodies, infoGlobal );
return 0;
}
internal override void allSolved( btContactSolverInfo info, btIDebugDraw debugDrawer ) {; }
internal abstract void allSolved( btContactSolverInfo info, btIDebugDraw debugDrawer );
///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 );
internal void Initialize( btManifoldResult originalResult
, ref btTransform transformA, ref btTransform transformB
, ref btTransform unPerturbedTransform, bool perturbA, btIDebugDraw debugDrawer )
{
m_originalManifoldResult = ( originalResult );
m_transformA = ( transformA );
m_transformB = ( transformB );
m_unPerturbedTransform = ( unPerturbedTransform );
m_perturbA = ( perturbA );
m_debugDrawer = ( debugDrawer );
}
public void setup( btContactSolverInfo solverInfo
, btTypedConstraint[] sortedConstraints, int numConstraints
, btIDebugDraw debugDrawer )
{
Debug.Assert( solverInfo != null );
m_solverInfo = solverInfo;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.Count = ( 0 );
m_manifolds.Count = ( 0 );
m_constraints.Count = ( 0 );
}
// // give either closest points (distance > 0) or penetration (distance) // the normal always points from B towards A // internal abstract void getClosestPoints( ClosestPointInput input, Result output, btIDebugDraw debugDraw , bool swapResults = false );
