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; }