/// <summary>
/// Add a spring force
/// </summary>
void AddSpringForce(Body bA, Vec2 localA, Body bB, Vec2 localB, float k, float friction, float desiredDist)
{
Vec2 pA = bA.GetWorldPoint(localA);
Vec2 pB = bB.GetWorldPoint(localB);
Vec2 diff = pB - pA;
//Find velocities of attach points
Vec2 vA = bA.GetLinearVelocity() - Vec2.Cross(bA.GetWorldVector(localA), bA.GetAngularVelocity());
Vec2 vB = bB.GetLinearVelocity() - Vec2.Cross(bB.GetWorldVector(localB), bB.GetAngularVelocity());
Vec2 vdiff = vB - vA;
float dx = diff.Normalize(); //normalizes diff and puts length into dx
float vrel = vdiff.X * diff.X + vdiff.Y * diff.Y;
float forceMag = -k * (dx - desiredDist) - friction * vrel;
diff *= forceMag; // diff *= forceMag
bB.ApplyForce(diff, bA.GetWorldPoint(localA));
diff *= -1.0f;
bA.ApplyForce(diff, bB.GetWorldPoint(localB));
}
