KrisLibrary  1.0.0
PiecewisePolynomial.h
1 #ifndef PIECEWISE_POLYNOMIAL_H
2 #define PIECEWISE_POLYNOMIAL_H
3 
4 #include "Polynomial.h"
5 class File;
6 
7 namespace Spline {
8 
23 {
24  public:
25  typedef Polynomial<double> Poly;
26 
28  PiecewisePolynomial(const Poly& p,double a,double b);
29  PiecewisePolynomial(const std::vector<Poly>& _segments,const std::vector<double>& _times,bool relative=false);
30  PiecewisePolynomial(const std::vector<Poly>& _segments,const std::vector<double>& _times,const std::vector<double>& _timeShifts);
31 
32  int FindSegment(double t) const;
33  double Evaluate(double t) const;
34  double Derivative(double t) const;
35  double Derivative(double t,int n) const;
36  double operator () (double t) const { return Evaluate(t); }
37 
38  PiecewisePolynomial Differentiate(int n=1) const;
39  inline double Start() const { return segments.front()(times.front()-timeShift.front()); }
40  inline double End() const { return segments.back()(times.back()-timeShift.back()); }
41  inline double StartTime() const { return times.front(); }
42  inline double EndTime() const { return times.back(); }
43 
54  void Append(const Poly& p,double t,bool relative=false);
55 
61  void Concat(const PiecewisePolynomial& traj,bool relative=false);
62 
64  void TimeShift(double dt);
65 
70  void ZeroTimeShift();
71 
73  void Split(double t,PiecewisePolynomial& front,PiecewisePolynomial& back) const;
74 
76  void TrimFront(double tstart);
77 
79  void TrimBack(double tend);
80 
82  PiecewisePolynomial Select(double a,double b) const;
83 
87  std::pair<double,double> MaxDiscontinuity(int derivative=0) const;
88 
89  bool Read(File& f);
90  bool Write(File& f) const;
91 
92  void operator += (double val);
93  void operator -= (double val);
94  void operator *= (double val);
95  void operator /= (double val);
96  void operator += (const Polynomial<double>& b);
97  void operator -= (const Polynomial<double>& b);
98  void operator *= (const Polynomial<double>& b);
99 
100  std::vector<Poly> segments;
101  std::vector<double> timeShift;
102  std::vector<double> times;
103 };
104 
106 {
107  public:
108  typedef Polynomial<double> Poly;
109  typedef std::vector<double> Vector;
110 
112  PiecewisePolynomialND(const std::vector<Poly>& elements,double a,double b);
113  PiecewisePolynomialND(const std::vector<PiecewisePolynomial>& elements);
114 
115  Vector Evaluate(double t) const;
116  Vector Derivative(double t) const;
117  Vector Derivative(double t,int n) const;
118  Vector operator () (double t) const { return Evaluate(t); }
119 
120  PiecewisePolynomialND Differentiate(int n=1) const;
121  Vector Start() const;
122  Vector End() const;
123  double StartTime() const;
124  double EndTime() const;
125 
126  void Concat(const PiecewisePolynomialND& traj,bool relative=false);
127  void Append(const std::vector<Polynomial<double> >& polys,double t,bool relative=false);
128  void TimeShift(double dt);
129  void Split(double t,PiecewisePolynomialND& front,PiecewisePolynomialND& back) const;
130  void TrimFront(double tstart);
131  void TrimBack(double tend);
132  PiecewisePolynomialND Select(double a,double b) const;
133  std::pair<Vector,Vector> MaxDiscontinuity(int derivative=0) const;
134 
135  bool Read(File& f);
136  bool Write(File& f) const;
137 
138  std::vector<PiecewisePolynomial> elements;
139 };
140 
141 
142 PiecewisePolynomial Constant(double x,double ta,double tb);
143 PiecewisePolynomial Linear(double a,double b,double ta,double tb);
144 PiecewisePolynomial PiecewiseLinear(const std::vector<double>& milestones,const std::vector<double>& times);
145 
146 PiecewisePolynomialND Constant(const std::vector<double>& q,double ta,double tb);
147 PiecewisePolynomialND Linear(const std::vector<double>& a,const std::vector<double>& b,double ta,double tb);
148 PiecewisePolynomialND PiecewiseLinear(const std::vector<std::vector<double> >& milestones,const std::vector<double>& times);
149 PiecewisePolynomialND Subspace(const std::vector<double>& x0,const std::vector<double>& dx,PiecewisePolynomial& poly);
150 
151 inline PiecewisePolynomial operator + (const PiecewisePolynomial& a,double b) {
152  PiecewisePolynomial res=a;
153  res += b;
154  return res;
155 }
156 inline PiecewisePolynomial operator - (const PiecewisePolynomial& a,double b) {
157  PiecewisePolynomial res=a;
158  res -= b;
159  return res;
160 }
161 inline PiecewisePolynomial operator * (const PiecewisePolynomial& a,double b) {
162  PiecewisePolynomial res=a;
163  res *= b;
164  return res;
165 }
166 inline PiecewisePolynomial operator / (const PiecewisePolynomial& a,double b) {
167  PiecewisePolynomial res=a;
168  res /= b;
169  return res;
170 }
171 inline PiecewisePolynomial operator + (double a,const PiecewisePolynomial& b) {
172  PiecewisePolynomial res=b;
173  res += a;
174  return res;
175 }
176 inline PiecewisePolynomial operator * (double a,const PiecewisePolynomial& b) {
177  PiecewisePolynomial res=b;
178  res *= a;
179  return res;
180 }
181 inline PiecewisePolynomial operator + (const PiecewisePolynomial& a,const Polynomial<double>& b) {
182  PiecewisePolynomial res=a;
183  res += b;
184  return res;
185 }
186 inline PiecewisePolynomial operator - (const PiecewisePolynomial& a,const Polynomial<double>& b) {
187  PiecewisePolynomial res=a;
188  res -= b;
189  return res;
190 }
191 inline PiecewisePolynomial operator * (const PiecewisePolynomial& a,const Polynomial<double>& b) {
192  PiecewisePolynomial res=a;
193  res *= b;
194  return res;
195 }
196 inline PiecewisePolynomial operator + (const Polynomial<double>& a,const PiecewisePolynomial& b) {
197  PiecewisePolynomial res=b;
198  res += a;
199  return res;
200 }
201 inline PiecewisePolynomial operator * (const Polynomial<double>& a,const PiecewisePolynomial& b) {
202  PiecewisePolynomial res=b;
203  res *= a;
204  return res;
205 }
206 
207 
208 } //namespace Spline
209 
210 #endif
void ZeroTimeShift()
Definition: PiecewisePolynomial.cpp:176
void TrimBack(double tend)
Definition: PiecewisePolynomial.cpp:236
A simple polynomial class, p(x) = sum_{i=0}^n coef[i]*x^i.
Definition: Polynomial.h:14
void Concat(const PiecewisePolynomial &traj, bool relative=false)
Definition: PiecewisePolynomial.cpp:137
Definition: PiecewisePolynomial.h:22
void Split(double t, PiecewisePolynomial &front, PiecewisePolynomial &back) const
Definition: PiecewisePolynomial.cpp:189
void Append(const Poly &p, double t, bool relative=false)
Definition: PiecewisePolynomial.cpp:120
PiecewisePolynomial Select(double a, double b) const
Definition: PiecewisePolynomial.cpp:254
void TrimFront(double tstart)
Definition: PiecewisePolynomial.cpp:215
Definition: BSpline.cpp:9
Definition: PiecewisePolynomial.h:105
A cross-platform class for reading/writing binary data.
Definition: File.h:47
void TimeShift(double dt)
Definition: PiecewisePolynomial.cpp:168
std::pair< double, double > MaxDiscontinuity(int derivative=0) const
Definition: PiecewisePolynomial.cpp:262