MultiPath.h

#ifndef MULTI_PATH_H
#define MULTI_PATH_H

#include "DynamicPath.h"
#include <Klampt/Contact/Hold.h>
#include <Klampt/Contact/Stance.h>
#include <KrisLibrary/planning/GeneralizedBezierCurve.h>
#include <KrisLibrary/utils/PropertyMap.h>
#include <KrisLibrary/math/vector.h>

class TiXmlElement;

namespace Klampt {
  using namespace std;

class MultiPath
{
 public:
  bool Load(const string& fn);
  bool Save(const string& fn) const;
  bool Load(TiXmlElement* in);
  bool Save(TiXmlElement* out) const;
  size_t NumSections() const { return sections.size(); }
  bool IsValid() const;
  bool IsContinuous(Real tol=0.0) const;
  bool HasTiming(int section=0) const;
  bool HasVelocity(int section=0) const;
  bool HasConstraints(int section=0) const;
  bool HasContacts(int section=0) const;
  void GetMilestones(vector<Vector>& milestones,int section=0) const;
  void GetTimedMilestones(vector<Real>& times,vector<Vector>& milestones,int section=0) const;
  bool GetDynamicPath(ParabolicRamp::DynamicPath& path,int section=0) const;
  void SetMilestones(const vector<Vector>& milestones,int section=0);
  void SetTimedMilestones(const vector<Real>& times,const vector<Vector>& milestones,int section=0);
  void SetDynamicPath(const ParabolicRamp::DynamicPath& path,int section=0);
  Real StartTime() const;
  Real EndTime() const;
  Real Duration() const;
  void SetDuration(Real duration,bool uniformSectionTimes=true);
  void SetSmoothTiming(Real duration,bool uniformSectionTimes=true);
  void Concat(const MultiPath& suffix,bool relative=true);
  void GetIKProblem(vector<IKGoal>& goals,int section=0) const;
  void SetIKProblem(const vector<IKGoal>& goals,int section=0);
  void GetStance(Stance& stance,int section=0) const;
  void SetStance(const Stance& stance,int section=0);
  void SetHold(const string& str,const Hold& h);
  bool GetHold(const string& str,Hold& h) const;
  int TimeToSection(Real time) const;

  enum InterpPolicy { InterpLinear, InterpCubic };
  int Evaluate(Real time,GeneralizedCubicBezierCurve& curve,Real& duration,Real& u,InterpPolicy policy=InterpCubic) const;
  int Evaluate(Real time,Vector& q,InterpPolicy policy=InterpCubic) const;
  int Evaluate(Real time,Vector& q,Vector& v,InterpPolicy policy=InterpCubic) const;

  struct PathSection 
  {
    PropertyMap settings;

    //section-specific constraints
    vector<IKGoal> ikGoals;
    vector<Hold> holds;
    vector<int> holdIndices;  //indexes into the holdSet array
    vector<string> holdNames;  //indexes into the holdSet array

    vector<Real> times;
    vector<Vector> milestones;
    vector<Vector> velocities;
  };

  //typically store things like name, robot, scenario, etc
  PropertyMap settings;
  vector<PathSection> sections;
  vector<Hold> holdSet;
  vector<string> holdSetNames;
};

ostream& operator << (ostream& out,const MultiPath& path);

} //namespace Klampt

#endif