KinodynamicPath.h

#ifndef KINODYNAMIC_PATH_H
#define KINODYNAMIC_PATH_H

#include "EdgePlanner.h"
class KinodynamicSpace;
class ControlSpace;
class SteeringFunction;
typedef Vector State;
typedef Vector ControlInput;


class KinodynamicMilestonePath : public Interpolator
{
 public:
  KinodynamicMilestonePath();
  KinodynamicMilestonePath(const ControlInput& u,const InterpolatorPtr& path);
  virtual const Config& Start() const { return milestones.front(); }
  virtual const Config& End() const { return milestones.back(); }
  virtual void Eval(Real u,Config& q) const { Eval2(u,q); }
  virtual Real Length() const;

  bool Empty() const;
  bool IsConstant() const;
  CSpace* Space() const;
  inline int NumMilestones() const { return milestones.size(); }
  inline const Config& GetMilestone(int i) const { return milestones[i]; }

  void Clear();
  void SimulateFromControls(KinodynamicSpace* space);
  void MakePaths(ControlSpace* space);
  void MakePaths(KinodynamicSpace* space);
  void MakeEdges(KinodynamicSpace* space);
  void Append(const ControlInput& u,KinodynamicSpace* space);
  void Append(const ControlInput& u,const InterpolatorPtr& path,KinodynamicSpace* space);
  void Append(const ControlInput& u,const InterpolatorPtr& path,const EdgePlannerPtr& e);
  void Concat(const KinodynamicMilestonePath& suffix);
  bool IsValid() const;
  bool IsFeasible();

  //evaluates the state, given path parameter [0,1].  Returns the edge index
  int Eval2(Real u,Config& q) const;

  Real StartTime(int timeindex=0) const { return Start()[timeindex]; }
  Real EndTime(int timeindex=0) const { return End()[timeindex]; }
  Real Duration(int timeIndex=0) const { return milestones.back()[timeIndex] - milestones.front()[timeIndex]; }
  //evaluates the state at time t.  Returns the edge index
  int EvalTime(Real t,Config& q,int timeIndex=0) const;
  //splits the path at time t.  Note: Assumes u(timeIndex) is equal to dt!
  void SplitTime(Real t,KinodynamicMilestonePath& before,KinodynamicMilestonePath& after,int timeIndex=0) const;
  //ensures that the time is monotonically increasing
  bool IsValidTime(int timeIndex=0) const;

  int Shortcut(KinodynamicSpace* space,SteeringFunction* fn);
  int Reduce(KinodynamicSpace* space,SteeringFunction* fn,int numIters);
  void Splice(Real u1,Real u2,const KinodynamicMilestonePath& path,KinodynamicSpace* space=NULL); 
  void Splice2(int start,int goal,const KinodynamicMilestonePath& path); 

  std::vector<State> milestones;
  std::vector<ControlInput> controls;
  std::vector<InterpolatorPtr> paths;
  std::vector<EdgePlannerPtr> edges;
};

#endif