Tabular.h

#ifndef TABULAR_CSPACE_H
#define TABULAR_CSPACE_H

#include <KrisLibrary/math3d/primitives.h>
#include <KrisLibrary/meshing/VolumeGrid.h>
#include <KrisLibrary/meshing/AreaGrid.h>
#include "CSpace.h"
#include "CSetHelpers.h"
#include "Objective.h"
using namespace Math3D;

template <class T>
class Table1D
{
public:
  Table1D(int n,Real a,Real b,
    int dim=0);
  Table1D(const std::vector<T>& values,Real a,Real b,
    int dim=0);
  T Lookup(Real x) const;
  int Index(Real x,Real* u=NULL) const;
  std::pair<Real,Real> Cell(int index) const;
  inline T Lookup(const Config& x) const { return Lookup(x[dim]); }
  inline int Index(const Config& x,Real* u=NULL) const { return Index(x[dim],u); }

  std::vector<T> values;
  Real a,b;
  int dim;
};

class Tabular1DSet : public BoxSet, public Table1D<bool>
{
public:
  Tabular1DSet(const std::vector<bool>& valid,Real a,Real b);
  Tabular1DSet(int n,Real a,Real b);
  virtual ~Tabular1DSet() {}
  virtual int NumDimensions() const { return 1; }
  virtual bool Contains(const Config& x);
  virtual bool Project(Config& x);
  virtual bool IsSampleable() const { return true; }
  virtual void Sample(Config& x);

  bool calculatedValidIndices;
  std::vector<int> validIndices;
};

class Tabular2DSet : public BoxSet, public Meshing::AreaGridTemplate<bool>
{
public:
  Tabular2DSet(const Array2D<bool>& valid,const Vector2& bmin,const Vector2& bmax);
  Tabular2DSet(int m,int n,const Vector2& bmin,const Vector2& bmax);
  virtual ~Tabular2DSet() {}
  virtual int NumDimensions() const { return 2; }
  virtual bool Contains(const Config& x);
  virtual bool Project(Config& x);
  virtual bool IsSampleable() const { return true; }
  virtual void Sample(Config& x);

  bool calculatedValidIndices;
  std::vector<IntPair> validIndices;
};

class Tabular3DSet : public BoxSet, public Meshing::VolumeGridTemplate<bool>
{
public:
  Tabular3DSet(const Array3D<bool>& valid,const Vector3& bmin,const Vector3& bmax);
  Tabular3DSet(int m,int n, int p,const Vector3& bmin,const Vector3& bmax);
  virtual ~Tabular3DSet() {}
  virtual int NumDimensions() const { return 2; }
  virtual bool Contains(const Config& x);
  virtual bool Project(Config& x);
  virtual bool IsSampleable() const { return true; }
  virtual void Sample(Config& x);

  bool calculatedValidIndices;
  std::vector<IntTriple> validIndices;
};

class Tabular1DLevelSet : public BoxSet, Table1D<Real>
{
public:
  Tabular1DLevelSet(const std::vector<Real>& f,Real a,Real b,Real threshold=0);
  virtual ~Tabular1DLevelSet() {}
  virtual int NumDimensions() const { return 1; }
  virtual bool Contains(const Config& x);
  virtual bool Project(Config& x);

  Real threshold;
};

class Tabular2DLevelSet : public BoxSet
{
public:
  Tabular2DLevelSet(const Array2D<Real>& f,const Vector2& bmin,const Vector2& bmax,Real threshold=0);
  virtual ~Tabular2DLevelSet() {}
  virtual int NumDimensions() const { return 2; }
  virtual bool Contains(const Config& x);
  virtual bool Project(Config& x);

  Meshing::AreaGridTemplate<Real> agrid;
  Real threshold;
};

class Tabular3DLevelSet : public BoxSet
{
public:
  Tabular3DLevelSet(const Array3D<Real>& f,const Vector3& bmin,const Vector3& bmax,Real threshold=0);
  virtual ~Tabular3DLevelSet() {}
  virtual int NumDimensions() const { return 3; }
  virtual bool Contains(const Config& x);
  virtual bool Project(Config& x);

  Meshing::VolumeGridTemplate<Real> vgrid;
  Real threshold;
};



class Tabular1DObjective : public ObjectiveFunctionalBase
{
public:
  Tabular1DObjective(const std::vector<Real>& differentialCosts,const std::vector<Real>& terminalCosts,
    Real a,Real b,
    int dim);
  virtual ~Tabular1DObjective() {}
  virtual const char* TypeString() { return "tabular1d"; }
  virtual std::string Description();
  virtual bool PathInvariant() const { return differentialCosts.values.empty(); }
  virtual Real IncrementalCost(const Interpolator* path);
  virtual Real TerminalCost(const Vector& qend);

  Table1D<Real> differentialCosts,terminalCosts;
  int dim;
};

class Tabular2DObjective : public ObjectiveFunctionalBase
{
public:
  Tabular2DObjective(const Array2D<Real>& differentialCosts,const Array2D<Real>& terminalCosts,
    const Math3D::Vector2& bmin,const Math3D::Vector2& bmax,
    int dim0=0,int dim1=1);
  virtual ~Tabular2DObjective() {}
  virtual const char* TypeString() { return "tabular2d"; }
  virtual std::string Description();
  virtual bool PathInvariant() const { return differentialCosts.value.empty(); }
  virtual Real IncrementalCost(const Interpolator* path);
  virtual Real TerminalCost(const Vector& qend);

  Meshing::AreaGridTemplate<Real> differentialCosts,terminalCosts;
  int dim0,dim1;
};

class Tabular3DObjective : public ObjectiveFunctionalBase
{
public:
  Tabular3DObjective(const Array3D<Real>& differentialCosts,const Array3D<Real>& terminalCosts,
    const Math3D::Vector3& bmin,const Math3D::Vector3& bmax,
    int dim0=0,int dim1=1,int dim2=2);
  virtual ~Tabular3DObjective() {}
  virtual const char* TypeString() { return "tabular23"; }
  virtual std::string Description();
  virtual bool PathInvariant() const { return differentialCosts.value.empty(); }
  virtual Real IncrementalCost(const Interpolator* path);
  virtual Real TerminalCost(const Vector& qend);

  Meshing::VolumeGridTemplate<Real> differentialCosts,terminalCosts;
  int dim0,dim1,dim2;
};







template <class T>
Table1D<T>::Table1D(int n,Real _a,Real _b,int _dim)
:values(n,T(0)),a(_a),b(_b),dim(_dim)
{}

template <class T>
Table1D<T>::Table1D(const std::vector<T>& _values,Real _a,Real _b,int _dim)
:values(_values),a(_a),b(_b),dim(_dim)
{}

template <class T>
T Table1D<T>::Lookup(Real x) const
{
  Real u;
  int i=Index(x,&u);
  //linearly interpolate by default
  if(u == 0)
    return values[i];
  else if(u > 0)
    return values[i] + u*(values[i+1]-values[i]);
  else
    return values[i] + (-u)*(values[i-1]+values[i]);
}

template <class T>
std::pair<Real,Real> Table1D<T>::Cell(int index) const
{
  return std::make_pair(a + index*(b-a)/values.size(),a + (index+1)*(b-a)/values.size());
}

template <class T>
int Table1D<T>::Index(Real x,Real* u) const
{
  if(values.size() <= 1) {
    if(u) u[0]=0;
    return 0;
  }
  if(x <= a) {
    if(u) u[0]=0;
    return 0;
  }
  if(x >= b) {
    if(u) u[0]=0;
    return (int)values.size()-1;
  }
  Real xscale = (x-a)/(b-a)*(values.size()-1);
  Real xbase = Floor(xscale);
  if(u) u[0] = xscale-xbase-0.5;
  return (int)xbase;
}

//template specialization
template <>
bool Table1D<bool>::Lookup(Real x) const 
{
  int i=Index(x);
  return values[i];
}

#endif