///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1, RigidBody body2, ref IndexedVector3 pos2, float distance, ref IndexedVector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); IndexedVector3 vel = vel1 - vel2; IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose(); IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose(); 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(), body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(), body2.GetLinearVelocity(), body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity()); float a = jacDiagABInv; rel_vel = normal.Dot(ref 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,"SetupFriction-rb0"); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse, "SetupFriction-rb1"); } } 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,"SetFriction-rb0"); } if (rb1 != null) { rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse,"SetFriction-rb1"); } } 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; } }
public float SolveLinearAxis( float timeStep, float jacDiagABInv, RigidBody body1, ref IndexedVector3 pointInA, RigidBody body2, ref IndexedVector3 pointInB, int limit_index, ref IndexedVector3 axis_normal_on_a, ref IndexedVector3 anchorPos) { ///find relative velocity // IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); // IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition(); IndexedVector3 vel1 = IndexedVector3.Zero; body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1); IndexedVector3 vel2 = IndexedVector3.Zero; ; body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2); IndexedVector3 vel = vel1 - vel2; float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel); /// apply displacement correction //positional error (zeroth order error) float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a); float lo = float.MinValue; float hi = float.MaxValue; float minLimit = m_lowerLimit[limit_index]; float maxLimit = 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 = m_accumulatedImpulse[limit_index]; float sum = oldNormalImpulse + normalImpulse; m_accumulatedImpulse[limit_index] = (sum > hi ? 0f : sum < lo ? 0f : sum); normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse; //body1.applyImpulse( impulse_vector, rel_pos1); //body2.applyImpulse(-impulse_vector, rel_pos2); IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a); IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a); body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1"); body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2"); return normalImpulse; }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1, RigidBody body2, ref IndexedVector3 pos2, float distance, ref IndexedVector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); IndexedVector3 vel = vel1 - vel2; IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose(); IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose(); 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(), body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(), body2.GetLinearVelocity(), body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity()); float a = jacDiagABInv; rel_vel = normal.Dot(ref 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; } }