FMMMotionPlanner.h

#ifndef PLANNING_FMM_MOTION_PLANNER_H
#define PLANNING_FMM_MOTION_PLANNER_H

#include "CSpace.h"
#include "Path.h"
#include "FMM.h"

class FMMMotionPlanner
{
 public:
  FMMMotionPlanner(CSpace* space);
  FMMMotionPlanner(CSpace* space,const Vector& bmin,const Vector& bmax,int divs = 10);
  void Init(const Config& a,const Config& b);
  bool SolveFMM();
  bool SolveAnytime(Real time);

  Vector ToGrid(const Vector& q) const;
  Vector FromGrid(const Vector& q) const;
  Vector FromGrid(const std::vector<int>& pt) const;

  CSpace* space;
  Vector bmin,bmax;
  bool dynamicDomain;
  Vector resolution;
  Config start,goal;
  ArrayND<Real> distances;
  MilestonePath solution;
  //debug: a path that failed the secondary feasibility check
  MilestonePath failedCheck;
};

#endif