//response between two dynamic objects without friction, assuming 0 penetration depth public static float ResolveSingleCollision( RigidBody body1, CollisionObject colObj2, ref IndexedVector3 contactPositionWorld, ref IndexedVector3 contactNormalOnB, ContactSolverInfo solverInfo, float distance) { RigidBody body2 = RigidBody.Upcast(colObj2); IndexedVector3 normal = contactNormalOnB; IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin; IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin; IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2 != null?body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero; IndexedVector3 vel = vel1 - vel2; float rel_vel = normal.Dot(ref vel); float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution(); float restitution = combinedRestitution * -rel_vel; float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep; float velocityError = -(1.0f + restitution) * rel_vel; // * damping; float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal); float denom1 = body2 != null?body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f; float relaxation = 1.0f; float jacDiagABInv = relaxation / (denom0 + denom1); float penetrationImpulse = positionalError * jacDiagABInv; float velocityImpulse = velocityError * jacDiagABInv; float normalImpulse = penetrationImpulse + velocityImpulse; normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse; body1.ApplyImpulse(normal * (normalImpulse), rel_pos1); if (body2 != null) { body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2); } return(normalImpulse); }