btScalar btRotationalLimitMotor::solveAngularLimits( btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, btRigidBody * body0, btRigidBody * body1 ) { if (needApplyTorques()==false) return 0.0f; btScalar target_velocity = m_targetVelocity; btScalar 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->internalGetAngularVelocity(angVelA); btVector3 angVelB; body1->internalGetAngularVelocity(angVelB); btVector3 vel_diff; vel_diff = angVelA-angVelB; btScalar rel_vel = axis.dot(vel_diff); // correction velocity btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) { return 0.0f;//no need for applying force } // correction impulse btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse btScalar 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 btScalar lo = btScalar(-BT_LARGE_FLOAT); btScalar hi = btScalar(BT_LARGE_FLOAT); btScalar oldaccumImpulse = m_accumulatedImpulse; btScalar sum = oldaccumImpulse + clippedMotorImpulse; m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; btVector3 motorImp = clippedMotorImpulse * axis; //body0->applyTorqueImpulse(motorImp); //body1->applyTorqueImpulse(-motorImp); body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); return clippedMotorImpulse; }
real_t G6DOFRotationalLimitMotorSW::solveAngularLimits( real_t timeStep,Vector3& axis,real_t jacDiagABInv, BodySW * body0, BodySW * body1) { if (needApplyTorques()==false) return 0.0f; real_t target_velocity = m_targetVelocity; real_t maxMotorForce = m_maxMotorForce; //current error correction if (m_currentLimit!=0) { target_velocity = -m_ERP*m_currentLimitError/(timeStep); maxMotorForce = m_maxLimitForce; } maxMotorForce *= timeStep; // current velocity difference Vector3 vel_diff = body0->get_angular_velocity(); if (body1) { vel_diff -= body1->get_angular_velocity(); } real_t rel_vel = axis.dot(vel_diff); // correction velocity real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON ) { return 0.0f;//no need for applying force } // correction impulse real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse real_t 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 real_t lo = real_t(-1e30); real_t hi = real_t(1e30); real_t oldaccumImpulse = m_accumulatedImpulse; real_t sum = oldaccumImpulse + clippedMotorImpulse; m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum; clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; Vector3 motorImp = clippedMotorImpulse * axis; body0->apply_torque_impulse(motorImp); if (body1) body1->apply_torque_impulse(-motorImp); return clippedMotorImpulse; }