KrisLibrary  1.0.0
GeodesicSpace.h
1 #ifndef PLANNING_GEODESIC_SPACE_H
2 #define PLANNING_GEODESIC_SPACE_H
3 
4 #include <KrisLibrary/math/vector.h>
7 using namespace Math;
8 typedef Vector Config;
9 
32 {
33  public:
34  GeodesicSpace() {}
35  virtual ~GeodesicSpace() {}
36  virtual int NumDimensions() { FatalError("NumDimensions method not implemented"); return -1; }
37  virtual int NumIntrinsicDimensions() { return NumDimensions(); }
38  virtual Real Distance(const Config& x, const Config& y) { return Distance_L2(x,y); }
39  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) { interpolate(x,y,u,out); }
40 
41  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx) { dx.sub(b,a); }
42  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx) { dx.mul(da,1-u); }
43  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx) { dx.mul(db,u); }
44  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx) { ddx.resize(a.n,Zero); }
45  virtual void Integrate(const Config& a,const Vector& da,Config& b) { b.add(a,da); }
46 };
47 
49 {
50 public:
51  CartesianSpace(int d);
52  virtual ~CartesianSpace() {}
53  virtual int NumDimensions() { return d; }
54 
55  int d;
56 };
57 
58 /*
59 class MultiGeodesicSpace : public GeodesicSpace
60 {
61 public:
62  MultiGeodesicSpace();
63  MultiGeodesicSpace(const std::vector<std::shared_ptr<GeodesicSpace> >& components);
64  void Add(const std::shared_ptr<GeodesicSpace>& space,Real weight=1.0);
65 
66  void Split(const Vector& x,std::vector<Vector>& items) const;
67  void Join(const std::vector<Vector>& items,Vector& x) const;
68 
69  virtual int NumDimensions();
70  virtual int NumIntrinsicDimensions();
71  virtual Real Distance(const Config& x, const Config& y);
72  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
73 
74  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx);
75  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx);
76  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx);
77  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx);
78  virtual void Integrate(const Config& a,const Vector& da,Config& b);
79 
80  std::vector<std::shared_ptr<GeodesicSpace> > components;
81  std::vector<Real> weights;
82 };
83 */
84 
85 #endif
A space with geodesics and (optionally) geodesic derivatives.
Definition: GeodesicSpace.h:31
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
Definition: GeodesicSpace.h:48
Standard vector/matrix metrics.
Contains all definitions in the Math package.
Definition: WorkspaceBound.h:12
Convenience functions for linear interpolation of vectors and matrices.