TimeFwd.cpp

This file estimates computation time for Dubins' and FSC paths generation (their 'goto' constructors): a set of configurations is randomly generated, and paths connecting any two of these configurations are computed.Global computation time is estimated, and divided by the number of path generations to obtain average computation time of 'goto' constructors for each type of path.

// 
// This program estimates the computation time 
//  of ISeeML' `goto' constructor for forward-only paths
// 


// For Borland C++
#ifdef __BORLANDC__
#include <condefs.h>
#endif

// To get the definition of time
#include <time.h>           
// To get the definition of random and srandom
#include <stdlib.h>           
// To get the definition of ISeeML' forward paths (Dubins & FSC). 
#include <iSeeML/fwdPaths>


int main() 
{
  //### Constant definitions ###################################

  // number of configurations (~ sqrt of nb of computations) 
  const int nbConfig = 1000; 
  // size of the workspace (configurations have (x, y, th) 
  //  coordinates with 0 <= x <= xSize, 0 <= y <= ySize and
  //  - pi < th <= pi)
  const double xSize = 50, ySize = 50; 
  // minimum turning radius (clearer than maximum curvature)
  const double minTurnRadius = 5; 
  // turning distance (dist. to reach min. turn. rad.)
  const double minTurnDist   = 5; 


  //### Configurations definitions #############################

  // random seed initialization
#ifdef WIN32
  srand((unsigned int) time(NULL));
#else
  srandom((unsigned int) time(NULL));
#endif
  // maximum random value
  const int maxRnd = ~(01 << 31); 
  // configurations array allocation
  iSeeML::rob::OrPtConfig configs[nbConfig]; 
  // configuration's initialisations
  int i; 
#ifdef WIN32
#define random rand
#endif
  for(i = 0; i < nbConfig; i++) {
    // coordinates are randomly taken
    const double x = xSize * random() / maxRnd, 
      y  = ySize * random() / maxRnd, 
      th = (2.0 * random() / maxRnd - 1) * M_PI; 
    // configurations is defined
    configs[i] = iSeeML::rob::OrPtConfig(x, y, th); } 


  //### Dubins' paths computations #############################

  clock_t ctime; 
  double time, atime; 
  int j, nbPaths; 
  // maximum curvature is deduced from minimum turning radius
  const double maxCurv = 1 / minTurnRadius; 

  // starting time
  ctime = clock(); 
  // computation loop
  for(i = 0, nbPaths = 0; i < nbConfig; i++) 
    for(j = 0; j < nbConfig; j++) 
      if (i != j) { 
    // compute the path (and forget it immediately)
    iSeeML::rob::DubinsPath path(configs[i], configs[j], 
                    maxCurv); 
    // increment the paths' counter
    nbPaths++; } 
  // computation time
  ctime = clock() - ctime; 
  time  = ctime * 1.0 / CLOCKS_PER_SEC; 
  atime = time * 1000 / nbPaths; 
  cout << "Elapsed time for " << nbPaths 
       << " Dubins' paths computation: \n\t" 
       << time << " s, average "<< atime << " ms\n"; 


  //### FSC paths computations #################################

  // max. curv. derivative is deduced from turning distance
  const double maxCurvDeriv = maxCurv / minTurnDist; 
  // limit defl., turning radius and turning angle 
  // are computed (once and for all)
  double limDeflect, turnRadius, turnAngle; 
  iSeeML::rob::FscPath::computeValues(maxCurv, maxCurvDeriv, 
                     limDeflect, turnRadius, 
                     turnAngle); 
  // starting time
  ctime = clock(); 
  // computation loop
  for(i = 0, nbPaths = 0; i < nbConfig; i++) 
    for(j = 0; j < nbConfig; j++) 
      if (i != j) { 
    // compute the path (and forget it immediately)
    iSeeML::rob::FscPath path(configs[i], configs[j], 
                 maxCurv, maxCurvDeriv, 
                 limDeflect, turnRadius, 
                 turnAngle); 
    // increment the paths' counter
    nbPaths++; } 
  // computation time
  ctime = clock() - ctime; 
  time  = ctime * 1.0 / CLOCKS_PER_SEC; 
  atime = time * 1000 / nbPaths; 
  cout << "Elapsed time for " << nbPaths 
       << " FSC paths computation: \n\t" 
       << time << " s, average "<< atime << " ms\n"; 

#ifdef WIN32
  system(pause);
#endif
  return(0); 
} // end of main()