ConstrainedInterpolator.h

#ifndef CONSTRAINED_INTERPOLATOR_H
#define CONSTRAINED_INTERPOLATOR_H

#include <KrisLibrary/math/function.h>
#include <KrisLibrary/planning/GeneralizedBezierCurve.h>
#include <KrisLibrary/optimization/Newton.h>
using namespace std;

class ConstrainedInterpolator
{
 public:
  ConstrainedInterpolator(CSpace* space,VectorFieldFunction* constraint);
  virtual ~ConstrainedInterpolator() {}
  bool Make(const Config& a,const Config& b,vector<Config>& path,bool checkConstraints=false);
  virtual void ConstraintValue(const Config& x,Vector& v);
  virtual bool Project(Config& x);

  CSpace* space;
  VectorFieldFunction* constraint;
  Config xmin,xmax; 
  VectorFieldFunction* inequalities;  
  int maxNewtonIters;
  Real ftol,xtol;
  Real maxGrowth;

  //temp: solver
  Optimization::NewtonRoot solver;
};

class SmoothConstrainedInterpolator
{
 public:
  SmoothConstrainedInterpolator(CSpace* space,VectorFieldFunction* constraint);
  virtual ~SmoothConstrainedInterpolator() {}
  bool Make(const Config& a,const Config& b,
            GeneralizedCubicBezierSpline& path,
            bool checkConstraints=false);
  bool Make(const Config& a,const Vector& da,const Config& b,const Vector& db,
            GeneralizedCubicBezierSpline& path,
            bool checkConstraints=false);
  virtual void ConstraintValue(const Config& x,Vector& v);
  virtual bool Project(Config& x);
  virtual bool ProjectVelocity(const Config& x,Config& v);

  CSpace* space;
  GeodesicSpace* manifold;
  VectorFieldFunction* constraint;
  Config xmin,xmax; 
  VectorFieldFunction* inequalities;  
  int maxNewtonIters;
  Real ftol,xtol;
  Real maxGrowth;

  //temp: solver
  Optimization::NewtonRoot solver;
};

bool MultiSmoothInterpolate(SmoothConstrainedInterpolator& interp,
                            const vector<Vector>& pts,
                            GeneralizedCubicBezierSpline& path);


bool MultiSmoothInterpolate(SmoothConstrainedInterpolator& interp,
                            const vector<Vector>& pts,
                            const Vector& dq0,const Vector& dq1,
                            GeneralizedCubicBezierSpline& path);


bool AppendInterpolate(SmoothConstrainedInterpolator& interp,
                       const Vector& pt,Real suffixDuration,
                       GeneralizedCubicBezierSpline& path);


#endif