public Vector3 Collide(Contact c) { /// IMPLEMENTATION EXPLANATION /// point p is the collision point /// vector n is the collision normal and it defines the 'collision plane' /// the relative velocity vector and n are in another plane called the 'parallel plane" and it is defined by a vector called nPerp (vRel % n = nPerp) /// point a is the cm of body a /// point b is the cm of body b /// imagine the collision occuring in the frame where body b is fixed IBody a = c.BodyA; IBody b = c.BodyB; Intersection intersection = c.Intersection; IKinematics kinematicsA = a.Dynamics.Kinematics; IKinematics kinematicsB = b.Dynamics.Kinematics; Vector3 aToP = intersection.Point - kinematicsA.Transform.Pos; Vector3 bToP = intersection.Point - kinematicsB.Transform.Pos; // the velocity of p relative to both bodies Vector3 aVelP = kinematicsA.SurfaceVelocity(aToP); Vector3 bVelP = kinematicsB.SurfaceVelocity(bToP); Vector3 vRel = aVelP - bVelP; // the relative velocity of the colliding points double vRelPerp = vRel * intersection.Normal.Inverse; // negative means A is moving towards the plane (colliding) if (vRelPerp > CollidingThresholdSpeed) return Vector3.Zero; // objects are receding double scalarNormalImpulse = NormalImpulse(vRelPerp, a, b, aToP, bToP, intersection.Normal); Vector3 normalImpulse = scalarNormalImpulse * intersection.Normal.Inverse; Vector3 frictionImpulse = Vector3.Zero; Vector3 nPerp = (vRel % intersection.Normal).UnitDirection; // will be zero if vRel is completely normal to collision plane and therefore no friction component if (nPerp.MagSquared != 0) // there is motion in collision plane -> friction exists { // the normalized projectio0n of vRel onto the collision plane // it lies in the collision plane and defines the paralell plane Vector3 nPar = (intersection.Normal % nPerp).UnitDirection; double vRelPar = vRel * nPar; double fric = FrictionImpulse(scalarNormalImpulse, vRelPar, a.Material, b.Material); double maxFric = MaxFrictionImpulse(a, b, vRelPar, nPar, nPerp, scalarNormalImpulse, aToP, bToP); if (Math.Abs(fric) > Math.Abs(maxFric)) fric = maxFric; frictionImpulse = fric * nPar.Inverse; } Validate(frictionImpulse); Validate(normalImpulse); return frictionImpulse + normalImpulse; }
public Vector3 Collide(Contact c) => Vector3.Zero;