//! apply the correction impulses for two bodies //double solveAngularLimits(double timeStep,ref btVector3 axis, double jacDiagABInv,btRigidBody * body0, btRigidBody * body1); internal double solveAngularLimits( double timeStep, ref btVector3 axis, double jacDiagABInv, btRigidBody body0, btRigidBody body1 ) { if( needApplyTorques() == false ) return 0.0f; double target_velocity = m_targetVelocity; double maxMotorForce = m_maxMotorForce; //current error correction if( m_currentLimit != 0 ) { target_velocity = -m_stopERP * m_currentLimitError / ( timeStep ); maxMotorForce = m_maxLimitForce; } maxMotorForce *= timeStep; // current velocity difference btVector3 angVelA = body0.getAngularVelocity(); btVector3 angVelB = body1.getAngularVelocity(); btVector3 vel_diff; vel_diff = angVelA - angVelB; double rel_vel = axis.dot( vel_diff ); // correction velocity double motor_relvel = m_limitSoftness * ( target_velocity - m_damping * rel_vel ); if( motor_relvel < btScalar.SIMD_EPSILON && motor_relvel > -btScalar.SIMD_EPSILON ) { return 0.0f;//no need for applying force } // correction impulse double unclippedMotorImpulse = ( 1 + m_bounce ) * motor_relvel * jacDiagABInv; // clip correction impulse double clippedMotorImpulse; ///@todo: should clip against accumulated impulse if( unclippedMotorImpulse > 0.0f ) { clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; } else { clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; } // sort with accumulated impulses double lo = (double)( -btScalar.BT_LARGE_FLOAT ); double hi = (double)( btScalar.BT_LARGE_FLOAT ); double oldaccumImpulse = m_accumulatedImpulse; double sum = oldaccumImpulse + clippedMotorImpulse; m_accumulatedImpulse = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum; clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; btVector3 motorImp; axis.Mult( clippedMotorImpulse, out motorImp ); body0.applyTorqueImpulse( ref motorImp ); btVector3 tmp; motorImp.Invert( out tmp ); body1.applyTorqueImpulse( ref tmp ); return clippedMotorImpulse; }
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; } }