OrPtConfig.hpp

//
// Definition of iSeeML::rob::OrPtConfig, class of ISeeML standard  
// robotic configurations.
//

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


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


class iSeeML::rob::OrPtConfig : public iSeeML::rob::Object {
  // === Object's Fields ==========================================
  
  iSeeML::geom::Point Position;  
  double Orientation;           

public:
  static const string ClassName; 


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

  OrPtConfig() : Position(), Orientation(0) {} 

  OrPtConfig(const iSeeML::geom::Point& P, const double& theta) :
    Position(P), Orientation( mod2pi(theta) ) {}

  OrPtConfig(const double& x, const double& y,
         const double& theta) :
    Position(x, y), Orientation( mod2pi(theta) ) {}

  // The default copy constructor is OK. 


  virtual ~OrPtConfig() {} 


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

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

  void writeTo(ostream& O) const { algWriteTo(O); } 


  const iSeeML::geom::Point& position() const { return Position; } 

  const double& orientation() const { return Orientation; } 

  iSeeML::geom::Vector projection(const iSeeML::geom::Point& P) const
  { return ( P - position() ).rotate( -orientation() ); } 

  OrPtConfig projection(const OrPtConfig& q) const
  { return OrPtConfig( iSeeML::geom::Point()
               + projection( q.position() ),
               q.orientation() - orientation() ); } 

protected:
  int algDimension() const { return 3; } 

  double algCoord(const int i) const { 
    switch (i) {  case 1: return position().xCoord(); 
    case 2: return position().yCoord();  
    case 3: return orientation();  default: 
#ifdef ISEEML_CHECK_ORPT_CONFIG_PRECOND
      cerr << ClassName << "::coord: parameter " << i << endl
       << " outside allowed values (1, 2 or 3)," 
       << " returning zero..." << endl;
#endif
      return 0; } } 

public:
  // === Modification Methods =====================================

  OrPtConfig& uTurn() { 
    // do not use rotate to avoid unusefull checks
    Orientation += (Orientation > 0 ? - M_PI : M_PI); 
    return *this; } 


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

  bool operator==(const OrPtConfig& other) const { 
    return( ( position() == other.position() ) && 
        isParallelTo(other) ); } 

   void operator+=(const OrPtConfig& other) { *this = *this + other; } 

   OrPtConfig operator+(const OrPtConfig& other) const {
     const double theta = orientation(); 
     iSeeML::geom::Vector v = other.position() - iSeeML::geom::Point(); 
     return( OrPtConfig(position() + v.rotate(theta), 
            other.orientation() + theta) );
   } // end of OrPtConfig operator+(const OrPtConfig&)


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

  OrPtConfig opposite() const 
  { OrPtConfig res(*this);  return res.uTurn(); } 


  double distance2(const OrPtConfig& other) const 
  { return( position().distance( other.position() ) ); } 


  bool isParallelTo(const OrPtConfig& other) const 
  { return( isZero( mod2pi( orientation() - 
                other.orientation() ) ) ); } 

  bool isSymmetricTo(const OrPtConfig& other) const
    // Cf. end of doc. comment
   { return( isZero( ( position() - other.position() ) ^  
             geom::Vector(( orientation() + 
                       other.orientation() ) / 2) 
             ) ); } 

  bool isAlignedWith(const OrPtConfig& other) const
  { return( isParallelTo(other) && isSymmetricTo(other) ); }  


  bool hasInFront(const iSeeML::geom::Point& point) const 
    // Cf. end of doc. comment
  { return( ( point - position() ) * 
        geom::Vector( orientation() ) >= 0 ); } 

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

#endif  // end of definition