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; }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref Vector3 pos1, RigidBody body2, ref Vector3 pos2, float distance, ref Vector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } Vector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); Vector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations Vector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); Vector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); Vector3 vel = vel1 - vel2; Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform()); Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform()); JacobianEntry jac = new JacobianEntry(m1,m2,rel_pos1,rel_pos2,normal, body1.GetInvInertiaDiagLocal(),body1.GetInvMass(), body2.GetInvInertiaDiagLocal(),body2.GetInvMass()); float jacDiagAB = jac.GetDiagonal(); float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( body1.GetLinearVelocity(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1), body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2)); float a = jacDiagABInv; rel_vel = Vector3.Dot(normal,vel); //todo: move this into proper structure float contactDamping = 0.2f; if(ONLY_USE_LINEAR_MASS) { float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass()); impulse = - contactDamping * rel_vel * massTerm; } else { float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; } }
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; } }