LengthFwd.cpp

This file computes paths' length of Dubins' and FSC paths to connect a random set of configurations' pairs.The mean, standart deviation, minimum and maximum value of the ratio FSC path's length on Dubins' path's length are given, as well as the percentage of ratio under 1.3.

// 
// This program estimates the length ratio between ISeeML' `goto'  
//  constructors (SFC and Dubins' paths) 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 __BORLANDC__
  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 __BORLANDC__
#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); } 


  //### Paths computations #####################################

  // variables used in the loop
  int j, nbPaths, nbShort; 
  double minRatio, maxRatio, ratioSum, squareSum;
  // maximum curvature is deduced from minimum turning radius
  const double maxCurv = 1 / minTurnRadius; 
  // 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); 

  // computation loop
  for(i = 0, nbPaths = 0, nbShort = 0, minRatio = 2, 
    maxRatio = 1, ratioSum = 0, squareSum = 0; 
      i < nbConfig; i++) 
    for(j = 0; j < nbConfig; j++) 
      if (i != j) { 
    // compute Dubins' path 
    iSeeML::rob::DubinsPath dp(configs[i], configs[j], maxCurv); 
    // compute FSC path 
    iSeeML::rob::FscPath fscp(configs[i], configs[j], maxCurv, 
                 maxCurvDeriv, limDeflect, 
                 turnRadius, turnAngle); 
    // compute the length ratio
    const double ratio = fscp.length() / dp.length(); 
    // modify the loop variables
    nbPaths++; 
    if (ratio <= 1.3)      nbShort++; 
    if (ratio < minRatio)  minRatio = ratio; 
    if (ratio > maxRatio)  maxRatio = ratio; 
    ratioSum  += ratio; 
    squareSum += ratio * ratio; } 
  // compute final values
  const double mean = ratioSum / nbPaths, 
    variance = squareSum / nbPaths - mean * mean; 
  // display the result 
  cout << "Mean of ratio on " << nbPaths << " computations: " 
       << mean << "; std deviation: " << sqrt(variance)
       << endl; 
  cout << " minimum: " << minRatio << "; maximum: " << maxRatio 
       << "; % under 1.3: " << (double)(nbShort) * 100 / nbPaths 
       << endl; 

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