public void setup( btContactSolverInfo solverInfo
, btTypedConstraint[] sortedConstraints, int numConstraints
, btIDebugDraw debugDrawer )
{
Debug.Assert( solverInfo != null );
m_solverInfo = solverInfo;
m_sortedConstraints = sortedConstraints;
m_numConstraints = numConstraints;
m_debugDrawer = debugDrawer;
m_bodies.Count = ( 0 );
m_manifolds.Count = ( 0 );
m_constraints.Count = ( 0 );
}
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 InplaceSolverIslandCallback(
btConstraintSolver solver,
btDispatcher dispatcher )
{
m_solverInfo = null;
m_solver = ( solver );
m_sortedConstraints = null;
m_numConstraints = ( 0 );
m_debugDrawer = null;
m_dispatcher = ( dispatcher );
}
internal void setFrictionConstraintImpulse( btSolverConstraint solverConstraint,
btSolverBody bodyA, btSolverBody bodyB,
btManifoldPoint cp, btContactSolverInfo infoGlobal )
{
btRigidBody rb0 = bodyA.m_originalBody;
btRigidBody rb1 = bodyB.m_originalBody;
{
btSolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
btScalar.Dbg( "New Applied source is " + cp.m_appliedImpulseLateral1.ToString( "g17" ) );
if( rb0 != null )
{
btVector3 tmp; frictionConstraint1.m_contactNormal1.Mult2( ref rb0.m_linearFactor, rb0.getInvMass(), out tmp );
bodyA.applyImpulse( ref tmp, ref frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse );
}
if( rb1 != null )
{
btVector3 tmp; frictionConstraint1.m_contactNormal2.Mult2( ref rb1.m_linearFactor, -rb1.getInvMass(), out tmp );
btVector3 tmp2; frictionConstraint1.m_angularComponentB.Invert( out tmp2 );
bodyB.applyImpulse( ref tmp, ref tmp2, -(double)frictionConstraint1.m_appliedImpulse );
}
}
else
{
frictionConstraint1.m_appliedImpulse = 0;
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
btSolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if( rb0 != null )
{
btVector3 tmp; frictionConstraint2.m_contactNormal1.Mult( rb0.getInvMass(), out tmp );
bodyA.applyImpulse( ref tmp, ref frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse );
}
if( rb1 != null )
{
btVector3 tmp; frictionConstraint2.m_contactNormal2.Mult( -rb1.getInvMass(), out tmp );
btVector3 tmp2; frictionConstraint2.m_angularComponentB.Invert( out tmp2 );
bodyB.applyImpulse( ref tmp, ref tmp2, -(double)frictionConstraint2.m_appliedImpulse );
}
}
else
{
frictionConstraint2.m_appliedImpulse = 0;
}
}
}
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 );
}
}
}
internal override void allSolved( btContactSolverInfo info, btIDebugDraw debugDrawer ) {; }
internal void setupContactConstraint( btSolverConstraint solverConstraint,
btSolverBody bodyA, btSolverBody bodyB,
btManifoldPoint cp, btContactSolverInfo infoGlobal,
out double relaxation,
ref btVector3 rel_pos1, ref btVector3 rel_pos2 )
{
// ref btVector3 pos1 = cp.getPositionWorldOnA();
// ref btVector3 pos2 = cp.getPositionWorldOnB();
btRigidBody rb0 = bodyA.m_originalBody;
btRigidBody rb1 = bodyB.m_originalBody;
// btVector3 rel_pos1 = pos1 - colObj0.getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1.getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA.getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB.getWorldTransform().getOrigin();
relaxation = 1;
btVector3 torqueAxis0; rel_pos1.cross( ref cp.m_normalWorldOnB, out torqueAxis0 );
btVector3 tmp;
//solverConstraint.m_angularComponentA = rb0 != null ? rb0.m_invInertiaTensorWorld * torqueAxis0 * rb0.getAngularFactor() : btVector3.Zero;
if( rb0 != null )
{
rb0.m_invInertiaTensorWorld.Apply( ref torqueAxis0, out tmp );
tmp.Mult( ref rb0.m_angularFactor, out solverConstraint.m_angularComponentA );
}
else
solverConstraint.m_angularComponentA = btVector3.Zero;
btVector3 torqueAxis1; rel_pos2.cross( ref cp.m_normalWorldOnB, out torqueAxis1 );
torqueAxis1.Invert( out torqueAxis1 );
//solverConstraint.m_angularComponentB = rb1 != null ? rb1.m_invInertiaTensorWorld * -torqueAxis1 * rb1.getAngularFactor() : btVector3.Zero;
if( rb1 != null )
{
rb1.m_invInertiaTensorWorld.Apply( ref torqueAxis1, out tmp );
tmp.Mult( ref rb1.m_angularFactor, out solverConstraint.m_angularComponentB );
}
else
solverConstraint.m_angularComponentB = btVector3.Zero;
{
#if COMPUTE_IMPULSE_DENOM
double denom0 = rb0.computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
double denom1 = rb1.computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
btVector3 vec;
double denom0 = 0;
double denom1 = 0;
if( rb0 != null )
{
( solverConstraint.m_angularComponentA ).cross( ref rel_pos1, out vec );
denom0 = rb0.getInvMass() + cp.m_normalWorldOnB.dot( vec );
}
if( rb1 != null )
{
solverConstraint.m_angularComponentB.Invert( out tmp );
tmp.cross( ref rel_pos2, out vec );
denom1 = rb1.getInvMass() + cp.m_normalWorldOnB.dot( vec );
}
#endif //COMPUTE_IMPULSE_DENOM
double denom = relaxation / ( denom0 + denom1 );
btScalar.Dbg( "m_jacDiagABInv 3 set to " + denom.ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = denom;
}
if( rb0 != null )
{
solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
btScalar.Dbg( "Torque Axis to relpos1 " + torqueAxis0 );
}
else
{
solverConstraint.m_contactNormal1 = btVector3.Zero;
solverConstraint.m_relpos1CrossNormal = btVector3.Zero;
}
if( rb1 != null )
{
cp.m_normalWorldOnB.Invert( out solverConstraint.m_contactNormal2 );
solverConstraint.m_relpos2CrossNormal = torqueAxis1;
btScalar.Dbg( "Torque Axis to relpos2 " + torqueAxis1 );
}
else
{
solverConstraint.m_contactNormal2 = btVector3.Zero;
solverConstraint.m_relpos2CrossNormal = btVector3.Zero;
}
double restitution = 0;
double penetration = cp.getDistance() + infoGlobal.m_linearSlop;
{
btVector3 vel1, vel2;
vel1 = rb0 != null ? rb0.getVelocityInLocalPoint( ref rel_pos1 ) : btVector3.Zero;
vel2 = rb1 != null ? rb1.getVelocityInLocalPoint( ref rel_pos2 ) : btVector3.Zero;
// btVector3 vel2 = rb1 ? rb1.getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
btVector3 vel; vel1.Sub( ref vel2, out vel );
double rel_vel = cp.m_normalWorldOnB.dot( ref vel );
solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve( rel_vel, cp.m_combinedRestitution );
if( restitution <= btScalar.BT_ZERO )
{
restitution = 0;
};
}
///warm starting (or zero if disabled)
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if( rb0 != null )
{
solverConstraint.m_contactNormal1.Mult2( ref bodyA.m_invMass, ref rb0.m_linearFactor, out tmp );
bodyA.applyImpulse( ref tmp, ref solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse );
}
if( rb1 != null )
{
solverConstraint.m_contactNormal2.Mult2( ref rb1.m_linearFactor, ref bodyB.m_invMass, out tmp );
tmp.Invert( out tmp );
btVector3 tmp2; solverConstraint.m_angularComponentB.Invert( out tmp2 );
bodyB.applyImpulse( ref tmp, ref tmp2, -(double)solverConstraint.m_appliedImpulse );
}
}
else
{
solverConstraint.m_appliedImpulse = 0;
}
solverConstraint.m_appliedPushImpulse = 0;
{
btVector3 externalForceImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseA = bodyA.m_originalBody != null ? bodyA.m_externalTorqueImpulse : btVector3.Zero;
btVector3 externalForceImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseB = bodyB.m_originalBody != null ? bodyB.m_externalTorqueImpulse : btVector3.Zero;
btScalar.Dbg( "external torque impulses " + externalTorqueImpulseA + externalTorqueImpulseB );
double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref bodyA.m_linearVelocity, ref externalForceImpulseA )
+ solverConstraint.m_relpos1CrossNormal.dotAdded( ref bodyA.m_angularVelocity, ref externalTorqueImpulseA );
double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref bodyB.m_linearVelocity, ref externalForceImpulseB )
+ solverConstraint.m_relpos2CrossNormal.dotAdded( ref bodyB.m_angularVelocity, ref externalTorqueImpulseB );
double rel_vel = vel1Dotn + vel2Dotn;
double positionalError = 0;
double velocityError = restitution - rel_vel;// * damping;
double erp = infoGlobal.m_erp2;
if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) )
{
erp = infoGlobal.m_erp;
}
if( penetration > 0 )
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
}
else
{
positionalError = -penetration * erp / infoGlobal.m_timeStep;
}
double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
if( !infoGlobal.m_splitImpulse || ( penetration > infoGlobal.m_splitImpulsePenetrationThreshold ) )
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA.m_externalForce*bodyA.m_invMass-bodyB.m_externalForce/bodyB.m_invMass)*solverConstraint.m_jacDiagABInv;
btScalar.Dbg( "Constraint 3 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = 0;
}
else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
btScalar.Dbg( "Constraint 4 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
protected virtual double solveGroupCacheFriendlyFinish( btCollisionObject[] bodies, int numBodies, btContactSolverInfo infoGlobal )
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int i, j;
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_WARMSTARTING ) != 0 )
{
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[j];
btManifoldPoint pt = solveManifold.m_originalContactPoint as btManifoldPoint;
Debug.Assert( pt != null );
pt.m_appliedImpulse = solveManifold.m_appliedImpulse;
// float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
// Console.WriteLine("pt.m_appliedImpulseLateral1 = %f\n", f);
pt.m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
btScalar.Dbg( "New manifold source is " + pt.m_appliedImpulseLateral1.ToString( "g17" ) + " from " + solveManifold.m_frictionIndex );
//Console.WriteLine("pt.m_appliedImpulseLateral1 = %f\n", pt.m_appliedImpulseLateral1);
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
pt.m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
}
//do a callback here?
}
}
numPoolConstraints = m_tmpSolverNonContactConstraintPool.Count;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solverConstr = m_tmpSolverNonContactConstraintPool[j];
btTypedConstraint constr = (btTypedConstraint)solverConstr.m_originalContactPoint;
btTypedConstraint.btJointFeedback fb = constr.getJointFeedback();
if( fb != null )
{
double scalar = solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
btVector3 tmp;
solverConstr.m_contactNormal1.Mult2( ref constr.m_rbA.m_linearFactor, scalar, out tmp );
fb.m_appliedForceBodyA.Add( ref tmp, out fb.m_appliedForceBodyA );
//fb.m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr.getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep;
solverConstr.m_contactNormal2.Mult2( ref constr.m_rbB.m_linearFactor, scalar, out tmp );
fb.m_appliedForceBodyB.Add( ref tmp, out fb.m_appliedForceBodyB );
//fb.m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr.getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep;
solverConstr.m_relpos1CrossNormal.Mult2( ref constr.m_rbA.m_angularFactor, scalar, out tmp );
fb.m_appliedTorqueBodyA.Add( ref tmp, out fb.m_appliedTorqueBodyA );
//fb.m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr.getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
solverConstr.m_relpos2CrossNormal.Mult2( ref constr.m_rbB.m_angularFactor, scalar, out tmp );
fb.m_appliedTorqueBodyB.Add( ref tmp, out fb.m_appliedTorqueBodyB );
//fb.m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr.getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */
}
constr.internalSetAppliedImpulse( solverConstr.m_appliedImpulse );
if( btScalar.btFabs( solverConstr.m_appliedImpulse ) >= constr.getBreakingImpulseThreshold() )
{
constr.setEnabled( false );
}
}
for( i = 0; i < m_tmpSolverBodyPool.Count; i++ )
{
btSolverBody solverBody = m_tmpSolverBodyPool[i];
btRigidBody body = solverBody.m_originalBody;
if( body != null )
{
if( infoGlobal.m_splitImpulse )
solverBody.writebackVelocityAndTransform( infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp );
else
solverBody.writebackVelocity();
btVector3 tmp;
solverBody.m_linearVelocity.Add( ref solverBody.m_externalForceImpulse, out tmp );
solverBody.m_originalBody.setLinearVelocity( ref tmp );
solverBody.m_angularVelocity.Add( ref solverBody.m_externalTorqueImpulse, out tmp );
solverBody.m_originalBody.setAngularVelocity( ref tmp );
if( infoGlobal.m_splitImpulse && solverBody.modified )
{
btScalar.Dbg( DbgFlag.PredictedTransform, "Solver body transform is " + solverBody.m_worldTransform );
solverBody.m_originalBody.setWorldTransform( ref solverBody.m_worldTransform );
}
BulletGlobals.SolverBodyPool.Free( solverBody );
body.setCompanionBody( null );
}
}
foreach( btSolverConstraint constraint in m_tmpSolverContactConstraintPool )
BulletGlobals.SolverConstraintPool.Free( constraint );
foreach( btSolverConstraint constraint in m_tmpSolverNonContactConstraintPool )
BulletGlobals.SolverConstraintPool.Free( constraint );
foreach( btSolverConstraint constraint in m_tmpSolverContactFrictionConstraintPool )
BulletGlobals.SolverConstraintPool.Free( constraint );
foreach( btSolverConstraint constraint in m_tmpSolverContactRollingFrictionConstraintPool )
BulletGlobals.SolverConstraintPool.Free( constraint );
m_tmpSolverContactConstraintPool.Count = 0; // resizeNoInitialize( 0 );
m_tmpSolverNonContactConstraintPool.Count = 0; //resizeNoInitialize( 0 );
m_tmpSolverContactFrictionConstraintPool.Count = 0; //resizeNoInitialize( 0 );
m_tmpSolverContactRollingFrictionConstraintPool.Count = 0; //resizeNoInitialize( 0 );
m_tmpSolverBodyPool.Count = 0; //resizeNoInitialize( 0 );
m_fixedBody = null;
return 0;
}
/// 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 );
}
}
}
}
}
}
protected virtual double solveGroupCacheFriendlyIterations( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlyIterations" );
{
///this is a special step to resolve penetrations (just for contacts)
solveGroupCacheFriendlySplitImpulseIterations( bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
for( int iteration = 0; iteration < maxIterations; iteration++ )
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
{
solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, start_manifold, numManifolds, constraints, startConstraint, numConstraints, infoGlobal, debugDrawer );
}
}
return 0;
}
protected virtual double solveSingleIteration( int iteration, btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints
, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_RANDMIZE_ORDER ) != 0 )
{
if( true ) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for( int j = 0; j < numNonContactPool; ++j )
{
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraintPool; ++j )
{
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for( int j = 0; j < numFrictionPool; ++j )
{
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2( j + 1 );
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
}
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_SIMD ) != 0 )
{
///solve all joint constraints, using SIMD, if available
for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ )
{
btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if( iteration < constraint.m_overrideNumSolverIterations )
resolveSingleConstraintRowGenericSIMD( constraint.m_solverBodyA
, constraint.m_solverBodyB, constraint );
}
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
if( constraint.isEnabled() )
{
btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep );
btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep );
constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
}
}
///solve all contact constraints using SIMD, if available
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS ) != 0 )
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int multiplier = ( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 ) ? 2 : 1;
for( int c = 0; c < numPoolConstraints; c++ )
{
double totalImpulse = 0;
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if( applyFriction )
{
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
int j;
for( j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
gResolveSingleConstraintRowLowerLimit_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count;
for( j = 0; j < numFrictionPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
btScalar.Dbg( "PreFriction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) );
gResolveSingleConstraintRowGeneric_scalar_reference( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
//resolveSingleConstraintRowGenericSIMD( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
btScalar.Dbg( "Friction Impulse?" + solveManifold.m_appliedImpulse.ToString( "g17" ) );
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count;
for( j = 0; j < numRollingFrictionPoolConstraints; j++ )
{
btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
gResolveSingleConstraintRowGeneric_scalar_reference( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
//resolveSingleConstraintRowGenericSIMD( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
}
}
}
}
}
else
{
//non-SIMD version
///solve all joint constraints
for( int j = 0; j < m_tmpSolverNonContactConstraintPool.Count; j++ )
{
btSolverConstraint constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if( iteration < constraint.m_overrideNumSolverIterations )
resolveSingleConstraintRowGeneric( constraint.m_solverBodyA, constraint.m_solverBodyB, constraint );
}
if( iteration < infoGlobal.m_numIterations )
{
for( int j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
if( constraint.isEnabled() )
{
//int bodyAid = ;
//int bodyBid = ;
btSolverBody bodyA = getOrInitSolverBody( constraint.getRigidBodyA(), infoGlobal.m_timeStep );
btSolverBody bodyB = getOrInitSolverBody( constraint.getRigidBodyB(), infoGlobal.m_timeStep );
constraint.solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep );
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.Count;
for( int j = 0; j < numPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimit( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.Count;
for( int j = 0; j < numFrictionPoolConstraints; j++ )
{
btSolverConstraint solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
double totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
solveManifold.m_lowerLimit = -( solveManifold.m_friction * totalImpulse );
solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
resolveSingleConstraintRowGeneric( solveManifold.m_solverBodyA, solveManifold.m_solverBodyB, solveManifold );
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.Count;
for( int j = 0; j < numRollingFrictionPoolConstraints; j++ )
{
btSolverConstraint rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
double totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if( totalImpulse > (double)( 0 ) )
{
double rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
if( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction )
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
resolveSingleConstraintRowGeneric( rollingFrictionConstraint.m_solverBodyA, rollingFrictionConstraint.m_solverBodyB, rollingFrictionConstraint );
}
}
}
}
return 0;
}
protected virtual double solveGroupCacheFriendlySetup( btCollisionObject[] bodies, int numBodies
, btPersistentManifold[] manifoldPtr, int start_manifold, int numManifolds
, btTypedConstraint[] constraints, int startConstraint, int numConstraints, btContactSolverInfo infoGlobal, btIDebugDraw debugDrawer )
{
if( m_fixedBody != null )
{
BulletGlobals.SolverBodyPool.Free( m_fixedBody );
m_fixedBody = null;
}
//m_fixedBodyId = -1;
CProfileSample sample = new CProfileSample( "solveGroupCacheFriendlySetup" );
//(void)debugDrawer;
m_maxOverrideNumSolverIterations = 0;
#if BT_ADDITIONAL_DEBUG
//make sure that dynamic bodies exist for all (enabled)raints
for (int i=0;i<numConstraints;i++)
{
btTypedConstraint* constraint = constraints[i];
if (constraint.isEnabled())
{
if (!constraint.getRigidBodyA().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint.getRigidBodyA()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
if (!constraint.getRigidBodyB().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint.getRigidBodyB()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
}
}
//make sure that dynamic bodies exist for all contact manifolds
for (int i=0;i<numManifolds;i++)
{
if (!manifoldPtr[i].getBody0().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (manifoldPtr[i].getBody0()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
if (!manifoldPtr[i].getBody1().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (manifoldPtr[i].getBody1()==bodies[b])
{
found = true;
break;
}
}
Debug.Assert(found);
}
}
#endif //BT_ADDITIONAL_DEBUG
for( int i = 0; i < numBodies; i++ )
{
bodies[i].setCompanionBody( null );
}
m_tmpSolverBodyPool.Capacity = ( numBodies + 1 );
m_tmpSolverBodyPool.Count = ( 0 );
//btSolverBody fixedBody = m_tmpSolverBodyPool.expand();
//initSolverBody(&fixedBody,0);
//convert all bodies
for( int i = 0; i < numBodies; i++ )
{
//int bodyId = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep );
btRigidBody body = btRigidBody.upcast( bodies[i] );
if( body != null && body.getInvMass() != 0 )
{
btSolverBody solverBody = getOrInitSolverBody( bodies[i], infoGlobal.m_timeStep );
btVector3 gyroForce = btVector3.Zero;
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT ) != 0 )
{
body.computeGyroscopicForceExplicit( infoGlobal.m_maxGyroscopicForce, out gyroForce );
btVector3 tmp;
body.m_invInertiaTensorWorld.ApplyInverse( ref gyroForce, out tmp );
//solverBody.m_externalTorqueImpulse -= gyroForce * body.m_invInertiaTensorWorld * infoGlobal.m_timeStep;
solverBody.m_externalTorqueImpulse.SubScale( ref tmp, infoGlobal.m_timeStep, out solverBody.m_externalTorqueImpulse );
}
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD ) != 0 )
{
body.computeGyroscopicImpulseImplicit_World( infoGlobal.m_timeStep, out gyroForce );
solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse );
}
if( ( body.getFlags() & btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ) != 0 )
{
body.computeGyroscopicImpulseImplicit_Body( infoGlobal.m_timeStep, out gyroForce );
btScalar.Dbg( "Gyroforce " + gyroForce );
solverBody.m_externalTorqueImpulse.Add( ref gyroForce, out solverBody.m_externalTorqueImpulse );
}
}
}
if( true )
{
int j;
for( j = 0; j < numConstraints; j++ )
{
btTypedConstraint constraint = constraints[j + startConstraint];
constraint.buildJacobian();
constraint.internalSetAppliedImpulse( 0.0f );
}
}
//btRigidBody rb0=0,*rb1=0;
//if (1)
{
{
int totalNumRows = 0;
int i;
m_tmpConstraintSizesPool.Capacity = ( numConstraints );
//calculate the total number of contraint rows
for( i = 0; i < numConstraints; i++ )
{
int infoNumConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows;
//btTypedConstraint.btConstraintInfo1 info1 = m_tmpConstraintSizesPool[i];
btTypedConstraint.btJointFeedback fb = constraints[i + startConstraint].getJointFeedback();
if( fb != null )
{
fb.m_appliedForceBodyA.setZero();
fb.m_appliedTorqueBodyA.setZero();
fb.m_appliedForceBodyB.setZero();
fb.m_appliedTorqueBodyB.setZero();
}
if( constraints[i + startConstraint].isEnabled() )
{
constraints[i + startConstraint].getInfo1( ref m_tmpConstraintSizesPool.InternalArray[i] );
}
else
{
m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows = 0;
m_tmpConstraintSizesPool.InternalArray[i].nub = 0;
}
totalNumRows += m_tmpConstraintSizesPool.InternalArray[i].m_numConstraintRows;
}
m_tmpSolverNonContactConstraintPool.Count = ( totalNumRows );
for( i = 0; i < totalNumRows; i++ )
m_tmpSolverNonContactConstraintPool[i] = BulletGlobals.SolverConstraintPool.Get();
///setup the btSolverConstraints
int currentRow = 0;
for( i = 0; i < numConstraints; i++ )
{
int infoConstraintRows = m_tmpConstraintSizesPool[i].m_numConstraintRows;
if( infoConstraintRows != 0 )
{
Debug.Assert( currentRow < totalNumRows );
btSolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow];
btTypedConstraint constraint = constraints[i + startConstraint];
btRigidBody rbA = constraint.getRigidBodyA();
btRigidBody rbB = constraint.getRigidBodyB();
//int solverBodyIdA = ;
//int solverBodyIdB = ;
btSolverBody bodyAPtr = getOrInitSolverBody( rbA, infoGlobal.m_timeStep );
btSolverBody bodyBPtr = getOrInitSolverBody( rbB, infoGlobal.m_timeStep );
int overrideNumSolverIterations = constraint.getOverrideNumSolverIterations() > 0 ? constraint.getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
if( overrideNumSolverIterations > m_maxOverrideNumSolverIterations )
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
int j;
for( j = 0; j < infoConstraintRows; j++ )
{
btSolverConstraint current = m_tmpSolverNonContactConstraintPool[currentRow + j];
current.Clear();
//memset( ¤tConstraintRow[j], 0, sizeof( btSolverConstraint ) );
current.m_lowerLimit = btScalar.SIMD_NEG_INFINITY;
current.m_upperLimit = btScalar.SIMD_INFINITY;
current.m_appliedImpulse = 0;
current.m_appliedPushImpulse = 0;
current.m_solverBodyA = bodyAPtr;
current.m_solverBodyB = bodyBPtr;
current.m_overrideNumSolverIterations = overrideNumSolverIterations;
}
bodyAPtr.Clear();
btTypedConstraint.btConstraintInfo2 info2 = m_tmpConstraintInfo2Pool.Get();// new btTypedConstraint.btConstraintInfo2();
info2.m_numRows = infoConstraintRows;
for( j = 0; j < infoConstraintRows; ++j )
{
info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j];
}
info2.fps = 1 / infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
#if OLD_CONSTRAINT_INFO_INIT
info2.m_J1linearAxis = currentConstraintRow.m_contactNormal1;
info2.m_J1angularAxis = currentConstraintRow.m_relpos1CrossNormal;
info2.m_J2linearAxis = currentConstraintRow.m_contactNormal2;
info2.m_J2angularAxis = currentConstraintRow.m_relpos2CrossNormal;
info2.rowskip = 0;// sizeof( btSolverConstraint ) / sizeof( double );//check this
///the size of btSolverConstraint needs be a multiple of double
//Debug.Assert( info2.rowskip * sizeof( double ) == sizeof( btSolverConstraint ) );
info2.m_constraintError = currentConstraintRow.m_rhs;
info2.cfm = currentConstraintRow.m_cfm;
info2.m_lowerLimit = currentConstraintRow.m_lowerLimit;
info2.m_upperLimit = currentConstraintRow.m_upperLimit;
#endif
currentConstraintRow.m_cfm = infoGlobal.m_globalCfm;
info2.m_damping = infoGlobal.m_damping;
info2.m_numIterations = infoGlobal.m_numIterations;
constraint.getInfo2( info2 );
///finalize the constraint setup
for( j = 0; j < infoConstraintRows; j++ )
{
btSolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j];
if( solverConstraint.m_upperLimit >= constraint.getBreakingImpulseThreshold() )
{
solverConstraint.m_upperLimit = constraint.getBreakingImpulseThreshold();
}
if( solverConstraint.m_lowerLimit <= -constraint.getBreakingImpulseThreshold() )
{
solverConstraint.m_lowerLimit = -constraint.getBreakingImpulseThreshold();
}
solverConstraint.m_originalContactPoint = constraint;
btVector3 tmp;
{
//solverConstraint.m_angularComponentA = constraint.getRigidBodyA().m_invInertiaTensorWorld
// *solverConstraint.m_relpos1CrossNormal * constraint.getRigidBodyA().getAngularFactor();
constraint.m_rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out tmp );
tmp.Mult( ref constraint.m_rbA.m_angularFactor, out solverConstraint.m_angularComponentA );
}
{
//solverConstraint.m_angularComponentB = constraint.getRigidBodyB().m_invInertiaTensorWorld
// * solverConstraint.m_relpos2CrossNormal * constraint.getRigidBodyB().getAngularFactor();
constraint.m_rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out tmp );
tmp.Mult( ref constraint.m_rbB.m_angularFactor, out solverConstraint.m_angularComponentB );
}
{
btVector3 iMJlA; solverConstraint.m_contactNormal1.Mult( rbA.m_inverseMass, out iMJlA );
btVector3 iMJaA; rbA.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos1CrossNormal, out iMJaA );
btVector3 iMJlB; solverConstraint.m_contactNormal2.Mult( rbB.m_inverseMass, out iMJlB );//sign of normal?
btVector3 iMJaB; rbB.m_invInertiaTensorWorld.Apply( ref solverConstraint.m_relpos2CrossNormal, out iMJaB );
double sum = iMJlA.dot( ref solverConstraint.m_contactNormal1 );
sum += iMJaA.dot( ref solverConstraint.m_relpos1CrossNormal );
sum += iMJlB.dot( ref solverConstraint.m_contactNormal2 );
sum += iMJaB.dot( ref solverConstraint.m_relpos2CrossNormal );
double fsum = btScalar.btFabs( sum );
Debug.Assert( fsum > btScalar.SIMD_EPSILON );
btScalar.Dbg( "m_jacDiagABInv 4 set to " + ( fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0 ).ToString( "g17" ) );
solverConstraint.m_jacDiagABInv = fsum > btScalar.SIMD_EPSILON ? btScalar.BT_ONE / sum : 0;
}
{
double rel_vel;
btVector3 externalForceImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseA = bodyAPtr.m_originalBody != null ? bodyAPtr.m_externalTorqueImpulse : btVector3.Zero;
btVector3 externalForceImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalForceImpulse : btVector3.Zero;
btVector3 externalTorqueImpulseB = bodyBPtr.m_originalBody != null ? bodyBPtr.m_externalTorqueImpulse : btVector3.Zero;
btScalar.Dbg( "external torque2 impulses " + externalTorqueImpulseA + externalTorqueImpulseB );
double vel1Dotn = solverConstraint.m_contactNormal1.dotAdded( ref rbA.m_linearVelocity, ref externalForceImpulseA )
+ solverConstraint.m_relpos1CrossNormal.dotAdded( ref rbA.m_angularVelocity, ref externalTorqueImpulseA );
double vel2Dotn = solverConstraint.m_contactNormal2.dotAdded( ref rbB.m_linearVelocity, ref externalForceImpulseB )
+ solverConstraint.m_relpos2CrossNormal.dotAdded( ref rbB.m_angularVelocity, ref externalTorqueImpulseB );
rel_vel = vel1Dotn + vel2Dotn;
double restitution = 0;
double positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
double velocityError = restitution - rel_vel * info2.m_damping;
double penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
double velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
btScalar.Dbg( "Constraint 5 m_rhs " + solverConstraint.m_rhs.ToString( "g17" ) );
solverConstraint.m_appliedImpulse = 0;
}
}
}
currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows;
}
}
btScalar.Dbg( "About to convert contacts " + start_manifold + " " + numManifolds );
convertContacts( manifoldPtr, start_manifold, numManifolds, infoGlobal );
}
// btContactSolverInfo info = infoGlobal;
int numNonContactPool = m_tmpSolverNonContactConstraintPool.Count;
int numConstraintPool = m_tmpSolverContactConstraintPool.Count;
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count;
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderNonContactConstraintPool.Capacity = ( numNonContactPool );
if( ( infoGlobal.m_solverMode & btSolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS ) != 0 )
m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool * 2 );
else
m_orderTmpConstraintPool.Count = m_orderTmpConstraintPool.Capacity = ( numConstraintPool );
m_orderFrictionConstraintPool.Count = m_orderFrictionConstraintPool.Capacity = ( numFrictionPool );
{
int i;
for( i = 0; i < numNonContactPool; i++ )
{
m_orderNonContactConstraintPool[i] = i;
}
for( i = 0; i < numConstraintPool; i++ )
{
m_orderTmpConstraintPool[i] = i;
}
for( i = 0; i < numFrictionPool; i++ )
{
m_orderFrictionConstraintPool[i] = i;
}
}
return 0;
}
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 );
}
}
internal abstract void allSolved( btContactSolverInfo info, btIDebugDraw debugDrawer );
///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 );
