CompoundPath.hpp

//
// Definition of iSeeML::rob::CompoundPath, superclass of every 
// other ISeeML compound/complex/composed robotic paths' class.
//

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


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

class iSeeML::rob::CompoundPath : public iSeeML::rob::Path { 
  // === Object's Fields ==========================================

  // The set of basic paths is defined in sub-class, and is 
  // manipulated through piece and replace virtual methods, as well
  // as through copy operator.

  static const string ClassName; 


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

  // None, this is an abstract class with no object's fields. 

public:
  // === Description Methods ======================================

#define ISEEML_ROB_COMPATH_forall_pieces(i) \
        int i;  for(i = 1; i <= nbPieces(); i++) 

  void writeTo(ostream& O) const {  
    O << '{'; ISEEML_ROB_COMPATH_forall_pieces(i)
      O << piece(i) << (i == nbPieces() ? "}" : ", "); } 


  const iSeeML::rob::OrPtConfig& start() const  { return piece(1).start(); }

  const iSeeML::rob::OrPtConfig& end()   const 
  { return piece( nbPieces() ).end(); }


  const iSeeML::rob::BasicPath& piece(const int index) const { 
    // cout << this << "->CP::piece(" << index << ") const" << endl; 
    return getPiece(index); } 


  double length() const { 
    double res = 0; ISEEML_ROB_COMPATH_forall_pieces(i)
      res += piece(i).length(); return res; } 

  double deflection() const { 
    double res = 0; ISEEML_ROB_COMPATH_forall_pieces(i)
      res += piece(i).deflection(); return res; } 

  virtual int nbPieces() const = 0;

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

  iSeeML::rob::BasicPath& piece(const int index) { 
    // cout << this << "->CP::piece(" << index << ")" << endl; 
    return getPiece(index); } 


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

  iSeeML::rob::CurvConfig operator[](const double& s) const {
    int pieceNb = 1;   // Start from the first piece
    double arcLength = s, l = piece(pieceNb).length(); 
#ifdef ISEEML_CHECK_COMP_PATH_PRECOND
    if (s < 0)  cerr << ClassName << "::operator[]:"
             << " negative arc length " << s << endl; 
#endif
    // Search the piece corresponding to the arc length
    while (arcLength > l) 
#ifdef ISEEML_CHECK_COMP_PATH_PRECOND
      if ( pieceNb == nbPieces() ) 
    cerr << ClassName << "::operator[]:"
         << " arc length " << s << endl 
         << " bigger than length " << length() << endl; 
      else 
#endif
    { arcLength -= l; 
#ifdef ISEEML_CHECK_ARRAY_ELEMT
    if ( pieceNb == nbPieces() )  l = arcLength;  // stop 
    else 
#endif
      l = piece(++pieceNb).length(); }
    // Return the configuration in the selected piece
    return( piece(pieceNb)[arcLength] ); } // end of operator[]

protected:
  // === Various Methods ==========================================

  iSeeML::rob::BasicPath& getPiece(const int index) const { 
    // cout << this << "->CP::getPiece(" << index << ")" << endl; 
#ifdef ISEEML_CHECK_COMP_PATH_PRECOND
    if ( !nbPieces() )
      cerr << ClassName << "::piece: no pieces!!!" << endl;
    else if ( (index < 1) || ( index > nbPieces() ) )
      cerr << ClassName << "::piece: abnormal index " 
       << index << ", not in 1.." << nbPieces() << endl; 
#endif
    const int nb = 
#ifdef ISEEML_CHECK_ARRAY_ELEMT
      index < 2 ? 1 : index >= nbPieces() ? nbPieces() : 
#endif
    index;  return _piece(nb); } 

  virtual iSeeML::rob::BasicPath& _piece(const int index) const = 0;

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

#endif  // end of definition