protected void initSolverBody( btSolverBody solverBody, btRigidBody rb, double timeStep )
{
//btRigidBody rb = collisionObject != null ? btRigidBody.upcast( collisionObject ) : null;
solverBody.m_deltaLinearVelocity = btVector3.Zero;
solverBody.m_deltaAngularVelocity = btVector3.Zero;
solverBody.m_pushVelocity = btVector3.Zero;
solverBody.m_turnVelocity = btVector3.Zero;
solverBody.modified = false;
solverBody.pushed = false;
if( rb != null )
{
solverBody.m_worldTransform = rb.m_worldTransform;
btVector3 tmp = new btVector3( rb.getInvMass() );
btVector3 tmp2;
btVector3 tmp3;
rb.getLinearFactor( out tmp2 );
tmp.Mult( ref tmp2, out tmp3 );
solverBody.internalSetInvMass( ref tmp3 );
solverBody.m_originalBody = rb;
rb.getAngularFactor( out solverBody.m_angularFactor );
rb.getLinearFactor( out solverBody.m_linearFactor );
rb.getLinearVelocity( out solverBody.m_linearVelocity );
rb.getAngularVelocity( out solverBody.m_angularVelocity );
rb.m_totalForce.Mult( rb.m_inverseMass * timeStep, out solverBody.m_externalForceImpulse );
//= rb.getTotalForce() * rb.getInvMass() * timeStep;
rb.m_invInertiaTensorWorld.Apply( ref rb.m_totalTorque, out tmp );
tmp.Mult( timeStep, out solverBody.m_externalTorqueImpulse );
btScalar.Dbg( "Setup external impulses " + solverBody.m_externalForceImpulse + " " + solverBody.m_externalTorqueImpulse );
///solverBody.m_externalTorqueImpulse = rb.getTotalTorque() * rb.getInvInertiaTensorWorld() * timeStep;
}
else
{
solverBody.modified = false;
solverBody.m_worldTransform = btTransform.Identity;
solverBody.internalSetInvMass( ref btVector3.Zero );
solverBody.m_originalBody = null;
solverBody.m_angularFactor = btVector3.One;
solverBody.m_linearFactor = btVector3.One;
solverBody.m_linearVelocity = btVector3.Zero;
solverBody.m_angularVelocity = btVector3.Zero;
solverBody.m_externalForceImpulse = btVector3.Zero;
solverBody.m_externalTorqueImpulse = btVector3.Zero;
}
}