CSpace.h

#ifndef ROBOTICS_CSPACE_H
#define ROBOTICS_CSPACE_H


#include <KrisLibrary/math/vector.h>
#include <KrisLibrary/math/metric.h>
#include <KrisLibrary/utils/PropertyMap.h>
#include <memory>
#include "CSet.h"

namespace Optimization {
  class NonlinearProgram;
} //namespace Math
using namespace Math;
typedef Vector Config;
class Interpolator;
class EdgePlanner;
class CSpace;
typedef std::shared_ptr<EdgePlanner> EdgePlannerPtr;


class CSpace 
{
public:
  virtual ~CSpace() {}
  void AddConstraint(const std::string& name,CSet* constraint);
  void AddConstraint(const std::string& name,const std::shared_ptr<CSet>& constraint);
  void AddConstraint(const std::string& name,CSet::CPredicate test);
  void CopyConstraints(const CSpace* space,const std::string& prefix="");
  virtual int NumDimensions();
  virtual std::string VariableName(int i);
  virtual int NumConstraints() { return (int)constraints.size(); }
  virtual std::string ConstraintName(int i) { return constraintNames[i]; }
  virtual std::shared_ptr<CSet> Constraint(int i) { return constraints[i]; }
  virtual void Sample(Config& x)=0;
  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
  virtual bool IsFeasible(const Config&);
  virtual bool IsFeasible(const Config&,int constraint);
  virtual EdgePlannerPtr LocalPlanner(const Config& a,const Config& b);
  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int constraint);

  virtual Real Distance(const Config& x, const Config& y) { return Distance_L2(x,y); }
  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
  virtual void Midpoint(const Config& x,const Config& y,Config& out);

  virtual bool ProjectFeasible(Config& x);

  virtual Optimization::NonlinearProgram* FeasibleNumeric();

  virtual Real ObstacleDistance(const Config& a);

  virtual void Properties(PropertyMap&);

  virtual void CheckConstraints(const Config&,std::vector<bool>& satisfied);

  void GetFeasibleNames(const Config& q,std::vector<std::string>& names);
  void GetInfeasibleNames(const Config& q,std::vector<std::string>& names);
  void PrintInfeasibleNames(const Config& q,std::ostream& out=std::cout,const char* prefix="",const char* suffix="\n");

  std::vector<std::string> constraintNames;
  std::vector<std::shared_ptr<CSet> > constraints;
};

#endif