CSpaceHelpers.h

#ifndef CSPACE_HELPERS_H
#define CSPACE_HELPERS_H

#include "CSpace.h"
#include "GeodesicSpace.h"

class GeodesicCSpace : public CSpace, public GeodesicSpace
{
public:
  GeodesicCSpace() {}
  virtual ~GeodesicCSpace() {}
};

class GeodesicCSpaceAdaptor : public GeodesicCSpace
{
public:
  GeodesicCSpaceAdaptor(const std::shared_ptr<GeodesicSpace>& geodesic);
  virtual int NumDimensions() { return geodesic->NumDimensions(); }
  virtual int NumIntrinsicDimensions() { return geodesic->NumIntrinsicDimensions(); }
  virtual Real Distance(const Config& x, const Config& y) { return geodesic->Distance(x,y); }
  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) { geodesic->Interpolate(x,y,u,out); }
  virtual void Midpoint(const Config& x,const Config& y,Config& out) { geodesic->Interpolate(x,y,0.5,out); }

  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx) { geodesic->InterpolateDeriv(a,b,u,dx); }
  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx) { geodesic->InterpolateDerivA(a,b,u,da,dx); }
  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx) { geodesic->InterpolateDerivB(a,b,u,db,dx); }
  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx) { geodesic->InterpolateDeriv2(a,b,u,ddx); }
  virtual void Integrate(const Config& a,const Vector& da,Config& b) { geodesic->Integrate(a,da,b); }

  std::shared_ptr<GeodesicSpace> geodesic;
};

class CartesianCSpace : public GeodesicCSpace
{
public:
  CartesianCSpace(int d);
  virtual int NumDimensions() { return d; }
  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
  virtual Real Distance(const Config& x, const Config& y) { return CSpace::Distance(x,y); }
  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) { CSpace::Interpolate(x,y,u,out); }
  virtual void Midpoint(const Config& x,const Config& y,Config& out) { out.add(x,y); out.inplaceMul(0.5); }

  virtual void Sample(Config& q) { FatalError("Not implemented"); }

  int d;
};

class BoxCSpace : public CartesianCSpace
{
public:
  BoxCSpace(Real xmin,Real xmax,int d=1);
  BoxCSpace(const Vector& bmin,const Vector& bmax);
  void SetDomain(const Vector& bmin,const Vector& bmax);
  void GetDomain(Vector& bmin,Vector& bmax);
  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
  virtual void Sample(Config& x);
  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
  virtual bool ProjectFeasible(Config& x);
  virtual void Properties(PropertyMap&);

  Vector bmin,bmax;
};


class MultiCSpace : public GeodesicCSpace
{
public:
  MultiCSpace();
  MultiCSpace(const std::shared_ptr<CSpace>& space1,const std::shared_ptr<CSpace>& space2);
  MultiCSpace(const std::vector<std::shared_ptr<CSpace> >& components);
  void Add(const std::string& name,const std::shared_ptr<CSpace>& space,Real distanceWeight=1);

  inline bool AreSubspacesIndependent() const { return constraints.empty(); }

  void FlattenConstraints();
  void AddConstraint(int spaceIndex,const std::string& name,CSet* constraint);
  void AddConstraint(int spaceIndex,const std::string& name,const std::shared_ptr<CSet>& constraint);
  void AddConstraint(int spaceIndex,const std::string& name,CSet::CPredicate test);

  void Split(const Vector& x,std::vector<Vector>& items);
  void SplitRef(const Vector& x,std::vector<Vector>& items);
  void Join(const std::vector<Vector>& items,Vector& x);

  bool IsFeasible_Independent(const Config&);
  EdgePlannerPtr LocalPlanner_Independent(const Config& a,const Config& b);
  EdgePlannerPtr PathChecker_Independent(const Config& a,const Config& b);
  EdgePlannerPtr PathChecker_Independent(const Config& a,const Config& b,int constraint);

  //CSpace overloads
  virtual int NumDimensions();
  virtual int NumIntrinsicDimensions();
  virtual std::string VariableName(int i);
  virtual int NumConstraints();
  virtual std::string ConstraintName(int i);
  virtual std::shared_ptr<CSet> Constraint(int i);
  virtual void Sample(Config& x);
  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
  virtual bool IsFeasible(const Config& q);
  virtual bool ProjectFeasible(Config& x);
  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 obstacle);

  virtual Optimization::NonlinearProgram* FeasibleNumeric();
  virtual Real Distance(const Config& x, const Config& 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 Real ObstacleDistance(const Config& a);
  virtual void Properties(PropertyMap&);

  //GeodesicSpace overloads
  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx);
  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx);
  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx);
  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx);
  virtual void Integrate(const Config& a,const Vector& da,Config& b);

  std::vector<std::shared_ptr<CSpace> > components;
  std::vector<std::string> componentNames;
  std::vector<Real> distanceWeights;
};

class PiggybackCSpace : public CSpace
{
public:
  PiggybackCSpace(CSpace* _baseSpace=NULL);
  virtual int NumDimensions();
  virtual std::string VariableName(int i);
  virtual int NumConstraints();
  virtual std::string ConstraintName(int i);
  virtual std::shared_ptr<CSet> Constraint(int i);
  virtual void Sample(Config& x);
  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
  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 obstacle);
  virtual bool IsFeasible(const Config& x);
  virtual bool IsFeasible(const Config&,int constraint);
  virtual bool ProjectFeasible(Config& x);
  virtual Optimization::NonlinearProgram* FeasibleNumeric();
  virtual Real Distance(const Config& x, const Config& 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 void Properties(PropertyMap& map);

  CSpace* baseSpace;
};

class SubsetConstraintCSpace : public PiggybackCSpace
{
public:
  SubsetConstraintCSpace(CSpace* baseSpace,const std::vector<int>& constraints);
  SubsetConstraintCSpace(CSpace* baseSpace,int constraints);
  virtual bool IsFeasible(const Config& x) { return CSpace::IsFeasible(x); }
  virtual bool IsFeasible(const Config& x,int obstacle) { return CSpace::IsFeasible(x,obstacle); }
  virtual bool ProjectFeasible(Config& x) { return CSpace::ProjectFeasible(x); }
  virtual Optimization::NonlinearProgram* FeasibleNumeric() { return CSpace::FeasibleNumeric(); }
  virtual EdgePlannerPtr LocalPlanner(const Config& a,const Config& b) { return CSpace::LocalPlanner(a,b); }
  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle);

  std::vector<int> activeConstraints;
};


class AdaptiveCSpace : public PiggybackCSpace
{
public:
  AdaptiveCSpace(CSpace* baseSpace);
  virtual bool IsFeasible(const Config& x);
  virtual bool IsFeasible(const Config& x,int obstacle);
  virtual void CheckConstraints(const Config& x,std::vector<bool>& satisfied);
  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle);
  bool IsFeasible_NoDeps(const Config& x,int obstacle);
  EdgePlannerPtr PathChecker_NoDeps(const Config& a,const Config& b,int obstacle);
  void SetupAdaptiveInfo();
  bool AddFeasibleDependency(int constraint,int dependency);
  bool AddVisibleDependency(int constraint,int dependency);
  bool AddFeasibleDependency(const char* name,const char* dependency);
  bool AddVisibleDependency(const char* name,const char* dependency);
  void OptimizeQueryOrder();
  void GetFeasibleDependencies(int obstacle,std::vector<int>& deps,bool recursive=true) const;
  void GetVisibleDependencies(int obstacle,std::vector<int>& deps,bool recursive=true) const;
  void GetStats(PropertyMap& stats) const;
  void LoadStats(const PropertyMap& stats);

  struct PredicateStats
  {
    double cost;
    double probability;
    double count;
  };
  bool adaptive;
  std::map<std::string,int> constraintMap;
  std::vector<PredicateStats> feasibleStats,visibleStats;
  std::vector<std::vector<int> > feasibleTestDeps,visibleTestDeps;
  std::vector<int> feasibleTestOrder,visibleTestOrder;
  bool useBaseVisibleTest;
  PredicateStats baseVisibleStats;
};


EdgePlannerPtr MakeSingleConstraintEpsilonChecker(CSpace* space,const Config& a,const Config& b,int obstacle,Real epsilon);

EdgePlannerPtr MakeSingleConstraintBisectionPlanner(CSpace* space,const Config& a,const Config& b,int obstacle,Real epsilon);


#endif