DubinsLikePath.hpp

//
// Definition of iSeeML::rob::DubinsLikePath, superclass 
// of every ISeeML robotic paths similar 
// to @ref iSeeML::rob::DubinsPath "Dubins paths".
//

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


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


class iSeeML::rob::DubinsLikePath : public iSeeML::rob::CompoundPath 
{ 

  // === Inner Type Definitions ===================================
public:
  enum Type {
    lsl, 
    lsr, 
    rsl, 
    rsr, 
    lrl, 
    rlr  }; 

  enum {
    nbPossiblePaths = 6, 
    nbTurningCircles = 4  }; 

private:
  // === Object's Fields ==========================================

  static const string ClassName; 


  int NbPieces; 
  iSeeML::rob::LinCurvPath *LcPieces; 

  Type PathType;   

  double MaximumCurvature; 
  
protected:
  // === Constructors and Destructor ==============================

  DubinsLikePath(const Type& type, const double& maxCurv) :
    NbPieces(0), LcPieces(NULL), PathType(type), 
    MaximumCurvature(fabs(maxCurv)) {} 

  DubinsLikePath(const DubinsLikePath& other) : 
    NbPieces(0), LcPieces(NULL) { *this = other; } 


  ~DubinsLikePath() { if (LcPieces != NULL)  delete[] LcPieces; } 


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

  static int turnSign(const int number, const Type& type) { 
    static int values[3][nbPossiblePaths] = 
    { {1,  1, -1, -1,  1, -1}, {0,  0,  0,  0, -1,  1}, 
      {1, -1,  1, -1,  1, -1} }; 
#ifdef ISEEML_CHECK_DLIKE_PATH_PRECOND
    if ( (number < 1) || (number > 3) )
      cerr << ClassName << "::turnSign: abnormal number " 
       << number << endl; 
#endif
    const int nb = 
#ifdef ISEEML_CHECK_ARRAY_ELEMT
      number < 1 ? 0 : (number > 3) ? 2 : 
#endif
      number - 1; 
    return(values[nb][(int) type]); } // end of int turnSign(...)


public:
  // cf. CompoundPath 
  int nbPieces() const { return NbPieces; } 

  const Type&   type()     const { return PathType; }

  const double& maxCurv()  const { return MaximumCurvature; }


  const iSeeML::rob::LinCurvPath& lcPiece(const int index) const {
    return( *((LinCurvPath*) &( piece(index) )) ); } 


  virtual double turnRadius() const = 0; 


protected:
  virtual int turnNbPieces(const double& length) const = 0; 


  virtual bool getConnection(const double& dist, double& length, 
                 double& angle) const = 0; 


  // === Modification Methods =====================================

  iSeeML::rob::LinCurvPath& lcPiece(const int index) {
    return( *((LinCurvPath*) &( piece(index) )) ); } 

private:
  void reset(const int newNbPieces, 
         const iSeeML::rob::LinCurvPath& stdPath) { 
    // cout << this << "->DLP::reset(" << newNbPieces << ")..." << endl; 
    // free previous LinCurvPath array
    if (LcPieces != NULL)  delete[] LcPieces; 
#ifdef ISEEML_CHECK_DLIKE_PATH_PRECOND
    if (newNbPieces < 0) 
      cerr << ClassName << "::reset: negative nb of pieces," 
       << endl << "  considered as zero!" << endl; 
#endif
    NbPieces = newNbPieces > 0 ? newNbPieces : 0; 
    if (NbPieces) { 
      LcPieces = new LinCurvPath[NbPieces]; 
#ifdef ISEEML_CHECK_NULL_POINTER
      if (LcPieces == NULL) { 
    cerr << ClassName << "reset: out of memory!!!" << endl; 
    exit(-1); }
#endif
      ISEEML_ROB_COMPATH_forall_pieces(i)  lcPiece(i) = stdPath; } 
    else
      LcPieces = NULL; }


  void definePieces(const iSeeML::rob::OrPtConfig& start,
            const iSeeML::rob::OrPtConfig& goal, 
            const iSeeML::geom::Point 
            centers[nbTurningCircles]); 
protected:
  void setNoPiece(const iSeeML::rob::OrPtConfig& start) {
    reset( 1, iSeeML::rob::LinCurvPath(iSeeML::rob::CurvConfig
                       (start, 0), 0, 0) ); } 

  void definePieces(const iSeeML::rob::OrPtConfig& start, 
            const double& defl1, const double& lenDefl, 
            const double& defl3); 


  virtual void addTurn(int& index, 
               const iSeeML::rob::OrPtConfig **start, 
               const double& defl) = 0; 

  void addPiece(int& index, const iSeeML::rob::CurvConfig& start, 
        const double& curvDeriv, const double& length) {
    // add the new piece and change next piece's index
    lcPiece(++index) = LinCurvPath( start, curvDeriv, 
                    fabs(length) );  } 

public:
  void connect(const iSeeML::rob::OrPtConfig& start, 
           const iSeeML::rob::OrPtConfig& goal); 


  // === Operators ================================================

  DubinsLikePath& operator=(const DubinsLikePath& other) { 
    NbPieces = other.nbPieces();   PathType = other.type(); 
    // free previous LinCurvPath array
    if (LcPieces != NULL)  delete[] LcPieces; 
    if (NbPieces) { 
      LcPieces = new LinCurvPath[NbPieces]; 
#ifdef ISEEML_CHECK_NULL_POINTER
      if (LcPieces == NULL) { 
    cerr << ClassName << "operator=: out of memory!!!" 
         << endl;  exit(-1); }
#endif
      ISEEML_ROB_COMPATH_forall_pieces(i) 
    lcPiece(i) = other.lcPiece(i);  } 
    else
      LcPieces = NULL;
    MaximumCurvature = other.maxCurv();  return *this; }


  // === Various Methods ==========================================

  virtual void computeCenters(const iSeeML::rob::OrPtConfig& start,
                  const iSeeML::rob::OrPtConfig& goal, 
                  iSeeML::geom::Point 
                  centers[nbTurningCircles]) const 
    = 0; 

protected:
  // cf. CompoundPath
  iSeeML::rob::BasicPath& _piece(const int index) const {
    // cout << this << "->DLP::_piece(" << index << ")" << endl; 
    return LcPieces[index - 1]; }


  virtual DubinsLikePath* connectArray() const = 0;

  virtual DubinsLikePath& 
  getSolution(DubinsLikePath* paths, const int index) const = 0;

private:
  int bestPathIndex(DubinsLikePath* paths) const {
    // Compare lsl and lsr paths and get the shortest's index
    bool test = 
      getSolution(paths, lsl) < getSolution(paths, lsr);
    const int ls   = test ? lsl : lsr; 
    // Compare rsl and rsr pathss and get the shortest's index
    test = getSolution(paths, rsl) < getSolution(paths, rsr);
    const int rs   = test ? rsl : rsr; 
    // Compare lrl and rlr pathss and get the shortest's index
    test = getSolution(paths, lrl) < getSolution(paths, rlr);
    const int ccc  = test ? lrl : rlr;
    // Compare ls* and rs* pathss and get the shortest's index
    test = getSolution(paths, ls) < getSolution(paths, rs);
    const int csc  = test ? ls  : rs; 
    // get the index of the shortest between all
    test = getSolution(paths, csc) < getSolution(paths, ccc);
    const int best = test ? csc : ccc;
    return best; } // end of int bestPathIndex(DubinsLikeArray&) 

}; // end of class iSeeML::rob::DubinsLikePath

#endif  // end of definition