#include <dms/Vector3.h>
#include "Spring.h"

Spring::Spring(PointMass& p0,PointMass& p1,float k,float len)
    {
    p0_ = &p0;
    p1_ = &p1;
    springConstant_ = k;
    if (len >= 0)
        restLength_ = len;
    else
        restLength_ = p0_->position().distance(p1_->position());
    }


void Spring::setRestLength(float len)
    {
    restLength_ = len;
    }


void Spring::setSpringConstant(float k)
    {
    springConstant_ = k;
    }


void Spring::apply(void)
    {
    dms::Vector3 v = p1_->position() - p0_->position();
    float vlength = v.length();
    float forceMagnitude = springConstant_ * (vlength - restLength_);
    if (vlength > 0)
        v /= vlength;
    v *= forceMagnitude;
    p0_->applyForce(v);
    p1_->applyForce(-v);
    }


void Spring::drawLine(void)
    {
    glBegin(GL_LINES);
     glVertex3fv(p0_->position().vec);
     glVertex3fv(p1_->position().vec);
    glEnd();
    }