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