/***************************************************************************
   File:        PBMProjectile.h                                                            
   Created:     02/06/2002                                                            
   Author:      Maxim Garber                                                
                Computer Science Department                                 
                University of North Carolina - Chapel Hill                  
                garber@cs.unc.edu                                           
                                                                            
   Description: Projectile object for physically based modeling simulation.                                                            
                                                                            
----------------------------------------------------------------------------.
   Copyright 2001 Maxim Garber                                              
   UNC Motion Planning Research Group                                       
                                                                            
                                                                            
*****************************************************************************/
#include "PBMProjectile.h"
#include "vec3fv.hpp"

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


/****************************************************************************
   Function   : PBMProjectile                              
   Description: Constructor, initializes launch parameters                                                            
****************************************************************************/
PBMProjectile::
PBMProjectile(PBMProjectileLaunchParams params)
// inialize ODEsystem with 6 dimensions
: ODESystem(6)
{
  // copy launch position
  Copy3fv(_position, params.position);
   
  // copy physical constants
  Copy3fv(_gravity, params.gravity);
  _mass = params.projectileMass;
  _friction = params.friction;

  // compute velocity unit vector
  _velocity[0] = cosf(params.azimuth)*cosf(params.elevation);
  _velocity[1] = sinf(params.elevation);
  _velocity[2] = sinf(params.azimuth)*cosf(params.elevation);


  // compute launch muzzle velocity
  // - assume a 1m muzzle with constant force applied along its length
  // - the force is 10000N per mass of powder
  // - equation for final velocity is v = (2*Force/mass * muzzleLength)^0.5
  float velocityMagnitude = sqrtf(2*(10000*params.powderMass)/_mass);

  // compute launch force magnitude
  ScalarMult3fv(_velocity, _velocity, velocityMagnitude);

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

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


/****************************************************************************
   Function   : Update                                     
   Description: Updates projectile position and velocity over one time step.
****************************************************************************/
void 
PBMProjectile::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 projectile                                                            
****************************************************************************/
void 
PBMProjectile::Draw()
{  
   //draw sphere
   glPushMatrix();
   glColor3f(0.3f, 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 projectile.
****************************************************************************/
void 
PBMProjectile::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);

  // 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]); 
}