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