public float SolveLinearAxis( float timeStep, float jacDiagABInv, RigidBody body1, ref Vector3 pointInA, RigidBody body2, ref Vector3 pointInB, int limit_index, ref Vector3 axis_normal_on_a, ref Vector3 anchorPos) { ///find relative velocity // Vector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // Vector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); Vector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition(); Vector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition(); Vector3 vel1 = Vector3.Zero; body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1,ref vel1); Vector3 vel2 = Vector3.Zero;; body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2,ref vel2); Vector3 vel = vel1 - vel2; float rel_vel = Vector3.Dot(axis_normal_on_a,vel); /// apply displacement correction //positional error (zeroth order error) float depth = -Vector3.Dot((pointInA - pointInB),axis_normal_on_a); float lo = float.MinValue; float hi = float.MaxValue; float minLimit = MathUtil.VectorComponent(ref m_lowerLimit,limit_index); float maxLimit = MathUtil.VectorComponent(ref m_upperLimit,limit_index); //handle the limits if (minLimit < maxLimit) { { if (depth > maxLimit) { depth -= maxLimit; lo = 0f; } else { if (depth < minLimit) { depth -= minLimit; hi = 0f; } else { return 0.0f; } } } } float normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; float oldNormalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index); float sum = oldNormalImpulse + normalImpulse; MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index,(sum > hi ? 0f: sum < lo ? 0f : sum)); normalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index) - oldNormalImpulse; Vector3 impulse_vector = axis_normal_on_a * normalImpulse; //body1.applyImpulse( impulse_vector, rel_pos1); //body2.applyImpulse(-impulse_vector, rel_pos2); Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1,axis_normal_on_a); Vector3 ftorqueAxis2 = Vector3.Cross(rel_pos2,axis_normal_on_a); body1.InternalApplyImpulse(axis_normal_on_a*body1.GetInvMass(), Vector3.TransformNormal(ftorqueAxis1,body1.GetInvInertiaTensorWorld()),normalImpulse); body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), Vector3.TransformNormal(ftorqueAxis2,body2.GetInvInertiaTensorWorld()), -normalImpulse); return normalImpulse; }
//! apply the correction impulses for two bodies public float SolveAngularLimits(float timeStep,ref Vector3 axis, float jacDiagABInv,RigidBody body0, RigidBody body1) { if (NeedApplyTorques() == false) { return 0.0f; } float target_velocity = m_targetVelocity; float 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 Vector3 angVelA = Vector3.Zero; body0.InternalGetAngularVelocity(ref angVelA); Vector3 angVelB = Vector3.Zero; body1.InternalGetAngularVelocity(ref angVelB); Vector3 vel_diff = angVelA-angVelB; float rel_vel = Vector3.Dot(axis,vel_diff); // correction velocity float motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); if ( motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON ) { return 0.0f;//no need for applying force } // correction impulse float unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse float 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 float lo = float.MinValue; float hi = float.MaxValue; float oldaccumImpulse = m_accumulatedImpulse; float sum = oldaccumImpulse + clippedMotorImpulse; m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum; clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; Vector3 motorImp = clippedMotorImpulse * axis; //body0.applyTorqueImpulse(motorImp); //body1.applyTorqueImpulse(-motorImp); body0.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body0.GetInvInertiaTensorWorld()),clippedMotorImpulse); body1.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body1.GetInvInertiaTensorWorld()),-clippedMotorImpulse); return clippedMotorImpulse; }