//btSphereSphereCollisionAlgorithm( btCollisionAlgorithmConstructionInfo ci )
// : btActivatingCollisionAlgorithm( ci) { }
internal void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper col0Wrap, btCollisionObjectWrapper col1Wrap )
{
base.Initialize( ci, col0Wrap, col1Wrap );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
if( m_manifoldPtr == null )
{
m_manifoldPtr = m_dispatcher.getNewManifold( col0Wrap.m_collisionObject, col1Wrap.m_collisionObject );
m_ownManifold = true;
}
}
void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci
, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
base.Initialize( ci, body0Wrap, body1Wrap );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
if( m_manifoldPtr == null && m_dispatcher.needsCollision( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject ) )
{
m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject );
m_ownManifold = true;
}
}
public void Initialize( btDispatcher dispatcher, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, bool isSwapped )
{
m_dispatcher = ( dispatcher );
m_dispatchInfoPtr = ( null );
m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap;
m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap;
//
// create the manifold from the dispatcher 'manifold pool'
//
m_manifoldPtr = m_dispatcher.getNewManifold( m_convexBodyWrap.m_collisionObject, m_triBodyWrap.m_collisionObject );
clearCache();
}
internal void Initialize( btPersistentManifold mf, btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper col0Wrap, btCollisionObjectWrapper col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold )
{
base.Initialize( ci );
m_ownManifold = ( false );
m_manifoldPtr = ( mf );
m_swapped = ( isSwapped );
m_numPerturbationIterations = ( numPerturbationIterations );
m_minimumPointsPerturbationThreshold = ( minimumPointsPerturbationThreshold );
btCollisionObjectWrapper convexObjWrap = m_swapped ? col1Wrap : col0Wrap;
btCollisionObjectWrapper planeObjWrap = m_swapped ? col0Wrap : col1Wrap;
if( m_manifoldPtr == null && m_dispatcher.needsCollision( convexObjWrap.m_collisionObject, planeObjWrap.m_collisionObject ) )
{
m_manifoldPtr = m_dispatcher.getNewManifold( convexObjWrap.m_collisionObject, planeObjWrap.m_collisionObject );
m_ownManifold = true;
}
}
protected void convertContacts( btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds, btContactSolverInfo infoGlobal )
{
int i;
btPersistentManifold manifold;
// btCollisionObject colObj0=0,*colObj1=0;
for( i = 0; i < numManifolds; i++ )
{
manifold = manifoldPtr[i + start_manifold];
convertContact( manifold, infoGlobal );
}
}
public void setPersistentManifold( btPersistentManifold manifoldPtr )
{
m_manifoldPtr = manifoldPtr;
}
internal void Initialize( btCollisionObjectWrapper compoundObjWrap, btCollisionObjectWrapper otherObjWrap, btDispatcher dispatcher, btDispatcherInfo dispatchInfo, btManifoldResult resultOut, btCollisionAlgorithm[] childCollisionAlgorithms, btPersistentManifold sharedManifold )
{
m_compoundColObjWrap = ( compoundObjWrap );
m_otherObjWrap = ( otherObjWrap );
m_dispatcher = ( dispatcher );
m_dispatchInfo = ( dispatchInfo );
m_resultOut = ( resultOut );
m_childCollisionAlgorithms = ( childCollisionAlgorithms );
m_sharedManifold = ( sharedManifold );
}
internal abstract void releaseManifold( btPersistentManifold manifold );
internal override void processIsland( btCollisionObject[] bodies, int numBodies, btPersistentManifold[] manifolds, int first_manifold, int numManifolds, int islandId )
{
if( islandId < 0 )
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver.solveGroup( bodies, numBodies, manifolds, first_manifold, numManifolds, m_sortedConstraints, 0, m_numConstraints, m_solverInfo, m_debugDrawer, m_dispatcher );
}
else
{
//also add all non-contact constraints/joints for this island
int startConstraint = 0;
int numCurConstraints = 0;
int i;
//find the first constraint for this island
for( i = 0; i < m_numConstraints; i++ )
{
if( btDiscreteDynamicsWorld.btGetConstraintIslandId( m_sortedConstraints[i] ) == islandId )
{
startConstraint = i;
break;
}
}
//count the number of constraints in this island
for( ; i < m_numConstraints; i++ )
{
if( btDiscreteDynamicsWorld.btGetConstraintIslandId( m_sortedConstraints[i] ) == islandId )
{
numCurConstraints++;
}
}
if( m_solverInfo.m_minimumSolverBatchSize <= 1 )
{
m_solver.solveGroup( bodies, numBodies, manifolds, first_manifold, numManifolds, m_sortedConstraints, startConstraint, numCurConstraints, m_solverInfo, m_debugDrawer, m_dispatcher );
}
else
{
for( i = 0; i < numBodies; i++ )
m_bodies.Add( bodies[i] );
for( i = 0; i < numManifolds; i++ )
m_manifolds.Add( manifolds[first_manifold + i] );
for( i = 0; i < numCurConstraints; i++ )
m_constraints.Add( m_sortedConstraints[startConstraint + i] );
if( ( m_constraints.Count + m_manifolds.Count ) > m_solverInfo.m_minimumSolverBatchSize )
{
processConstraints();
}
else
{
//Console.WriteLine("deferred\n");
}
}
}
}
/// 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;
}
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 );
}
}
}
}
}
}
internal override btCollisionAlgorithm findAlgorithm( btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, btPersistentManifold sharedManifold = null )
{
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = this;
ci.m_manifold = sharedManifold;
btCollisionAlgorithm algo = m_doubleDispatch[(int)body0Wrap.getCollisionShape().getShapeType(), (int)body1Wrap.getCollisionShape().getShapeType()].CreateCollisionAlgorithm( ci, body0Wrap, body1Wrap );
return algo;
}
internal override void releaseManifold( btPersistentManifold manifold )
{
gNumManifold--;
//Console.WriteLine("releaseManifold: gNumManifold %d\n",gNumManifold);
clearManifold( manifold );
btScalar.Dbg( "Remove Manifold from dispatcher" );
m_manifoldsPtr.Remove( manifold );
BulletGlobals.PersistentManifoldPool.Free( manifold );
}
internal override void clearManifold( btPersistentManifold manifold )
{
manifold.clearManifold();
}
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;
}
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;
}
///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 );
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;
}
internal btCompoundCompoundLeafCallback( btCollisionObjectWrapper compound1ObjWrap,
btCollisionObjectWrapper compound0ObjWrap,
btDispatcher dispatcher,
btDispatcherInfo dispatchInfo,
btManifoldResult resultOut,
btHashedSimplePairCache childAlgorithmsCache,
btPersistentManifold sharedManifold )
{
m_numOverlapPairs = ( 0 );
m_compound0ColObjWrap = ( compound1ObjWrap );
m_compound1ColObjWrap = ( compound0ObjWrap );
m_dispatcher = ( dispatcher );
m_dispatchInfo = ( dispatchInfo );
m_resultOut = ( resultOut );
m_childCollisionAlgorithmCache = ( childAlgorithmsCache );
m_sharedManifold = ( sharedManifold );
}
public void convertContact( btPersistentManifold manifold, btContactSolverInfo infoGlobal )
{
btCollisionObject colObj0, colObj1;
colObj0 = manifold.m_body0;
colObj1 = manifold.m_body1;
//int solverBodyIdA = ;
//int solverBodyIdB = ;
// btRigidBody bodyA = btRigidBody::upcast(colObj0);
// btRigidBody bodyB = btRigidBody::upcast(colObj1);
btSolverBody solverBodyA = getOrInitSolverBody( colObj0, infoGlobal.m_timeStep );
btSolverBody solverBodyB = getOrInitSolverBody( colObj1, infoGlobal.m_timeStep );
///avoid collision response between two static objects
if( solverBodyA == null || ( solverBodyA.m_invMass.fuzzyZero() && ( solverBodyB == null || solverBodyB.m_invMass.fuzzyZero() ) ) )
return;
int rollingFriction = 1;
btScalar.Dbg( DbgFlag.Manifolds, "Manifold cache points " + manifold.m_cachedPoints );
for( int j = 0; j < manifold.m_cachedPoints; j++ )
{
btManifoldPoint cp = manifold.getContactPoint( j );
if( cp.m_distance1 <= manifold.m_contactProcessingThreshold )
{
btVector3 rel_pos1;
btVector3 rel_pos2;
double relaxation;
int frictionIndex = m_tmpSolverContactConstraintPool.Count;
btSolverConstraint solverConstraint = BulletGlobals.SolverConstraintPool.Get();
m_tmpSolverContactConstraintPool.Add( solverConstraint );
btRigidBody rb0 = btRigidBody.upcast( colObj0 );
btRigidBody rb1 = btRigidBody.upcast( colObj1 );
solverConstraint.m_solverBodyA = solverBodyA;
solverConstraint.m_solverBodyB = solverBodyB;
solverConstraint.m_originalContactPoint = cp;
//btVector3 pos1 = cp.m_positionWorldOnA;
//btVector3 pos2 = cp.m_positionWorldOnB;
cp.m_positionWorldOnA.Sub( ref colObj0.m_worldTransform.m_origin, out rel_pos1 );
cp.m_positionWorldOnB.Sub( ref colObj1.m_worldTransform.m_origin, out rel_pos2 );
btVector3 vel1;// = rb0 ? rb0.getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2;// = rb1 ? rb1.getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
solverBodyA.getVelocityInLocalPointNoDelta( ref rel_pos1, out vel1 );
solverBodyB.getVelocityInLocalPointNoDelta( ref rel_pos2, out vel2 );
btVector3 vel; vel1.Sub( ref vel2, out vel );
double rel_vel = cp.m_normalWorldOnB.dot( vel );
setupContactConstraint( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal, out relaxation, ref rel_pos1, ref rel_pos2 );
/////setup the friction constraints
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.Count;
btVector3 relAngVel;
if( rb0 != null && rb1 != null )
rb1.m_angularVelocity.Sub( ref rb0.m_angularVelocity, out relAngVel );
else if( rb0 != null )
rb0.m_angularVelocity.Invert( out relAngVel );
else if( rb1 != null )
relAngVel = rb1.m_angularVelocity;
else
relAngVel = btVector3.Zero;
if( ( cp.m_combinedRollingFriction > 0 ) && ( rollingFriction > 0 ) )
{
//only a single rollingFriction per manifold
rollingFriction--;
if( relAngVel.length() > infoGlobal.m_singleAxisRollingFrictionThreshold )
{
relAngVel.normalize();
applyAnisotropicFriction( colObj0, ref relAngVel, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj1, ref relAngVel, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
if( relAngVel.length() > 0.001 )
addRollingFrictionConstraint( ref relAngVel, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
else
{
addRollingFrictionConstraint( ref cp.m_normalWorldOnB, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
btVector3 axis0, axis1;
btVector3.btPlaneSpace1( ref cp.m_normalWorldOnB, out axis0, out axis1 );
applyAnisotropicFriction( colObj0, ref axis0, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj1, ref axis0, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj0, ref axis1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
applyAnisotropicFriction( colObj1, ref axis1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_ROLLING_FRICTION );
if( axis0.length() > 0.001 )
addRollingFrictionConstraint( ref axis0, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
if( axis1.length() > 0.001 )
addRollingFrictionConstraint( ref axis1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
}
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically very frame
///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
///
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
///
///The user can manually override the friction directions for certain contacts using a contact callback,
///and set the cp.m_lateralFrictionInitialized to true
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
if( ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) == 0 )
|| !cp.m_lateralFrictionInitialized )
{
vel.AddScale( ref cp.m_normalWorldOnB, -rel_vel, out cp.m_lateralFrictionDir1 );
double lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if( ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) == 0 ) && lat_rel_vel > btScalar.SIMD_EPSILON )
{
cp.m_lateralFrictionDir1.Mult( 1 / btScalar.btSqrt( lat_rel_vel ), out cp.m_lateralFrictionDir1 );
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
cp.m_lateralFrictionDir1.cross( ref cp.m_normalWorldOnB, out cp.m_lateralFrictionDir2 );
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
}
else
{
btVector3.btPlaneSpace1( ref cp.m_normalWorldOnB, out cp.m_lateralFrictionDir1, out cp.m_lateralFrictionDir2 );
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir1, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
applyAnisotropicFriction( colObj0, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
applyAnisotropicFriction( colObj1, ref cp.m_lateralFrictionDir2, btCollisionObject.AnisotropicFrictionFlags.CF_ANISOTROPIC_FRICTION );
addFrictionConstraint( ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation );
}
if( ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
&& ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) != 0 ) )
{
cp.m_lateralFrictionInitialized = true;
}
}
}
else
{
addFrictionConstraint( ref cp.m_lateralFrictionDir1, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1 );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
addFrictionConstraint( ref cp.m_lateralFrictionDir2, solverBodyA, solverBodyB, frictionIndex, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2 );
}
setFrictionConstraintImpulse( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal );
}
}
}
///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 abstract btCollisionAlgorithm findAlgorithm( btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, btPersistentManifold sharedManifold );
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 abstract void clearManifold( btPersistentManifold manifold );
//
// Convex-Convex collision algorithm
//
internal override void processCollision( btCollisionObjectWrapper body0Wrap
, ref btTransform body0Transform
, btCollisionObjectWrapper body1Wrap
, ref btTransform body1Transform
, btDispatcherInfo dispatchInfo, btManifoldResult resultOut )
{
if( m_manifoldPtr == null )
{
//swapped?
m_manifoldPtr = m_dispatcher.getNewManifold( body0Wrap.m_collisionObject, body1Wrap.m_collisionObject );
m_ownManifold = true;
}
resultOut.setPersistentManifold( m_manifoldPtr );
//comment-out next line to test multi-contact generation
//resultOut.getPersistentManifold().clearManifold();
btConvexShape min0 = (btConvexShape)body0Wrap.getCollisionShape();
btConvexShape min1 = (btConvexShape)body1Wrap.getCollisionShape();
btVector3 normalOnB;
btVector3 pointOnBWorld;
#if !BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if( ( min0.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE )
&& ( min1.getShapeType() == BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE ) )
{
btCapsuleShape capsuleA = (btCapsuleShape)min0;
btCapsuleShape capsuleB = (btCapsuleShape)min1;
// btVector3 localScalingA = capsuleA.getLocalScaling();
// btVector3 localScalingB = capsuleB.getLocalScaling();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double dist = capsuleCapsuleDistance( out normalOnB, out pointOnBWorld
, capsuleA.getHalfHeight(), capsuleA.getRadius()
, capsuleB.getHalfHeight(), capsuleB.getRadius()
, capsuleA.getUpAxis(), capsuleB.getUpAxis()
, ref body0Wrap.m_collisionObject.m_worldTransform, ref body1Wrap.m_collisionObject.m_worldTransform, threshold );
if( dist < threshold )
{
Debug.Assert( normalOnB.length2() >= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) );
resultOut.addContactPoint( ref normalOnB, ref pointOnBWorld, dist );
}
resultOut.refreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
m_sepDistance.updateSeparatingDistance(body0.getWorldTransform(),body1.getWorldTransform());
}
if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0)
#endif //USE_SEPDISTANCE_UTIL2
{
btGjkPairDetector.ClosestPointInput input = BulletGlobals.ClosestPointInputPool.Get();
input.Initialize();
btGjkPairDetector gjkPairDetector = BulletGlobals.GjkPairDetectorPool.Get();
gjkPairDetector.Initialize( min0, min1, m_simplexSolver, m_pdSolver );
//TODO: if (dispatchInfo.m_useContinuous)
gjkPairDetector.setMinkowskiA( min0 );
gjkPairDetector.setMinkowskiB( min1 );
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
} else
#endif //USE_SEPDISTANCE_UTIL2
{
//if (dispatchInfo.m_convexMaxDistanceUseCPT)
//{
// input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactProcessingThreshold();
//} else
//{
input.m_maximumDistanceSquared = min0.getMargin() + min1.getMargin() + m_manifoldPtr.getContactBreakingThreshold();
// }
input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
}
input.m_transformA = body0Transform;
input.m_transformB = body1Transform;
#if USE_SEPDISTANCE_UTIL2
double sepDist = 0;
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
sepDist = gjkPairDetector.getCachedSeparatingDistance();
if (sepDist>SIMD_EPSILON)
{
sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
//now perturbe directions to get multiple contact points
}
}
#endif //USE_SEPDISTANCE_UTIL2
if( min0.isPolyhedral() && min1.isPolyhedral() )
{
btDummyResult dummy = new btDummyResult();
///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
double min0Margin = min0.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min0.getMargin();
double min1Margin = min1.getShapeType() == BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE ? 0 : min1.getMargin();
btWithoutMarginResult withoutMargin = new btWithoutMarginResult( resultOut, min0Margin, min1Margin );
btPolyhedralConvexShape polyhedronA = (btPolyhedralConvexShape)min0;
btPolyhedralConvexShape polyhedronB = (btPolyhedralConvexShape)min1;
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getConvexPolyhedron() != null )
{
double threshold = m_manifoldPtr.getContactBreakingThreshold();
double minDist = -1e30f;
btVector3 sepNormalWorldSpace;
bool foundSepAxis = true;
if( dispatchInfo.m_enableSatConvex )
{
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, withoutMargin, dispatchInfo.m_debugDraw );
//gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
//double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
//if (l2>SIMD_EPSILON)
{
sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1/l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0.getMargin()+min1.getMargin();
#if ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0;
#else
foundSepAxis = withoutMargin.m_foundResult && minDist < 0;//-(min0.getMargin()+min1.getMargin());
#endif
}
}
if( foundSepAxis )
{
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipHullAgainstHull( ref sepNormalWorldSpace, polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform
, minDist - threshold, threshold, resultOut );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
else
{
//we can also deal with convex versus triangle (without connectivity data)
if( polyhedronA.getConvexPolyhedron() != null && polyhedronB.getShapeType() == BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE )
{
btVertexArray vertices = new btVertexArray();
btTriangleShape tri = (btTriangleShape)polyhedronB;
btVector3 tmp;
body1Transform.Apply( ref tri.m_vertices1, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices2, out tmp );
vertices.Add( ref tmp );
body1Transform.Apply( ref tri.m_vertices3, out tmp );
vertices.Add( ref tmp );
//tri.initializePolyhedralFeatures();
double threshold = m_manifoldPtr.getContactBreakingThreshold();
btVector3 sepNormalWorldSpace;
double minDist = -btScalar.BT_LARGE_FLOAT;
double maxDist = threshold;
bool foundSepAxis = false;
if( false )
{
polyhedronB.initializePolyhedralFeatures();
foundSepAxis = btPolyhedralContactClipping.findSeparatingAxis(
polyhedronA.getConvexPolyhedron(), polyhedronB.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform,
ref body1Wrap.m_collisionObject.m_worldTransform,
out sepNormalWorldSpace, resultOut );
// Console.WriteLine("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z);
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
#if ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
gjkPairDetector.getClosestPoints( input, dummy, dispatchInfo.m_debugDraw );
#endif//ZERO_MARGIN
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
//minDist = gjkPairDetector.getCachedSeparatingDistance();
//maxDist = threshold;
minDist = gjkPairDetector.getCachedSeparatingDistance() - min0.getMargin() - min1.getMargin();
//foundSepAxis = true;
btPolyhedralContactClipping.clipFaceAgainstHull( ref sepNormalWorldSpace
, polyhedronA.getConvexPolyhedron(),
ref body0Wrap.m_collisionObject.m_worldTransform, vertices, minDist - threshold, maxDist, resultOut );
}
else
{
//sepNormalWorldSpace = btVector3.Zero;
}
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
return;
}
}
}
gjkPairDetector.getClosestPoints( input, resultOut, dispatchInfo.m_debugDraw );
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
if( m_numPerturbationIterations != 0
&& resultOut.m_manifoldPtr.m_cachedPoints < m_minimumPointsPerturbationThreshold )
{
int i;
btVector3 v0, v1;
btVector3 sepNormalWorldSpace;
double l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
if( l2 > btScalar.SIMD_EPSILON )
{
gjkPairDetector.getCachedSeparatingAxis().Mult( ( 1 / l2 ), out sepNormalWorldSpace );
btVector3.btPlaneSpace1( ref sepNormalWorldSpace, out v0, out v1 );
bool perturbeA = true;
double angleLimit = 0.125f * btScalar.SIMD_PI;
double perturbeAngle;
double radiusA = min0.getAngularMotionDisc();
double radiusB = min1.getAngularMotionDisc();
if( radiusA < radiusB )
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusA;
perturbeA = true;
}
else
{
perturbeAngle = btPersistentManifold.gContactBreakingThreshold / radiusB;
perturbeA = false;
}
if( perturbeAngle > angleLimit )
perturbeAngle = angleLimit;
btTransform unPerturbedTransform;
if( perturbeA )
{
unPerturbedTransform = input.m_transformA;
}
else
{
unPerturbedTransform = input.m_transformB;
}
for( i = 0; i < m_numPerturbationIterations; i++ )
{
if( v0.length2() > btScalar.SIMD_EPSILON )
{
btQuaternion perturbeRot = new btQuaternion( ref v0, perturbeAngle );
double iterationAngle = i * ( btScalar.SIMD_2_PI / (double)( m_numPerturbationIterations ) );
btQuaternion rotq = new btQuaternion( ref sepNormalWorldSpace, iterationAngle );
if( perturbeA )
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body0Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA.setBasis( ref m3 );
input.m_transformB = body1Transform;
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformA,10.0);
#endif //DEBUG_CONTACTS
}
else
{
btQuaternion tmpq;
btQuaternion tmpq2;
rotq.inverse( out tmpq );
btQuaternion.Mult( ref tmpq, ref perturbeRot, out tmpq2 );
btQuaternion.Mult( ref tmpq2, ref rotq, out tmpq );
btMatrix3x3 m = new btMatrix3x3( ref tmpq );
btMatrix3x3 m2; body1Transform.getBasis( out m2 );
btMatrix3x3 m3;
btMatrix3x3.Mult( ref m, ref m2, out m3 );
input.m_transformA = body0Transform;
input.m_transformB.setBasis( ref m3 );
#if DEBUG_CONTACTS
dispatchInfo.m_debugDraw.drawTransform(input.m_transformB,10.0);
#endif
}
btPerturbedContactResult perturbedResultOut = BulletGlobals.PerturbedContactResultPool.Get();
if( perturbeA )
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformA
, perturbeA
, dispatchInfo.m_debugDraw );
else
perturbedResultOut.Initialize( resultOut
, ref input.m_transformA, ref input.m_transformB
, ref input.m_transformB
, perturbeA
, dispatchInfo.m_debugDraw );
gjkPairDetector.getClosestPoints( input, perturbedResultOut, dispatchInfo.m_debugDraw );
BulletGlobals.PerturbedContactResultPool.Free( perturbedResultOut );
}
}
}
}
#if USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
{
m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0.getWorldTransform(),body1.getWorldTransform());
}
#endif //USE_SEPDISTANCE_UTIL2
BulletGlobals.ClosestPointInputPool.Free( input );
BulletGlobals.GjkPairDetectorPool.Free( gjkPairDetector );
}
if( m_ownManifold )
{
resultOut.refreshContactPoints();
}
}
internal void Initialize( btCollisionAlgorithmConstructionInfo ci, btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap, bool isSwapped )
{
base.Initialize( ci, body0Wrap, body1Wrap );
m_isSwapped = ( isSwapped );
m_sharedManifold = ( ci.m_manifold );
m_ownsManifold = false;
btCollisionObjectWrapper colObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
Debug.Assert( colObjWrap.getCollisionShape().isCompound() );
btCompoundShape compoundShape = (btCompoundShape)( colObjWrap.getCollisionShape() );
m_compoundShapeRevision = compoundShape.getUpdateRevision();
preallocateChildAlgorithms( body0Wrap, body1Wrap );
}
internal void Initialize( btCollisionObjectWrapper body0Wrap, btCollisionObjectWrapper body1Wrap )
{
m_manifoldPtr = null;
m_body0Wrap = ( body0Wrap );
m_body1Wrap = ( body1Wrap );
#if DEBUG_PART_INDEX
m_partId0 = (-1);
m_partId1 = (-1);
m_index0 = (-1);
m_index1 = (-1);
#endif //DEBUG_PART_INDEX
}