Trajectory.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------





/* @{ */




#ifndef TRAJECTORY_H
#define TRAJECTORY_H

#ifdef MIP_HOST_APPLE
 #include <applePatch.h>
#endif

#include <iostream>
#include <math.h>
#include <Spaces.h>
#include <Time.h>
#include <Path.h>
#include <TimeLaw.h>

using namespace std;


namespace MipBaselib {
class AnalyticalTrajParam {
 private:
  static const Decimal _threshold; 
  Decimal _start;  
  Decimal _stop;  
  Decimal _V;    
  Decimal _Vsquare; 
  Decimal _Vmax;  
  Decimal _W;    
  Decimal _Wmax;  
  Decimal _root;  
  Decimal _vp;   
  Decimal _vpd;   
  Decimal _vpdd;  
 public:
  AnalyticalTrajParam(){
   setStart(0.0);
   setStop(1.0);
   setVp(0);
   setWmax(1);
  }
  AnalyticalTrajParam(Decimal start, Decimal stop){
   setStart(start);
   setStop(stop);
   setVp(0);
   setWmax(1);
  }
  AnalyticalTrajParam(Decimal Wmax){
   setStart(0.0);
   setStop(1.0);
   setVp(0);
   setWmax(Wmax);
  }
  
  AnalyticalTrajParam(const AnalyticalTrajParam& param){
   _start=param._start;
   _stop=param._stop;
   _V=param._V;
   _Vsquare=param._Vsquare;
   _Vmax=param._Vmax;
   _W=param._W; 
   _Wmax=param._Wmax;
   _root=param._root;
   _vp=param._vp;
   _vpd=param._vpd;
   _vpdd=param._vpdd;
  }
  
  AnalyticalTrajParam& operator=(const AnalyticalTrajParam& param){
   if (this != &param){
    _start=param._start;
    _stop=param._stop;
    _V=param._V;
    _Vsquare=param._Vsquare;
    _Vmax=param._Vmax;
    _W=param._W;
    _Wmax=param._Wmax;
    _root=param._root;
    _vp=param._vp;
    _vpd=param._vpd;
    _vpdd=param._vpdd;
   }
   return *this;
  }
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setStart(Decimal value){
   _start=value;
  }
  
  void setStop(Decimal value){
   _stop=value;
  }
  
  void setV(Decimal value){
   _V=value;
  }
  
  void setVsquare(Decimal value){
   _Vsquare=value;
  }
  
  void setVmax(Decimal value){
   _Vmax=value;
  }
  
  void setW(Decimal value){
   _W=value;
  }
  
  void setWmax(Decimal value){
   _Wmax=value;
  }
  
  void setRoot(Decimal value){
   _root=value;
  }
  
  void setVp(Decimal value){
   _vp=value;
  }
  
  void setVpd(Decimal value){
   _vpd=value;
  }
  
  void setVpdd(Decimal value){
   _vpdd=value;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Decimal start(){
   return _start;
  }
  
  Decimal stop(){
   return _stop;
  }
  
  Decimal V(){
   return _V;
  }
  
  Decimal Vsquare(){
   return _Vsquare;
  }
  
  Decimal Vmax(){
   return _Vmax;
  }
  
  Decimal W(){
   return _W;
  }
  
  Decimal Wmax(){
   return _Wmax;
  }
  
  Decimal root(){
   return _root;
  }
  
  Decimal vp(){
   return _vp;
  }
  
  Decimal vpd(){
   return _vpd;
  }
  
  Decimal vpdd(){
   return _vpdd;
  }
  
  Decimal threshold(){
   return _threshold;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<"start\t"<<_start<<endl;
   s<<"stop\t"<<_stop<<endl;
   s<<"V\t"<<_V<<endl;
   s<<"Vsquare\t"<<_Vsquare<<endl;
   s<<"Vmax\t"<<_Vmax<<endl;
   s<<"W\t"<<_W<<endl;
   s<<"Wmax\t"<<_Wmax<<endl;
   s<<"root\t"<<_root<<endl;
   s<<"vp\t"<<_vp<<endl;
   s<<"vpd\t"<<_vpd<<endl;
   s<<"vpdd\t"<<_vpdd<<endl;
   return s.str();
  }
};


class SampledTrajParam{
};


class Trajectory{
 private:
  Time _Tc;      
 public:
  Trajectory(){
  }
  
  Trajectory(const Trajectory& t){
   _Tc=t._Tc;
  }
  
  Trajectory& operator=(const Trajectory& t){
   if (this != &t){
    _Tc=t._Tc;
   }
   return *this;
  }
  
  void setTc(Time value){
   _Tc=value;
  }
  
  Time Tc(){
   return _Tc;
  }
  
  virtual Position getPos(Time t) = 0;
  
  virtual Position getFirstDeriv(Time t) = 0;
  
  virtual Position getSecondDeriv(Time t) = 0;
};


//AnalyticalTrajectory = x(s(t)), y(s(t)) e derivate
class AnalyticalTrajectory:public Trajectory{
 private:
  AnalyticalTrajParam _param;  
  Path* _path;    
  TimeLaw* _timeLaw; 
  Time _Tc;      
  bool _firstAssignement;

  void evaluateVW();
  
  void evaluateXY();
  
  Decimal Wsat(Decimal omega,Decimal threshold);
  
  bool singularity(Decimal value){
   _param.setRoot(sqrt((_path->evalCentredPosp(value)).x()*(_path->evalCentredPosp(value)).x() + (_path->evalCentredPosp(value)).y()*(_path->evalCentredPosp(value)).y()));
   return (_param.root()<_param.threshold());
  }
  void updateS(Time time);
 public:
  AnalyticalTrajectory(){
   setFirstAssignement();
   setPath(new Segment());
   setTimeLaw(new Stillness());
  };
  AnalyticalTrajectory(AnalyticalTrajParam param, Path* path, TimeLaw* timeLaw){
   setFirstAssignement();
   setPath(path);
   setTimeLaw(timeLaw);
   setParam(_param);
  }
  
  AnalyticalTrajectory(const AnalyticalTrajectory& t){
   _param=t._param;
   _path=t._path;
   _timeLaw=t._timeLaw;
   _Tc=t._Tc;
   _firstAssignement=t._firstAssignement;
  }
  
  AnalyticalTrajectory& operator=(const AnalyticalTrajectory& t){
   if (this != &t){
    _param=t._param;
    _path=t._path;
    _timeLaw=t._timeLaw;
    _Tc=t._Tc;
    _firstAssignement=t._firstAssignement;
   }
   return *this;
  }
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParam(AnalyticalTrajParam param){
   _param=param;
  }
  
  void setPath(Path* path){
   if (!firstAssignement()){
    delete _path;
    unsetFirstAssignement();
   }
   _path=path;
  }
  
  void setTimeLaw(TimeLaw* timeLaw){
   if (!firstAssignement()){
    delete _timeLaw;
    unsetFirstAssignement();
   }
   _timeLaw=timeLaw;
  }
  
  void setTc(Time value){
   _Tc=value;
  }
  
  void setFirstAssignement(){
   _firstAssignement=true;
  }
  
  void unsetFirstAssignement(){
   _firstAssignement=false;
  }
  
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  AnalyticalTrajParam param(){
   return _param;
  }
  
  Path* path(){
  return _path;
  }
  
  TimeLaw* timeLaw(){
   return _timeLaw;
  }
  
  Time Tc(){
   return _Tc;
  }
  
  bool firstAssignement(){
   return _firstAssignement;
  }
  
  Position getPos(Time t);
  
  Position getFirstDeriv(Time t);
  
  Position getSecondDeriv(Time t);
  
//   string print(){/TODO switch per dynamic_cast di path e timeLaw
//    stringstream s;
//    s.precision(3);
//    s.setf(ios::fixed,ios::floatfield);
//    s<<"PATH:\n"<<PathNames[_path->pathType()]<<endl;
//    s<<"TimeLaw type -> "<<TimeLawNames[_timeLaw->timeLawType()]<<endl;
//    return s.str();
//   }
};


class SampledTrajectory:public Trajectory{
 private:
  vector<Position> _pos;
  vector<Position> _posp;
  vector<Position> _pospp;
  vector<Time> _samplingInstants;
  Time _duration;
  int _addedTrajectories;
  Time _samplingTime;
  int _lastReadIndex;
  
  bool _findIndex(Time time, int &index);
  
 public:
  
  SampledTrajectory();
  
  SampledTrajectory(Time samplingTime);
  
  SampledTrajectory(SampledTrajectory &traj);
  
  void addTrajectory(AnalyticalTrajectory* traj);
  
  void getSample(Time instant,Position &pos, Position &posp, Position &pospp);
  
  void clear();
  
  Position getPos(Time t);
  
  Position getFirstDeriv(Time t);
  
  Position getSecondDeriv(Time t);
};




}; // end of namespace



#endif

/* @} */