DubinsPath.hpp

//
// Definition of iSeeML::rob::DubinsPath, class of Dubins' paths 
// (optimal forward-only paths with discontinuous curvature 
//  profile).
//

// To only include this definition once
#ifndef ISEEML_DUBINS_HH
#define ISEEML_DUBINS_HH


// This class needs the definition of iSeeML::rob::DubinsLikePath. 
#include <iSeeML/rob/DubinsLikePath.hpp>


class iSeeML::rob::DubinsPath : public iSeeML::rob::DubinsLikePath {
  // === Object's Fields ==========================================

  // Array of nbPossiblePaths Dubins' paths. 
  static DubinsPath computationArray[nbPossiblePaths]; 

public: 
  static const string ClassName; 


  // === Constructors and Destructors =============================

  DubinsPath() : DubinsLikePath(lsl, 0) {} 

  DubinsPath(const iSeeML::rob::OrPtConfig& start, const Type& type, 
         const double& maxCurv,  const double& length1, 
         const double& length2,  const double& length3) :
    // Maximum curvature and type are directly given
    DubinsLikePath(type, fabs(maxCurv)) 
  { definePieces(start, length1, length2, length3); } 
  
  DubinsPath(const iSeeML::rob::OrPtConfig& start, const iSeeML::rob::OrPtConfig& goal,
         const double& maxCurv) : 
    DubinsLikePath((Type) -1, fabs(maxCurv)) 
  { connect(start, goal); } 


  DubinsPath(const DubinsPath& other) : DubinsLikePath(other) {} 


  // === Description Methods ======================================

  iSeeML::Object& clone() const 
  { return *( (iSeeML::Object*) (new DubinsPath(*this)) ); } 


  void writeTo(ostream& O) const;


  double turnRadius() const { return( 1 / maxCurv() ); } 

protected:
  int turnNbPieces(const double& defl) const  { 
    return( isZero(defl) ? 0 : 1 ); } 


  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 ================================================

  DubinsPath& operator=(const DubinsPath& other) 
  { DubinsLikePath::operator=(other);  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 {
    DubinsPath *array = (DubinsPath *) paths; 
#ifdef ISEEML_CHECK_DUBINS_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::DubinsPath


#endif  // end of definition