FscPath.hpp

//
// Definition of iSeeML::rob::FscPath, class of FSC paths 
// (Forward-only Sub-optimal Continuous-curvature paths).
//
 
// To only include this definition once
#ifndef ISEEML_FSC_PATH_HH
#define ISEEML_FSC_PATH_HH
 
 
// This class needs the definition of iSeeML::rob::DubinsLikePath. 
#include <iSeeML/rob/DubinsLikePath.hpp>
 
 
class iSeeML::rob::FscPath : public iSeeML::rob::DubinsLikePath {
  // === Object's Fields ==========================================
 
  double MaximumCurvDerivative; 
 
 
  double LimDefl; 
 
 
  double TurnRadius; 
 
  double TurnAngle; 
 
  // Array of nbPossiblePaths Dubins' paths. 
  static FscPath computationArray[nbPossiblePaths]; 
 
public:
  static const string ClassName; 
 
 
  // === Constructors and Destructors =============================
 
  FscPath() : DubinsLikePath(lsl, 0), MaximumCurvDerivative(0), 
    LimDefl(0), TurnRadius(-1), TurnAngle(-M_PI) {} 
 
  FscPath(const iSeeML::rob::OrPtConfig& start, const Type& type, 
      const double& maxCurv,   const double& maxCDer, 
      const double& length1,   const double& length2, 
      const double& length3) :
    // Max curv., Max curv.'s deriv. and type are directly given
    DubinsLikePath(type, fabs(maxCurv)), 
    MaximumCurvDerivative( fabs(maxCDer) ) { 
    // the rest is computed (values and pieces) 
    computeValues(maxCurv, maxCDer, 
          LimDefl, TurnRadius, TurnAngle); 
    definePieces(start, length1, length2, length3); } 
  
  FscPath(const iSeeML::rob::OrPtConfig& start, const Type& type, 
      const double& maxCurv,   const double& maxCDer, 
      const double& limDefl,   const double& turnRad, 
      const double& turnAng,   const double& length1, 
      const double& length2,   const double& length3) :
    // Max curv., Max curv.'s deriv. and type are directly given
    DubinsLikePath(type, fabs(maxCurv)), 
    MaximumCurvDerivative(fabs(maxCDer)), LimDefl( fabs(limDefl) ), 
    TurnRadius( fabs(turnRad) ), TurnAngle( fabs(turnAng) )  { 
    definePieces(start, length1, length2, length3); } 
  
  FscPath(const iSeeML::rob::OrPtConfig& start,
      const iSeeML::rob::OrPtConfig& goal, 
      const double& maxCurv,   const double& maxCDer) : 
    // set main fields values
    DubinsLikePath((Type) -1, fabs(maxCurv)), 
    MaximumCurvDerivative( fabs(maxCDer) ) { 
    // compute the other fields values
    computeValues(maxCurv, maxCDer, 
          LimDefl, TurnRadius, TurnAngle); 
    connect(start, goal); } 
 
  FscPath(const iSeeML::rob::OrPtConfig& start, 
      const iSeeML::rob::OrPtConfig& goal, 
      const double& maxCurv,   const double& maxCDer, 
      const double& limDefl, 
      const double& turnRad,   const double& turnAng) : 
    // set main fields values
    DubinsLikePath((Type) -1, fabs(maxCurv)), 
    MaximumCurvDerivative(fabs(maxCDer)), LimDefl( fabs(limDefl) ), 
    TurnRadius( fabs(turnRad) ), TurnAngle( fabs(turnAng) )
    // compute the other fields values
  { connect(start, goal); } 
 
 
  FscPath(const FscPath& other) : DubinsLikePath(other), 
    MaximumCurvDerivative( other.maxCurvDeriv() ), 
    LimDefl( other.limDefl() ), TurnRadius( other.turnRadius() ), 
    TurnAngle( other.turnAngle() ) {} 
 
 
  // === Description Methods ======================================
 
  static void computeValues(const double& maxCurv, 
                const double& maxCDer, double& limDefl, 
                double& turnRad, double& turnAng); 
 
 
  iSeeML::Object& clone() const 
  { return *( (iSeeML::Object*) (new FscPath(*this)) ); } 
 
 
  void writeTo(ostream& O) const;
 
 
  double maxCurvDeriv() const { return MaximumCurvDerivative; } 
 
 
  double limDefl() const { return LimDefl; } 
 
 
  double turnRadius() const { return TurnRadius; } 
 
  double turnAngle() const { return TurnAngle; } 
 
protected:
  bool isShortTurn(const double& defl) const 
  { return( fabs(defl) <= limDefl() ); } 
 
  int turnNbPieces(const double& defl) const 
  { return( isZero(defl) ? 1 : isShortTurn(defl) ? 2 : 3 ); } 
 
 
  virtual bool getConnection(const double& dist, 
                 double& length, double& angle) const;
 
 
  // === Modification Methods =====================================
 
  void addTurn(int& index, const iSeeML::rob::OrPtConfig **start, 
           const double& defl); 
 
public:
  // === Operators ================================================
 
  FscPath& operator=(const FscPath& other) { 
    DubinsLikePath::operator=(other); 
    MaximumCurvDerivative = other.maxCurvDeriv(); 
    LimDefl = other.limDefl();   TurnRadius = other.turnRadius(); 
    TurnAngle = other.turnAngle();  return *this; }
 
 
  // === Various Methods ==========================================
 
  void computeCenters(const iSeeML::rob::OrPtConfig& start,
              const iSeeML::rob::OrPtConfig& goal, 
              iSeeML::geom::Point centers[nbTurningCircles]) 
    const;
 
protected:
  // cf. DubinsLikePath
  iSeeML::rob::DubinsLikePath* connectArray() const { 
    // fill the array with clones of the current path
    int i; for(i = 0; i < nbPossiblePaths; i++) 
      computationArray[i] = *this; 
    // return it 
    return( (DubinsLikePath*) computationArray ); } 
 
  iSeeML::rob::DubinsLikePath& getSolution
  (iSeeML::rob::DubinsLikePath* paths, const int index) const {
    FscPath *array = (FscPath *) paths; 
#ifdef ISEEML_CHECK_FSC_PATH_PRECOND
    if ( (index < 0) || (index >= nbPossiblePaths) )
      cerr << ClassName << "::getConnection: abnormal index"
       << " parameter (" << index << ")!" << endl; 
#endif
    const int i = 
#ifdef ISEEML_CHECK_ARRAY_ELEMT
      index < 0 ? 0 : index > nbPossiblePaths ? nbPossiblePaths : 
#endif
      index;
    return array[i]; }
 
}; // end of class iSeeML::rob::FscPath
 
#endif  // end of definition