ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lpiecewisefunction.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/mad/alfunction.h>
17 #include <vector>
18 #include <algorithm>
19 
20 namespace adore
21 {
22  namespace mad
23  {
28  template<typename DT, typename CT>
29  class LPiecewiseFunction : public ALFunction<DT, CT>
30  {
31  private:
32  std::vector<ALFunction<DT, CT>*> m_data;
33  mutable unsigned int m_searchIndex;
35  public:
37  LPiecewiseFunction(bool deconstruct_pieces) :m_searchIndex(0), m_deconstruct_pieces(deconstruct_pieces) {}
38  void setDeconstructPieces(bool value)
39  {
40  m_deconstruct_pieces = value;
41  }
43  {
45  {
46  for (unsigned int i = 0; i < m_data.size(); i++)delete m_data.at(i);
47  }
48  }
49  //binary search to find index of x, throws an exception on failure
50  unsigned int findIndex(DT x) const
51  {
52  if (x > limitHi() || x < limitLo())
53  {
55  }
56  unsigned int lower = 0;
57  unsigned int upper = m_data.size() - 1;
60  while (current->limitHi() < x || current->limitLo() > x)
61  {
62  if (lower == upper)throw FunctionIndexingError();
63  if (current->limitHi() < x)//search higher
64  {
65  lower = m_searchIndex;
66  unsigned int new_index = (std::ceil)((float)(m_searchIndex + upper) / 2.0f);
67  if (new_index == m_searchIndex)throw FunctionIndexingError();
68  m_searchIndex = new_index;
69  current = m_data.at(m_searchIndex);
70  }
71  else//search lower
72  {
73  upper = m_searchIndex;
74  unsigned int new_index = (std::floor)((float)(m_searchIndex + lower) / 2.0f);
75  if (new_index == m_searchIndex)throw FunctionIndexingError();
76  m_searchIndex = new_index;
77  current = m_data.at(m_searchIndex);
78  }
79  }
80  return m_searchIndex;
81  }
83  {
84  if (m_data.size() > 0)
85  {
86  if(newfun->limitLo()<limitHi())
87  {
88  std::cout<<"domain error: old function's high limit is "<< limitHi()<<", new functions low limit is "<<newfun->limitLo()<<"\n";
89  }
90  assert(newfun->limitLo() >= limitHi());
91  assert(newfun->limitHi() > newfun->limitLo());
92  }
93  m_data.push_back(newfun);
94  }
96  {
97  if (m_data.size() > 0)
98  {
99  DT delta = limitHi() - newfun->limitLo();
100  appendHi(funop::stretch(newfun, newfun->limitLo() + delta, newfun->limitHi() + delta));
101  }
102  else
103  {
104  appendHi(newfun);
105  }
106  }
107  public://methods inherited from ALFunction interface
108  //function evaluation returns y of codomain type CT for a value x of domain type DT
109  virtual CT f(DT x) const override
110  {
111  if (m_data.size() == 0)throw FunctionNotInitialized();
112  return m_data.at(findIndex(x))->f(x);
113  }
114  virtual DT limitHi() const override
115  {
116  if (m_data.size() == 0)throw FunctionNotInitialized();
117  return m_data.at(m_data.size() - 1)->limitHi();
118  }
119  virtual DT limitLo() const override
120  {
121  if (m_data.size() == 0)throw FunctionNotInitialized();
122  return m_data.at(0)->limitLo();
123  }
124  //reduce or increase the limit of the function
125  virtual void setLimits(DT lo, DT hi) override
126  {
127  if (m_data.size() == 0)throw FunctionNotInitialized();
128 
129  // increase limits
130  if(limitLo() > lo)
131  {
132  m_data.at(0)->setLimits(lo, m_data.at(0)->limitHi());
133  }
134  if(limitHi() < hi)
135  {
136  m_data.at(m_data.size() - 1)->setLimits(m_data.at(m_data.size() - 1)->limitLo(), hi);
137  }
138 
139  std::vector<int> indicesToRemove;
140  // reduce: throw away functions that are out of bounds
141  // iterate over elements
142  for(unsigned int i=0; i < m_data.size(); i++)
143  {
144  // if new lo is greater than limitHi(), remove partial function
145  if(m_data.at(i)->limitHi() <= lo)
146  {
147  indicesToRemove.push_back(i);
148  }
149  else
150  {
151  // if new hi is lower than limitLo(), remove partial function
152  if(m_data.at(i)->limitLo() >= hi)
153  {
154  indicesToRemove.push_back(i);
155  }
156  // else: there is overlap in functions
157  else
158  {
159  // if new lo is greater than limitLo(): set new limitLo
160  if(m_data.at(i)->limitLo() < lo)
161  {
162  m_data.at(i)->setLimits(lo, m_data.at(i)->limitHi());
163  }
164  // if new hi is lower than limitHi(): set new limitHi
165  if(m_data.at(i)->limitHi() > hi)
166  {
167  m_data.at(i)->setLimits(m_data.at(i)->limitLo(), hi);
168  }
169  }
170  }
171  }
172  // erase backwards because otherwise indices change through vector
173  for(int i = indicesToRemove.size()-1; i >= 0; i--)
174  {
175  m_data.erase(m_data.begin()+indicesToRemove.at(i));
176  }
177  //m_data.at(0)->setLimits(lo, m_data.at(0)->limitHi());
178  //m_data.at(m_data.size() - 1)->setLimits(m_data.at(m_data.size() - 1)->limitLo(), hi);
179  }
181  {
183  for (auto it = m_data.begin(); it != m_data.end(); it++)
184  {
185  derivative->appendHi((*it)->create_derivative());
186  }
187  return derivative;
188  }
189  virtual ALFunction<DT, CT>* clone()override
190  {
192  for (auto it = m_data.begin(); it != m_data.end(); it++)
193  {
194  result->appendHi((*it)->clone());
195  }
196  return result;
197  }
198  virtual void bound(const DT& xmin, const DT& xmax, CT& ymin, CT& ymax)override
199  {
200  int i0 = findIndex(xmin);
201  int i1 = findIndex(xmax);
202  if (i0 == i1)
203  {
204  m_data.at(i0)->bound(xmin, xmax, ymin, ymax);
205  }
206  else
207  {
208  CT ymin_tmp, ymax_tmp;
209  m_data.at(i0)->bound(xmin, m_data.at(i0)->limitHi(), ymin, ymax);
210  m_data.at(i1)->bound(m_data.at(i1)->limitLo(), xmax, ymin_tmp, ymax_tmp);
211  ymin = adore::mad::min(ymin, ymin_tmp);
212  ymax = adore::mad::max(ymax, ymax_tmp);
213  for (int i = i0 + 1; i < i1; i++)
214  {
215  m_data.at(i)->bound(ymin_tmp, ymax_tmp);
216  ymin = adore::mad::min(ymin, ymin_tmp);
217  ymax = adore::mad::max(ymax, ymax_tmp);
218  }
219  }
220  }
221  };
222  template<typename T>
223  LPiecewiseFunction<T, T>* createPiecewiseLinear(const adoreMatrix<T, 2, 0>& xymatrix)
224  {
226  for (int i = 0; i < xymatrix.nc() - 1; i++)
227  {
228  T x0 = xymatrix(0, i);
229  T x1 = xymatrix(0, i + 1);
230  T y0 = xymatrix(1, i);
231  T y1 = xymatrix(1, i + 1);
232  that->appendHi(new LLinearFunction<T, T>(x0, x1, y0, (y1 - y0) / (x1 - x0)));
233  }
234  return that;
235  }
236  template<typename T>
237  LPiecewiseFunction<T, T>* createPiecewiseLinear(const adoreMatrix<T, 1, 0>& xvec, const adoreMatrix<T>& yvec, int row)
238  {
240  for (int i = 0; i < xvec.nc() - 1; i++)
241  {
242  T x0 = xvec(0, i);
243  T x1 = xvec(0, i + 1);
244  T y0 = yvec(row, i);
245  T y1 = yvec(row, i + 1);
246  that->appendHi(new LLinearFunction<T, T>(x0, x1, y0, (y1 - y0) / (x1 - x0)));
247  }
248  return that;
249  }
250  template<typename T, int n>
251  LPiecewiseFunction<T, adoreMatrix<T, n, 1>>* createPiecewiseLinear(const adoreMatrix<T, 1, 0>& xvec, const adoreMatrix<T, n, 0>& ymatrix)
252  {
254  for (int i = 0; i < xvec.nc() - 1; i++)
255  {
256  T x0 = xvec(i);
257  T x1 = xvec(i + 1);
258  adoreMatrix<T, n, 1> y0 = dlib::colm(ymatrix, i);
259  adoreMatrix<T, n, 1> y1 = dlib::colm(ymatrix, i+1);
260  that->appendHi(new LLinearFunction<T, adoreMatrix<T, n, 1>>(x0, x1, y0, (y1 - y0) / (x1 - x0)));
261  }
262  return that;
263  }
264  template<typename T, int n>
265  LPiecewiseFunction<T, adoreMatrix<T, n, 1>>* createPiecewiseLinear(const adoreMatrix<T, n + 1, 0>& xymatrix)
266  {
268  for (int i = 0; i < xymatrix.nc() - 1; i++)
269  {
270  T x0 = xymatrix(0, i);
271  T x1 = xymatrix(0, i + 1);
272  adoreMatrix<T, n, 1> y0 = dlib::subm(xymatrix,dlib::range(1,xymatrix.nr()),dlib::range(i, i));
273  adoreMatrix<T, n, 1> y1 = dlib::subm(xymatrix,dlib::range(1,xymatrix.nr()),dlib::range(i+1, i+1));
274  that->appendHi(new LLinearFunction<T, adoreMatrix<T, n, 1>>(x0, x1, y0, (y1 - y0) / (x1 - x0)));
275  }
276  return that;
277  }
278  template<typename T, int n>
279  void sample(LPiecewiseFunction<T, adoreMatrix<T, n, 1>>* fun, T* xvec, T* ydest, int row, int count)
280  {
281  for (int i = 0; i < count; i++)
282  {
283  ydest[i] = fun->f(xvec[i])(row);
284  }
285  }
286  }
287 }
Definition: alfunction.h:74
virtual DT limitHi() const =0
virtual DT limitLo() const =0
Definition: alfunction.h:54
Definition: alfunction.h:46
Definition: alfunction.h:30
Definition: alfunction.h:307
Definition: lpiecewisefunction.h:30
virtual void setLimits(DT lo, DT hi) override
Definition: lpiecewisefunction.h:125
virtual DT limitHi() const override
Definition: lpiecewisefunction.h:114
virtual void bound(const DT &xmin, const DT &xmax, CT &ymin, CT &ymax) override
Definition: lpiecewisefunction.h:198
virtual DT limitLo() const override
Definition: lpiecewisefunction.h:119
LPiecewiseFunction()
Definition: lpiecewisefunction.h:36
unsigned int m_searchIndex
Definition: lpiecewisefunction.h:33
virtual ALFunction< DT, CT > * clone() override
Definition: lpiecewisefunction.h:189
virtual ALFunction< DT, CT > * create_derivative() override
Definition: lpiecewisefunction.h:180
std::vector< ALFunction< DT, CT > * > m_data
Definition: lpiecewisefunction.h:32
unsigned int findIndex(DT x) const
Definition: lpiecewisefunction.h:50
LPiecewiseFunction(bool deconstruct_pieces)
Definition: lpiecewisefunction.h:37
void appendHi(ALFunction< DT, CT > *newfun)
Definition: lpiecewisefunction.h:82
virtual CT f(DT x) const override
Definition: lpiecewisefunction.h:109
void setDeconstructPieces(bool value)
Definition: lpiecewisefunction.h:38
bool m_deconstruct_pieces
Definition: lpiecewisefunction.h:34
void appendHi_shifted(ALFunction< DT, CT > *newfun)
Definition: lpiecewisefunction.h:95
virtual ~LPiecewiseFunction()
Definition: lpiecewisefunction.h:42
ALFunction< DT, CT > * stretch(ALFunction< DT, CT > *f, DT from, DT to)
Definition: alfunction.h:802
void sample(ALFunction< T, adoreMatrix< T, N, 1 >> *f, T *x, T *y, int dimension, unsigned int count)
Definition: alfunction.h:160
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
LPiecewiseFunction< T, T > * createPiecewiseLinear(const adoreMatrix< T, 2, 0 > &xymatrix)
Definition: lpiecewisefunction.h:223
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
Definition: areaofeffectconverter.h:20