EdgePlanner.h

#ifndef ROBOTICS_EDGE_PLANNER_H
#define ROBOTICS_EDGE_PLANNER_H

#include "CSpace.h"
#include "Interpolator.h"
#include <memory>
#include <list>
#include <queue>

class EdgePlanner : public Interpolator
{
public:
  EdgePlanner() {}
  virtual ~EdgePlanner() {}
  virtual bool IsVisible() =0;
  virtual CSpace* Space() const =0;
  virtual std::shared_ptr<EdgePlanner> Copy() const=0;
  virtual std::shared_ptr<EdgePlanner> ReverseCopy() const=0;

  //for incremental planners
  virtual bool IsIncremental() const { return false; } 
  virtual Real Priority() const { abort(); return 0; }
  virtual bool Plan() { abort(); return false; }
  virtual bool Done() const { abort(); return false; }
  virtual bool Failed() const { abort(); return false; }
};

typedef std::shared_ptr<EdgePlanner> EdgePlannerPtr;

class EdgeChecker : public EdgePlanner
{
public:
  EdgeChecker(CSpace* space,const InterpolatorPtr& path);
  EdgeChecker(CSpace* space,const Config& a,const Config& b);
  virtual ~EdgeChecker() {}
  virtual void Eval(Real u,Config& x) const { path->Eval(u,x); }
  virtual Real Length() const { return path->Length(); }
  virtual const Config& Start() const { return path->Start(); }
  virtual const Config& End() const { return path->End(); }
  virtual CSpace* Space() const { return space; }

  CSpace* space;
  InterpolatorPtr path;
};

typedef std::shared_ptr<EdgeChecker> EdgeCheckerPtr;


class EpsilonEdgeChecker : public EdgeChecker
{
public:
  EpsilonEdgeChecker(CSpace* space,const InterpolatorPtr& path,Real epsilon);
  EpsilonEdgeChecker(CSpace* space,const Config& a,const Config& b,Real epsilon);
  virtual bool IsVisible();
  virtual EdgePlannerPtr Copy() const;
  virtual EdgePlannerPtr ReverseCopy() const;
  virtual bool IsIncremental() const { return true; } 
  virtual Real Priority() const;
  virtual bool Plan();
  virtual bool Done() const;
  virtual bool Failed() const;

  Real epsilon;

protected:
  bool foundInfeasible;
  Real dist;
  int depth;
  int segs;
  Config m;
};

class ObstacleDistanceEdgeChecker : public EdgeChecker
{
public:
  ObstacleDistanceEdgeChecker(CSpace* space,const InterpolatorPtr& path);
  ObstacleDistanceEdgeChecker(CSpace* space,const Config& a, const Config& b);
  virtual bool IsVisible();
  virtual EdgePlannerPtr Copy() const;
  virtual EdgePlannerPtr ReverseCopy() const;

  bool CheckVisibility(Real ua,Real ub,const Config& a,const Config& b,Real da,Real db);
};

class BisectionEpsilonEdgePlanner : public EdgePlanner
{
public:
  BisectionEpsilonEdgePlanner(CSpace* space,const Config& a,const Config& b,Real epsilon);
  virtual ~BisectionEpsilonEdgePlanner() {}
  virtual bool IsVisible();
  virtual void Eval(Real u,Config& x) const;
  virtual Real Length() const;
  virtual const Config& Start() const;
  virtual const Config& End() const;
  virtual CSpace* Space() const { return space; }
  virtual EdgePlannerPtr Copy() const;
  virtual EdgePlannerPtr ReverseCopy() const;
  virtual bool IsIncremental() const { return true; } 
  virtual Real Priority() const;
  virtual bool Plan();
  virtual bool Done() const;
  virtual bool Failed() const;
  //on failure, returns the segment last checked
  bool Plan(Config*& pre,Config*& post);

  const std::list<Config>& GetPath() const { return path; }
  const Config& InfeasibleConfig() const { return x; }

protected:
  BisectionEpsilonEdgePlanner(CSpace* space,Real epsilon);

  CSpace* space;
  std::list<Config> path;
  Real epsilon;

  struct Segment
  {
    inline bool operator < (const Segment& s) const { return length<s.length; }

    std::list<Config>::iterator prev;
    Real length;
  };

  std::priority_queue<Segment,std::vector<Segment> > q;
  Config x;
};


inline EdgePlannerPtr IsVisible(CSpace* w,const Config& a,const Config& b)
{
  EdgePlannerPtr e=w->LocalPlanner(a,b);
  if(e->IsVisible()) return e;
  return EdgePlannerPtr();
}

#endif