/// <summary>
/// Apply forces to bodies.
/// </summary>
/// <param name="bodies">The list of bodies to which forces will be applied.</param>
public void Generate(IList <RigidBody> bodies)
{
Vector3 pa, pb, n;
Vector3.Transform(ref _bodyPointA, ref _bodyA.World.Combined, out pa);
Vector3.Transform(ref _bodyPointB, ref _bodyB.World.Combined, out pb);
Vector3.Subtract(ref pa, ref pb, out n);
Vector3.Subtract(ref pa, ref _bodyA.World.Position, out pa);
Vector3.Subtract(ref pb, ref _bodyB.World.Position, out pb);
float dist = n.Length();
Vector3.Divide(ref n, dist, out n);
float speed;
Vector3 va, vb;
_bodyA.GetVelocityAtPoint(ref pa, out va);
_bodyB.GetVelocityAtPoint(ref pb, out vb);
Vector3.Subtract(ref va, ref vb, out va);
Vector3.Dot(ref n, ref va, out speed);
dist -= _length;
Vector3.Multiply(ref n, dist * _k + speed * _c, out n);
_bodyB.ApplyForce(ref n, ref pb);
Vector3.Negate(ref n, out n);
_bodyA.ApplyForce(ref n, ref pa);
if (n.LengthSquared() >= Constants.Epsilon)
{
_bodyA.IsActive = true;
_bodyB.IsActive = true;
}
}