/***************************************************************************
   File:        PBMSpringMass.h                                                            
   Created:     02/12/2002                                                            
   Author:      Maxim Garber                                                
                Computer Science Department                                 
                University of North Carolina - Chapel Hill                  
                garber@cs.unc.edu                                           
                                                                            
   Description: Single spring mass object for physically based modeling simulation.                                                            
                                                                            
----------------------------------------------------------------------------.
   Copyright 2002 Maxim Garber                                              
   UNC Collide Research Group                                       
                                                                            
                                                                            
*****************************************************************************/
#include "PBMSpringMass.h"
#include "vec3fv.hpp"

#include <windows.h>
#include <math.h>
#include "GL\gl.h"
#include "GL\glu.h"
#include "GL\glut.h"


/****************************************************************************
   Function   : PBMSpringMass                              
   Description: Constructor, initializes the spring mass with a struct of 
                parameters
****************************************************************************/
PBMSpringMass::
PBMSpringMass(PBMSpringMassParams params)
// inialize ODEsystem with 6 dimensions
: ODESystem(6)
{
  // copy position, anchor, spring constant and rest length
  Copy3fv(_position, params.position);
  Copy3fv(_anchor,   params.anchor);
  _restLength = params.restLength;
  _springConstant = params.springConstant;
   
  // copy physical constants
  Copy3fv(_gravity, params.gravity);
  _mass = params.mass;
  _friction = params.friction;

  // velocity unit vector to zero
  Set3fv(_velocity,  0.0f, 0.0f, 0.0f);

  // set external forces to zero
  Set3fv(_externalForce, 0.0f, 0.0f, 0.0f);

  // initialize ODE solver
  _integrator = params.integrator;
}
  


/****************************************************************************
   Function   : Update                                     
   Description: Updates spring/mass state (position and velocity) 
                over one time step                                                             
****************************************************************************/
void 
PBMSpringMass::Update(float timeStep)
{
  // pack the ODE configuration
  ODEConfig config;
  unsigned int i;
  for(i=0; i<3;i++) config.push_back(_position[i]);
  for(i=0; i<3;i++) config.push_back(_velocity[i]);

  // call the integrator
  ODEConfig newConfig;
  _integrator->SetSystem(this);
  _integrator->Integrate(config, 0.0, timeStep, newConfig);

  // unpack the ODE configuration
  for(i=0; i<3;i++) _position[i] = newConfig[i  ];
  for(i=0; i<3;i++) _velocity[i] = newConfig[i+3];
}


/****************************************************************************
   Function   : Draw                                       
   Description: Draws the spring mass object                                                            
****************************************************************************/
void 
PBMSpringMass::Draw()
{  
   //draw archor sphere
   glPushMatrix();
   glColor3f(0.3f, 0.3f, 0.3f);
   glTranslatef(_anchor[0], _anchor[1], _anchor[2]);
   glutSolidSphere(0.25, 20, 20);
   glPopMatrix();

   // draw spring line
   glBegin(GL_LINES);
   glVertex3fv(_anchor);
   glVertex3fv(_position);
   glEnd();

   // draw position sphere
   glPushMatrix();
   glColor3f(1.0f, 0.3f, 0.3f);
   glTranslatef(_position[0], _position[1], _position[2]);
   glutSolidSphere(0.25, 20, 20);
   glPopMatrix();


}


/****************************************************************************
   Function   : ODEDerivative                              
   Description: A Callback for the ODE integrator which provides the
                integrator with the derivative for the state (position and 
                velocity) of the spring/mass.
****************************************************************************/
void 
PBMSpringMass::ODEDerivative(const ODEConfig& config, float time, ODEConfig& derivative)
{
  derivative.clear();

  unsigned int i;
  /////////////////////////////////////////
  // derivative of position is the velocity
  for(i=3;i<6;i++) derivative.push_back(config[i]);

  /////////////////////////////////////////
  // derivative of velocity is acceleration, or force / mass
  float accel[3];
  
  // gravity
  Copy3fv(accel, _gravity);

  // spring force
  float seperation[3];
  Subtract3fv(seperation, _anchor, _position);
  float springForceMagnitude = _springConstant*(Length3fv(seperation) - _restLength);
  Normalize3fv(seperation);
  for(i=0; i<3;i++) accel[i] += (seperation[i]*springForceMagnitude)/_mass;

  // air friction 
  // is proportional to velocity
  for(i=0; i<3;i++) accel[i] -= (_friction*config[i+3])/_mass;
  
  // external forces
  for(i=0; i<3;i++) accel[i] += _externalForce[i] / _mass;
  Set3fv(_externalForce, 0.0f, 0.0f, 0.0f);
  
  // pack force into derivative
  for(i=0; i<3;i++) derivative.push_back(accel[i]); 
}