public virtual void predictUnconstraintMotion( double timeStep )
{
CProfileSample sample = new CProfileSample( "predictUnconstraintMotion" );
for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ )
{
btRigidBody body = m_nonStaticRigidBodies[i];
if( !body.isStaticOrKinematicObject() )
{
//don't integrate/update velocities here, it happens in the constraint solver
body.applyDamping( timeStep );
body.predictIntegratedTransform( timeStep, out body.m_interpolationWorldTransform );
}
}
}
internal void synchronizeMotionStates()
{
CProfileSample sample = new CProfileSample( "synchronizeMotionStates" );
if( m_synchronizeAllMotionStates )
{
//iterate over all collision objects
for( int i = 0; i < m_collisionObjects.Count; i++ )
{
btCollisionObject colObj = m_collisionObjects[i];
btRigidBody body = btRigidBody.upcast( colObj );
if( body != null )
synchronizeSingleMotionState( body );
}
}
else
{
//iterate over all active rigid bodies
for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ )
{
btRigidBody body = m_nonStaticRigidBodies[i];
if( body.isActive() )
synchronizeSingleMotionState( body );
}
}
}
internal void createPredictiveContacts( double timeStep )
{
CProfileSample sample = new CProfileSample( "createPredictiveContacts" );
{
CProfileSample sub_sample = new CProfileSample( "release predictive contact manifolds" );
for( int i = 0; i < m_predictiveManifolds.Count; i++ )
{
btPersistentManifold manifold = m_predictiveManifolds[i];
this.m_dispatcher1.releaseManifold( manifold );
}
m_predictiveManifolds.Clear();
}
btTransform predictedTrans;
for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ )
{
btRigidBody body = m_nonStaticRigidBodies[i];
body.setHitFraction( 1 );
if( body.isActive() & ( !body.isStaticOrKinematicObject() ) )
{
body.predictIntegratedTransform( timeStep, out predictedTrans );
btVector3 posDelta;
predictedTrans.m_origin.Sub( ref body.m_worldTransform.m_origin, out posDelta );
double squareMotion = posDelta.length2();
if( getDispatchInfo().m_useContinuous && 0 != body.getCcdSquareMotionThreshold() && body.getCcdSquareMotionThreshold() < squareMotion )
{
CProfileSample subsample = new CProfileSample( "predictive convexSweepTest" );
if( body.getCollisionShape().isConvex() )
{
gNumClampedCcdMotions++;
#if PREDICTIVE_CONTACT_USE_STATIC_ONLY
StaticOnlyCallback sweepResults( body, body.getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase().getOverlappingPairCache(),getDispatcher());
#else
btClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get();
sweepResults.Initialize( body, ref body.m_worldTransform.m_origin
, ref predictedTrans.m_origin, getBroadphase().getOverlappingPairCache(), getDispatcher() );
#endif
//btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape());
btSphereShape tmpSphere = BulletGlobals.SphereShapePool.Get();
tmpSphere.Initialize( body.getCcdSweptSphereRadius() );//btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape());
sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
sweepResults.m_collisionFilterGroup = body.getBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.getBroadphaseProxy().m_collisionFilterMask;
btTransform modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.m_basis = body.m_worldTransform.m_basis;
convexSweepTest( tmpSphere, ref body.m_worldTransform, ref modifiedPredictedTrans, sweepResults );
if( sweepResults.hasHit() && ( sweepResults.m_closestHitFraction < 1 ) )
{
btVector3 tmp;
btVector3 distVec; posDelta.Mult( sweepResults.m_closestHitFraction, out distVec );
sweepResults.m_hitNormalWorld.Invert( out tmp );
double distance = distVec.dot( ref tmp );
btPersistentManifold manifold = m_dispatcher1.getNewManifold( body, sweepResults.m_hitCollisionObject );
m_predictiveManifolds.Add( manifold );
btVector3 worldPointB; body.m_worldTransform.m_origin.Add( ref distVec, out worldPointB );
btTransform tmpT;
sweepResults.m_hitCollisionObject.m_worldTransform.inverse( out tmpT );
btVector3 localPointB; tmpT.Apply( ref worldPointB, out localPointB );
btManifoldPoint newPoint = BulletGlobals.ManifoldPointPool.Get();
newPoint.Initialize( ref btVector3.Zero, ref localPointB, ref sweepResults.m_hitNormalWorld, distance );
bool isPredictive = true;
int index = manifold.addManifoldPoint( newPoint, isPredictive );
btManifoldPoint pt = manifold.getContactPoint( index );
pt.m_combinedRestitution = 0;
pt.m_combinedFriction = btManifoldResult.calculateCombinedFriction( body, sweepResults.m_hitCollisionObject );
body.m_worldTransform.getOrigin( out pt.m_positionWorldOnA );
pt.m_positionWorldOnB = worldPointB;
BulletGlobals.ManifoldPointPool.Free( newPoint );
}
BulletGlobals.SphereShapePool.Free( tmpSphere );
BulletGlobals.ClosestNotMeConvexResultCallbackPool.Free( sweepResults );
}
}
}
}
}
public override void debugDrawWorld()
{
if( getDebugDrawer() == null )
return;
CProfileSample sample = new CProfileSample( "debugDrawWorld" );
base.debugDrawWorld();
bool drawConstraints = false;
btIDebugDraw.DebugDrawModes mode = getDebugDrawer().getDebugMode();
if( ( mode & ( btIDebugDraw.DebugDrawModes.DBG_DrawConstraints | btIDebugDraw.DebugDrawModes.DBG_DrawConstraintLimits ) ) != 0 )
{
drawConstraints = true;
}
if( drawConstraints )
{
for( int i = getNumConstraints() - 1; i >= 0; i-- )
{
btTypedConstraint constraint = getConstraint( i );
debugDrawConstraint( constraint );
}
}
if( 0 != ( getDebugDrawer().getDebugMode() & ( btIDebugDraw.DebugDrawModes.DBG_DrawWireframe | btIDebugDraw.DebugDrawModes.DBG_DrawAabb | btIDebugDraw.DebugDrawModes.DBG_DrawNormals ) ) )
{
int i;
if( getDebugDrawer().getDebugMode() != 0 )
{
for( i = 0; i < m_actions.Count; i++ )
{
m_actions[i].debugDraw( m_debugDrawer );
}
}
}
getDebugDrawer().flushLines();
}
///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation)
///it can be useful to use if you perform ray tests without collision detection/simulation
public virtual void computeOverlappingPairs()
{
CProfileSample sample = new CProfileSample( "calculateOverlappingPairs" );
m_broadphasePairCache.calculateOverlappingPairs( m_dispatcher1 );
}
internal void calculateSimulationIslands()
{
CProfileSample sample = new CProfileSample( "calculateSimulationIslands" );
m_islandManager.updateActivationState( this, getDispatcher() );
{
//merge islands based on speculative contact manifolds too
for( int i = 0; i < this.m_predictiveManifolds.Count; i++ )
{
btPersistentManifold manifold = m_predictiveManifolds[i];
btCollisionObject colObj0 = manifold.m_body0;
btCollisionObject colObj1 = manifold.m_body1;
if( ( ( colObj0 != null ) && ( !( colObj0 ).isStaticOrKinematicObject() ) ) &&
( ( colObj1 != null ) && ( !( colObj1 ).isStaticOrKinematicObject() ) ) )
{
m_islandManager.getUnionFind().unite( ( colObj0 ).getIslandTag(), ( colObj1 ).getIslandTag() );
}
}
}
{
int i;
int numConstraints = m_constraints.Count;
for( i = 0; i < numConstraints; i++ )
{
btTypedConstraint constraint = m_constraints[i];
if( constraint.isEnabled() )
{
btRigidBody colObj0 = constraint.getRigidBodyA();
btRigidBody colObj1 = constraint.getRigidBodyB();
if( ( ( colObj0 != null ) && ( !( colObj0 ).isStaticOrKinematicObject() ) ) &&
( ( colObj1 != null ) && ( !( colObj1 ).isStaticOrKinematicObject() ) ) )
{
m_islandManager.getUnionFind().unite( ( colObj0 ).getIslandTag(), ( colObj1 ).getIslandTag() );
}
}
}
}
//Store the island id in each body
m_islandManager.storeIslandActivationState( this );
}
/// 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 void updateActions( double timeStep )
{
CProfileSample sample = new CProfileSample( "updateActions" );
for( int i = 0; i < m_actions.Count; i++ )
{
m_actions[i].updateAction( this, timeStep );
}
}
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 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;
}
/// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
/// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
internal void convexSweepTest( btConvexShape castShape, ref btTransform convexFromWorld, ref btTransform convexToWorld, ConvexResultCallback resultCallback, double allowedCcdPenetration = btScalar.BT_ZERO )
{
CProfileSample sample = new CProfileSample( "convexSweepTest" );
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
btTransform convexFromTrans, convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
btVector3 linVel, angVel;
btTransformUtil.calculateVelocity( ref convexFromWorld, ref convexToWorld, 1.0f, out linVel, out angVel );
btVector3 zeroLinVel = btVector3.Zero;
btTransform R = btTransform.Identity;
R.m_basis = convexFromWorld.m_basis;
castShape.calculateTemporalAabb( ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax );
}
#if !USE_BRUTEFORCE_RAYBROADPHASE
btSingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get();
convexCB.Initialize( castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration );
m_broadphasePairCache.rayTest( ref convexFromTrans.m_origin, ref convexToTrans.m_origin, convexCB, ref castShapeAabbMin, ref castShapeAabbMax );
BulletGlobals.SingleSweepCallbackPool.Free( convexCB );
#else
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for( i = 0; i < m_collisionObjects.Count; i++ )
{
btCollisionObject collisionObject = m_collisionObjects[i];
//only perform raycast if filterMask matches
if( resultCallback.needsCollision( collisionObject.getBroadphaseHandle() ) )
{
//RigidcollisionObject collisionObject = ctrl.GetRigidcollisionObject();
btVector3 collisionObjectAabbMin, collisionObjectAabbMax;
collisionObject.getCollisionShape().getAabb( collisionObject.getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax );
AabbExpand( collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax );
double hitLambda = (double)( 1.0 ); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if( btRayAabb( convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal ) )
{
objectQuerySingle( castShape, convexFromTrans, convexToTrans,
collisionObject,
collisionObject.getCollisionShape(),
collisionObject.getWorldTransform(),
resultCallback,
allowedCcdPenetration );
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
}
public static void objectQuerySingleInternal( btConvexShape castShape, ref btTransform convexFromTrans, ref btTransform convexToTrans
, btCollisionShape collisionShape//btCollisionObjectWrapper colObjWrap
, btCollisionObject collisionObject
, ref btTransform colObjTransform
, ConvexResultCallback resultCallback, double allowedPenetration )
{
//btCollisionShape collisionShape = colObjWrap.getCollisionShape();
//btTransform colObjWorldTransform = colObjWrap.m_worldTransform;
if( collisionShape.isConvex() )
{
//CProfileSample sample = new CProfileSample("convexSweepConvex");
btConvexCast.CastResult castResult = new btConvexCast.CastResult();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar.BT_ONE;//??
btConvexShape convexShape = (btConvexShape)collisionShape;
btVoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
// new btGjkEpaPenetrationDepthSolver();
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver );
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact( ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult =
new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free( gjkEpaPenetrationSolver );
BulletGlobals.VoronoiSimplexSolverPool.Free( simplexSolver );
BulletGlobals.ContinuousConvexCollisionPool.Free( convexCaster1 );
}
else
{
if( collisionShape.isConcave() )
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE )
{
//CProfileSample sample = new CProfileSample("convexSweepbtBvhTriangleMesh");
btConcaveShape triangleMesh = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
//ConvexCast::CastResult
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans
, resultCallback, collisionObject
, triangleMesh, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
triangleMesh.performConvexcast( tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
else
{
if( collisionShape.getShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE )
{
btConvexCast.CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
btStaticPlaneShape planeShape = (btStaticPlaneShape)collisionShape;
btContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize( castShape, planeShape );
btConvexCast castPtr = convexCaster1;
if( castPtr.calcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjTransform, ref colObjTransform, castResult ) )
{
//add hit
if( castResult.m_normal.length2() > (double)( 0.0001 ) )
{
if( castResult.m_fraction < resultCallback.m_closestHitFraction )
{
castResult.m_normal.normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
null,
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult( ref localConvexResult, normalInWorldSpace );
}
}
}
}
else
{
//CProfileSample sample = new CProfileSample("convexSweepConcave");
btConcaveShape concaveShape = (btConcaveShape)collisionShape;
btTransform worldTocollisionObject; colObjTransform.inverse( out worldTocollisionObject );
btVector3 convexFromLocal; worldTocollisionObject.Apply( ref convexFromTrans.m_origin, out convexFromLocal );
btVector3 convexToLocal; worldTocollisionObject.Apply( ref convexToTrans.m_origin, out convexToLocal );
// rotation of box in local mesh space = MeshRotation^-1 ConvexToRotation
btTransform rotationXform; worldTocollisionObject.m_basis.Apply( ref convexToTrans.m_basis, out rotationXform.m_basis );
rotationXform.m_origin = btVector3.Zero;
BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get();
tccb.Initialize( castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject
, concaveShape, ref colObjTransform );
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
btVector3 boxMinLocal, boxMaxLocal;
castShape.getAabb( ref rotationXform, out boxMinLocal, out boxMaxLocal );
btVector3 rayAabbMinLocal = convexFromLocal;
rayAabbMinLocal.setMin( ref convexToLocal );
btVector3 rayAabbMaxLocal = convexFromLocal;
rayAabbMaxLocal.setMax( ref convexToLocal );
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.processAllTriangles( tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal );
BulletGlobals.BridgeTriangleConvexcastCallbackPool.Free( tccb );
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if( collisionShape.isCompound() )
{
CProfileSample sample = new CProfileSample( "convexSweepCompound" );
btCompoundShape compoundShape = (btCompoundShape)( collisionShape );
int i = 0;
for( i = 0; i < compoundShape.getNumChildShapes(); i++ )
{
//btTransform childTrans = compoundShape.getChildTransform( i );
btCollisionShape childCollisionShape = compoundShape.getChildShape( i );
btTransform childWorldTrans; colObjTransform.Apply( ref compoundShape.m_children.InternalArray[i].m_transform
, out childWorldTrans );
LocalInfoAdder my_cb = new LocalInfoAdder( i, resultCallback );
//btCollisionObjectWrapper tmpObj = BulletGlobals.CollisionObjectWrapperPool.Get();
//tmpObj.Initialize( colObjWrap, childCollisionShape, colObjWrap.m_collisionObject, ref childWorldTrans, -1, i );
objectQuerySingleInternal( castShape, ref convexFromTrans, ref convexToTrans
, childCollisionShape
, collisionObject
, ref childWorldTrans
, my_cb, allowedPenetration );
//BulletGlobals.CollisionObjectWrapperPool.Free( tmpObj );
}
}
}
}
}
public virtual void performDiscreteCollisionDetection()
{
CProfileSample sample = new CProfileSample( "performDiscreteCollisionDetection" );
//btDispatcherInfo dispatchInfo = m_dispatchInfo;
updateAabbs();
computeOverlappingPairs();
//btDispatcher dispatcher = m_dispatcher1;
{
CProfileSample sample2 = new CProfileSample( "dispatchAllCollisionPairs" );
if( m_dispatcher1 != null )
m_dispatcher1.dispatchAllCollisionPairs( m_broadphasePairCache.getOverlappingPairCache(), m_dispatchInfo, m_dispatcher1 );
}
}
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
public override int stepSimulation( double timeStep, int maxSubSteps = 1, double fixedTimeStep = btScalar.BT_ONE_OVER_SIXTY )
{
startProfiling( timeStep );
CProfileSample sample = new CProfileSample( "stepSimulation" );
int numSimulationSubSteps = 0;
if( maxSubSteps != 0 )
{
//fixed timestep with interpolation
m_fixedTimeStep = fixedTimeStep;
m_localTime += timeStep;
if( m_localTime >= fixedTimeStep )
{
numSimulationSubSteps = (int)( m_localTime / fixedTimeStep );
m_localTime -= numSimulationSubSteps * fixedTimeStep;
}
}
else
{
//variable timestep
fixedTimeStep = timeStep;
m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
m_fixedTimeStep = 0;
if( btScalar.btFuzzyZero( timeStep ) )
{
numSimulationSubSteps = 0;
maxSubSteps = 0;
}
else
{
numSimulationSubSteps = 1;
maxSubSteps = 1;
}
}
//process some debugging flags
if( getDebugDrawer() != null )
{
btIDebugDraw debugDrawer = getDebugDrawer();
btRigidBody.gDisableDeactivation = ( debugDrawer.getDebugMode() & btIDebugDraw.DebugDrawModes.DBG_NoDeactivation ) != 0;
}
if( numSimulationSubSteps != 0 )
{
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
int clampedSimulationSteps = ( numSimulationSubSteps > maxSubSteps ) ? maxSubSteps : numSimulationSubSteps;
saveKinematicState( fixedTimeStep * clampedSimulationSteps );
applyGravity();
for( int i = 0; i < clampedSimulationSteps; i++ )
{
internalSingleStepSimulation( fixedTimeStep );
synchronizeMotionStates();
}
}
else
{
synchronizeMotionStates();
}
clearForces();
#if !BT_NO_PROFILE
CProfileManager.Increment_Frame_Counter();
#endif //BT_NO_PROFILE
return numSimulationSubSteps;
}
public virtual void integrateTransforms( double timeStep )
{
CProfileSample sample = new CProfileSample( "integrateTransforms" );
btTransform predictedTrans;
for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ )
{
btRigidBody body = m_nonStaticRigidBodies[i];
body.setHitFraction( 1 );
if( body.isActive() && ( !body.isStaticOrKinematicObject() ) )
{
body.predictIntegratedTransform( timeStep, out predictedTrans );
btVector3 delta;
predictedTrans.m_origin.Sub( ref body.m_worldTransform.m_origin, out delta );
double squareMotion = ( delta ).length2();
if( getDispatchInfo().m_useContinuous && body.getCcdSquareMotionThreshold() != 0 && body.getCcdSquareMotionThreshold() < squareMotion )
{
CProfileSample sample2 = new CProfileSample( "CCD motion clamping" );
if( body.getCollisionShape().isConvex() )
{
gNumClampedCcdMotions++;
#if USE_STATIC_ONLY
StaticOnlyCallback sweepResults( body, body.getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase().getOverlappingPairCache(),getDispatcher());
#else
btClosestNotMeConvexResultCallback sweepResults = BulletGlobals.ClosestNotMeConvexResultCallbackPool.Get();
sweepResults.Initialize( body, ref body.m_worldTransform.m_origin
, ref predictedTrans.m_origin, getBroadphase().getOverlappingPairCache(), getDispatcher() );
#endif
//btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape());
using( btSphereShape tmpSphere = BulletGlobals.SphereShapePool.Get() )
{
tmpSphere.Initialize( body.getCcdSweptSphereRadius() );//btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape());
sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration;
sweepResults.m_collisionFilterGroup = body.getBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.getBroadphaseProxy().m_collisionFilterMask;
btTransform modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.setBasis( ref body.m_worldTransform.m_basis );
convexSweepTest( tmpSphere, ref body.m_worldTransform, ref modifiedPredictedTrans, sweepResults );
if( sweepResults.hasHit() && ( sweepResults.m_closestHitFraction < 1 ) )
{
//Console.WriteLine("clamped integration to hit fraction = %f\n",fraction);
body.setHitFraction( sweepResults.m_closestHitFraction );
body.predictIntegratedTransform( timeStep * body.getHitFraction(), out predictedTrans );
body.setHitFraction( 0 );
body.proceedToTransform( ref predictedTrans );
#if false
btVector3 linVel = body.getLinearVelocity();
double maxSpeed = body.getCcdMotionThreshold()/getSolverInfo().m_timeStep;
double maxSpeedSqr = maxSpeed*maxSpeed;
if (linVel.length2()>maxSpeedSqr)
{
linVel.normalize();
linVel*= maxSpeed;
body.setLinearVelocity(linVel);
double ms2 = body.getLinearVelocity().length2();
body.predictIntegratedTransform(timeStep, predictedTrans);
double sm2 = (predictedTrans.getOrigin()-body.getWorldTransform().getOrigin()).length2();
double smt = body.getCcdSquareMotionThreshold();
Console.WriteLine("sm2=%f\n",sm2);
}
#else
//don't apply the collision response right now, it will happen next frame
//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
//double appliedImpulse = 0;
//double depth = 0;
//appliedImpulse = resolveSingleCollision(body,(btCollisionObject)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
#endif
BulletGlobals.ClosestNotMeConvexResultCallbackPool.Free( sweepResults );
continue;
}
BulletGlobals.ClosestNotMeConvexResultCallbackPool.Free( sweepResults );
}
}
}
btScalar.Dbg( DbgFlag.PredictedTransform, predictedTrans.ToString( "predicted orient\t", "\t\t\t", "predicted origin\t" ) );
body.proceedToTransform( ref predictedTrans );
}
}
///this should probably be switched on by default, but it is not well tested yet
if( m_applySpeculativeContactRestitution )
{
CProfileSample sub_sample = new CProfileSample( "apply speculative contact restitution" );
for( int i = 0; i < m_predictiveManifolds.Count; i++ )
{
btPersistentManifold manifold = m_predictiveManifolds[i];
btRigidBody body0 = btRigidBody.upcast( (btCollisionObject)manifold.m_body0 );
btRigidBody body1 = btRigidBody.upcast( (btCollisionObject)manifold.m_body1 );
for( int p = 0; p < manifold.m_cachedPoints; p++ )
{
btManifoldPoint pt = manifold.getContactPoint( p );
double combinedRestitution = btManifoldResult.calculateCombinedRestitution( body0, body1 );
if( combinedRestitution > 0 && pt.m_appliedImpulse != 0 )
//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0)
{
btVector3 imp; pt.m_normalWorldOnB.Mult( combinedRestitution * pt.m_appliedImpulse, out imp );
//btVector3 pos1 = pt.m_positionWorldOnA;
//btVector3 pos2 = pt.m_positionWorldOnB;
btVector3 rel_pos0; pt.m_positionWorldOnA.Sub( ref body0.m_worldTransform.m_origin, out rel_pos0 );
btVector3 rel_pos1; pt.m_positionWorldOnB.Sub( ref body1.m_worldTransform.m_origin, out rel_pos1 );
if( body0 != null )
body0.applyImpulse( ref imp, ref rel_pos0 );
if( body1 != null )
{
imp.Invert( out imp );
body1.applyImpulse( ref imp, ref rel_pos1 );
}
}
}
}
}
}
internal void internalSingleStepSimulation( double timeStep )
{
CProfileSample sample = new CProfileSample( "internalSingleStepSimulation" );
if( null != m_internalPreTickCallback )
{
m_internalPreTickCallback( this, timeStep );
}
///apply gravity, predict motion
predictUnconstraintMotion( timeStep );
btDispatcherInfo dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
createPredictiveContacts( timeStep );
///perform collision detection
performDiscreteCollisionDetection();
calculateSimulationIslands();
getSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
solveConstraints( m_solverInfo );
///CallbackTriggers();
///integrate transforms
integrateTransforms( timeStep );
///update vehicle simulation
updateActions( timeStep );
updateActivationState( timeStep );
if( null != m_internalTickCallback )
{
m_internalTickCallback( this, timeStep );
}
}
internal void solveConstraints( btContactSolverInfo solverInfo )
{
CProfileSample sample = new CProfileSample( "solveConstraints" );
if( m_constraints.Count > 0 )
{
if( m_sortedConstraints.Count < m_constraints.Count )
m_sortedConstraints.Count = m_constraints.Count;
m_sortedConstraints[m_constraints.Count - 1] = null;
//m_sortedConstraints.resize( m_constraints.Count );
int i;
for( i = 0; i < m_constraints.Count; i++ )
{
m_sortedConstraints[i] = m_constraints[i];
}
m_sortedConstraints.quickSort( compare );
}
// Debug.Assert(false);
btTypedConstraint[] constraintsPtr = ( getNumConstraints() != 0 ) ? m_sortedConstraints.InternalArray : null;
m_solverIslandCallback.setup( solverInfo, constraintsPtr, m_sortedConstraints.Count, getDebugDrawer() );
m_constraintSolver.prepareSolve( getNumCollisionObjects(), m_dispatcher1.getNumManifolds() );
/// solve all the constraints for this island
m_islandManager.buildAndProcessIslands( m_dispatcher1, this, m_solverIslandCallback );
m_solverIslandCallback.processConstraints();
m_constraintSolver.allSolved( solverInfo, m_debugDrawer );
}
internal void updateActivationState( double timeStep )
{
CProfileSample sample = new CProfileSample( "updateActivationState" );
for( int i = 0; i < m_nonStaticRigidBodies.Count; i++ )
{
btRigidBody body = m_nonStaticRigidBodies[i];
if( body != null )
{
body.updateDeactivation( timeStep );
if( body.wantsSleeping() )
{
if( body.isStaticOrKinematicObject() )
{
body.setActivationState( ActivationState.ISLAND_SLEEPING );
}
else
{
if( body.m_activationState1 == ActivationState.ACTIVE_TAG )
body.setActivationState( ActivationState.WANTS_DEACTIVATION );
if( body.m_activationState1 == ActivationState.ISLAND_SLEEPING )
{
body.setAngularVelocity( ref btVector3.Zero );
body.setLinearVelocity( ref btVector3.Zero );
}
}
}
else
{
if( body.getActivationState() != ActivationState.DISABLE_DEACTIVATION )
body.setActivationState( ActivationState.ACTIVE_TAG );
}
}
}
}
public virtual void updateAabbs()
{
CProfileSample sample = new CProfileSample( "updateAabbs" );
//btTransform predictedTrans;
for( int i = 0; i < m_collisionObjects.Count; i++ )
{
btCollisionObject colObj = m_collisionObjects[i];
//only update aabb of active objects
if( m_forceUpdateAllAabbs || colObj.isActive() )
{
updateSingleAabb( colObj );
}
}
}