Fwd.cpp

This file checks Dubins' and FSC paths generation (their 'goto' constructors): a path of each type is built to connect two given configurations, and its characteristics (including starting and final configurations) are given.

// 
// This program verifies the correctness
//  of ISeeML' `goto' constructors for forward-only paths
// 


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

// To get the definition of ISeeML' forward paths (Dubins & FSC). 
#include <iSeeML/fwdPaths>


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

  // 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 #############################

  double x, y, th; 
  cout << "Enter the starting configuration as x y th "
       << "(in meters and degres): "; 
  cin  >> x >> y >> th; 
  iSeeML::rob::OrPtConfig start(x, y, M_PI * th / 180);

  cout << "Enter the final configuration as x y th (same): "; 
  cin  >> x >> y >> th; 
  iSeeML::rob::OrPtConfig end(x, y, M_PI * th / 180); 

  cout << endl; 


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

  // maximum curvature is deduced from minimum turning radius
  const double maxCurv = 1 / minTurnRadius; 

  // Dubins' path
  iSeeML::rob::DubinsPath DPath(start, end, maxCurv); 

  cout << "\nDubins path connecting " << start << " to "
       << end << "\n  is " << DPath << endl; 


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

  // max. curv. derivative is deduced from turning distance
  const double maxCurvDeriv = maxCurv / minTurnDist; 

  // Fsc path
  iSeeML::rob::FscPath FPath(start, end, maxCurv, maxCurvDeriv); 

  cout << "\nFSC path connecting " << start << " to "
       << end << "\n  is " << FPath << endl << endl; 

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