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