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;