robert@464: /*
robert@464:  *
robert@464:  * Simple 1-Dimensional Mass Spring Damper
robert@464:  *
robert@464:  * Christian Heinrichs 04/2015
robert@464:  *
robert@464:  */
robert@464: 
robert@464: #include "MassSpringDamper.h"
robert@464: 
robert@464: MassSpringDamper::MassSpringDamper(float mass, float spring, float damp)	{
robert@464: 
robert@464: 	_dt = 1.0/44100.0;
robert@464: 	_mass = mass;
robert@464: 	_spring = spring;
robert@464: 	_damp = damp;
robert@464: 	_position = 0;
robert@464: 	_velocity = 0;
robert@464: 
robert@464: }
robert@464: 
robert@464: void MassSpringDamper::setup()	{
robert@464: 
robert@464: }
robert@464: 
robert@464: double MassSpringDamper::update(float inForce)	{
robert@464: 
robert@464: 	// 1. calculate spring/damper forces using current position and velocity
robert@464: 
robert@464: 	double out = (_position * (double)_spring * -1) + (_velocity * (double)_damp * -1);
robert@464: 
robert@464: 	// 2. apply external force
robert@464: 
robert@464: 	out += inForce;
robert@464: 
robert@464: 	// 3. derive acceleration (a = f/m)
robert@464: 
robert@464: 	out /= (double)_mass;
robert@464: 
robert@464: 	// 4. derive velocity (v = a*dt)
robert@464: 
robert@464: 	out *= _dt;
robert@464: 
robert@464: 	// 5. apply previous velocity
robert@464: 
robert@464: 	out += _velocity;
robert@464: 
robert@464: 	// 6. save current velocity state for next iteration
robert@464: 
robert@464: 	_velocity = out;
robert@464: 
robert@464: 	// 7. derive new position (x[n] = x[n-1] + v[n]) and save for next iteration
robert@464: 
robert@464: 	out += _position;
robert@464: 	_position = out;
robert@464: 
robert@464: 	return out;
robert@464: 
robert@464: }