FMM.h

#ifndef PLANNING_FMM_H
#define PLANNING_FMM_H

#include <KrisLibrary/structs/arraynd.h>
#include <KrisLibrary/math/vector.h>
using namespace Math;

bool FMMSearch(const std::vector<int>& start,const std::vector<int>& goal,const ArrayND<Real>& costs,ArrayND<Real>& distances);

bool FMMSearch(const Vector& start,const Vector& goal,const ArrayND<Real>& costs,ArrayND<Real>& distances);

bool FMMSearch(const Vector& start,const Vector& goal,
               const Vector& bmin,const Vector& bmax,const Vector& res,
               Real (*costFn) (const Vector& coords),
               ArrayND<Real>& distances);

std::vector<Vector> GradientDescent(const ArrayND<Real>& field,const Vector& start);

#endif