/***************************************************************************
   File:        ODEMidpoint.cpp                                                            
   Created:     08/22/2001                                                                
   Author:      Maxim Garber                                                
                Computer Science Department                                 
                University of North Carolina - Chapel Hill                  
                garber@cs.unc.edu                                           
                                                                            
   Description: Implements the midpoint method (Runge-Kutta 2/3 ODE
                solver.                                                                            
----------------------------------------------------------------------------.
   Copyright 2001 Maxim Garber                                              
   UNC Motion Planning Research Group                                       
                                                                            
                                                                            
*****************************************************************************/
#include "ODEMidpoint.h"

// sets the system used for evaluating the function derivative
void ODEMidpoint::SetSystem(ODESystem* system)
{ 
  ODESolver::SetSystem(system);
  EulerSolver::InstancePtr()->SetSystem(system);
}

/****************************************************************************
   Function   : Integrate                                                   
   Description: Given the starting config and a timestep, this function 
                integrates the system and returns the resulting configuration.
                It uses the Derivative finction (see ODESolver.h) to access the 
                derivative field acting on the system at any configuation and time.
****************************************************************************/
void ODEMidpoint::Integrate(const ODEConfig& config, float time, float timeStep, 
                            ODEConfig& newConfig)
{

  // Take an Euler intergration step
  ODEConfig eulerConfig;
  EulerSolver::InstancePtr()->SetSystem(_system);
  EulerSolver::InstancePtr()->Integrate(config, time, timeStep, eulerConfig);

  // compute midpoint between current config and the euler step
  ODEConfig midPoint;
  for(unsigned int i=0; i<_system->GetDimension(); i++)
    midPoint.push_back(0.5*(config[i] + eulerConfig[i]));

  // get the derivative at the midpoint from the system
  ODEConfig derivative;
  _system->ODEDerivative(midPoint, time + 0.5*timeStep, derivative);

  newConfig.clear();
  for(i=0; i<_system->GetDimension(); i++)
    newConfig.push_back(config[i] + derivative[i]*timeStep); 
}