planning/Path.h

#ifndef MILESTONE_PATH_H
#define MILESTONE_PATH_H

#include "CSpace.h"
#include "EdgePlanner.h"
#include <list>

//forward declaration
class ObjectiveFunctionalBase;

class MilestonePath : public Interpolator
{
public:
  MilestonePath();
  ~MilestonePath();

  virtual void Eval(Real u,Config& q) const { Eval2(u,q); }
  virtual Real Length() const;
  virtual const Config& Start() const { return edges.front()->Start(); }
  virtual const Config& End() const { return edges.back()->End(); }

  const Config& GetMilestone(int milestone) const;
  void SetMilestone(int milestone,const Config& x);
  bool CheckSetMilestone(int milestone,const Config& x);
  inline CSpace* Space(int i=0) const { assert(!edges.empty()); return edges[i]->Space(); }
  inline int NumMilestones() const { return (int)edges.size()+1; }
  inline int NumEdges() const { return (int)edges.size(); }
  bool IsValid();
  void Concat(const MilestonePath& path);
  void CreateEdgesFromMilestones(CSpace* space,const std::vector<Config>& milestones);
  bool InitializeEdgePlans();
  bool IsFeasible();
  int Eval2(Real t, Config& c) const;
  int Shortcut();
  int Shortcut(ObjectiveFunctionalBase* objective);
  int Reduce(int numIters);
  int Reduce(int numIters,ObjectiveFunctionalBase* objective);
  void Splice(int start,int goal,const MilestonePath& path); 
  void Discretize(Real h);
  int DiscretizeEdge(int e,Real h);
  void DiscretizeEdge(int e,const std::vector<Real>& u);
  bool Load(std::istream& in,CSpace* space);
  bool Save(std::ostream& out);

  std::vector<EdgePlannerPtr> edges;
};

#endif