/***************************************************************************
   File:        PBMCircleParticle.cpp                                                           
   Created:     03/31/2002                                                            
   Author:      Maxim Garber                                                
                Computer Science Department                                 
                University of North Carolina - Chapel Hill                  
                garber@cs.unc.edu                                           
                                                                            
   Description: A particle constrained to a circle.                                                           
                                                                            
----------------------------------------------------------------------------.
   Copyright 2002 Maxim Garber                                              
   UNC Collide Research Group                                               
                                                                            
                                                                            
*****************************************************************************/
#include "PBMCircleParticle.h"
#include <GL/glut.h>
#include <GL/gl.h>
#include <float.h>
#include <math.h>


/****************************************************************************
   Function   : PBMCircleParticle                      
   Description: constructor                                                            
****************************************************************************/
PBMCircleParticle::
PBMCircleParticle(float center[2], float radius, float theta, 
                  float mass, float damping, ODESolver* solver)
: ODESystem(2), _radius(radius), _solver(solver), _theta(theta), 
_mass(mass), _dampingFactor(damping)
{

  // set circle center
  for(unsigned int i=0; i<2; i++) _center[i] = center[i];

  // particle angular velocity
  _thetaDot = 0.0;

  // force on the particle in euclidean space
  _force[0] = 0.0f;
  _force[1] = -9.8*_mass;
}


/****************************************************************************
   Function   : ~PBMCircleParticle                     
   Description: destructor                                                            
****************************************************************************/
PBMCircleParticle::
~PBMCircleParticle(){}



/****************************************************************************
   Function   : GetParticlePosition                
   Description: returns the particle position                                                            
****************************************************************************/
void
PBMCircleParticle::GetParticlePosition(float& x, float& y)
{
  x = _center[0] + _radius*cosf(_theta);
  y = _center[1] + _radius*sinf(_theta);
}



/****************************************************************************
   Function   : Update                                 
   Description: Integrates the forces on the particle to update
                position and velocity
****************************************************************************/
void 
PBMCircleParticle::Update(float timeStep)
{
  //damp the velocity
  _thetaDot *= _dampingFactor;
  
  // pack an ODE configuration
  ODEConfig config;
  config.push_back(_theta);    // position
  config.push_back(_thetaDot); // velocity
  
  // solve for next configuration
  ODEConfig newConfig;
  _solver->SetSystem(this);
  _solver->Integrate(config, 0.0f, timeStep, newConfig);

  // unpack the resulting ODE configuration
  _theta    = newConfig[0];
  _thetaDot = newConfig[1];

  // clear force to just gravity
  _force[0] = 0.0f;
  _force[1] = -9.8*_mass;
}


/****************************************************************************
   Function   : ODEDerivative                          
   Description: derivative of the ODE system                                                            
****************************************************************************/
void 
PBMCircleParticle::ODEDerivative(const ODEConfig& config, float time, ODEConfig& derivative)
{
  derivative.clear();

  // read off current theta
  float theta = config[0];

  // derivative of position is velocity
  derivative.push_back(_thetaDot);

  // derivative of velocity if obtained from the force using the Lagrangian formulation
  // thetaDotDot = (1/(mass*radius))*[force_y*cos(theta) - force_x*sin(theta)]
  float thetaDotDot = (_force[1]*cosf(theta) - _force[0]*sinf(theta))/(_mass*_radius);
  derivative.push_back(thetaDotDot);
}


/****************************************************************************
   Function   : Draw                                   
   Description: rendering function, draws the circle and paricle                                                            
****************************************************************************/
void 
PBMCircleParticle::Draw()
{
  float PI = 3.14159f;

  glPushMatrix();
  // translate to cicle center position
  glTranslatef(_center[0], _center[1], 0.0f);
  

  /* draw a circle from a bunch of short lines */
  glColor3f(0.0f, 0.0f, 0.0f);
  float vectorY =_radius, vectorX = 0.0f;
  glBegin(GL_LINE_STRIP);			
  for(float angle=0.0f;angle<=(2.0f*PI);angle+=0.01f)
  {		
    glVertex2d(vectorX,vectorY);
    vectorX =(_radius*(float)sinf(angle));
    vectorY =(_radius*(float)cosf(angle));		
  }
  glEnd();

  // draw the particle
  glRotatef   (_theta*180/PI, 0.0f, 0.0f, 1.0f);
  glTranslatef(_radius, 0.0f, 0.0f);
  glColor3f(0.02, 0.6f, 0.2f);
  glutSolidSphere(0.1, 10, 10);
  
  glPopMatrix();
}