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; }
protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c) { //check magniture of applied impulse from SolverConstraint float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm; float deltaVel1Dotn = Vector3.Dot(c.m_contactNormal, body1.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetDeltaAngularVelocity()); float deltaVel2Dotn = -Vector3.Dot(c.m_contactNormal, body2.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetDeltaAngularVelocity()); deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; float sum = c.m_appliedImpulse + deltaImpulse; if (sum < c.m_lowerLimit) { deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; } else { c.m_appliedImpulse = sum; } Vector3 temp = c.m_contactNormal * body1.InternalGetInvMass(); body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse); temp = -c.m_contactNormal * body2.InternalGetInvMass(); body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse); }
//! 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; }
protected void SetFrictionConstraintImpulse(ref SolverConstraint solverConstraint, RigidBody rb0, RigidBody rb1, ManifoldPoint cp, ContactSolverInfo infoGlobal) { if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_FRICTION_WARMSTARTING)) { { SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0 != null) { rb0.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb0.GetInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse); } } else { frictionConstraint1.m_appliedImpulse = 0f; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1; } if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING)) { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0 != null) { rb0.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb0.GetInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse); } } else { frictionConstraint2.m_appliedImpulse = 0f; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2; } } else { SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; frictionConstraint1.m_appliedImpulse = 0f; if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS)) { SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; frictionConstraint2.m_appliedImpulse = 0f; m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2; } m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1; } }