/***************************************************************************
   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 standard Euler method ODE solver.                                                                            
----------------------------------------------------------------------------.
   Copyright 2001 Maxim Garber                                              
   UNC Motion Planning Research Group                                       
                                                                            
                                                                            
*****************************************************************************/
#include "stdio.h"
#include "ODEEuler.h"


/****************************************************************************
   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 ODEEuler::Integrate(const ODEConfig& config, float time, float timeStep, 
                         ODEConfig& newConfig)
{
  // get the derivative from the system
  ODEConfig derivative;
  _system->ODEDerivative(config, time, derivative);

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