ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
llinearpiecewisefunction.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 <adore/mad/adoremath.h>
18 #include <adore/mad/csvlog.h>
19 #include <vector>
20 #include <algorithm>
21 #include <iostream>
22 
23 namespace adore
24 {
25  namespace mad
26  {
30  template<typename T>
32  {
33  private:
34  adoreMatrix<T, 2, 0> m_data;
35  typedef T CT;
36  typedef T DT;
37  mutable unsigned int m_searchIndex;
38  public:
39  LLinearPiecewiseFunctionS(const adoreMatrix<T, 2, 0> &data)
40  {
41  m_data = data;
42  m_searchIndex = 0;
43  }
44  LLinearPiecewiseFunctionS(const adoreMatrix<T, 1, 0> &x, const adoreMatrix<T, 1, 0> &y)
45  {
46  assert(x.nc() == y.nc());
47  m_data = dlib::zeros_matrix<T>(2, x.nc());
48  set_rowm(m_data, 0) = x;
49  set_rowm(m_data, 1) = y;
50  m_searchIndex = 0;
51  }
52  //binary search to find index of x, throws an exception on failure
53  unsigned int findIndex(DT x) const
54  {
55  if (x > limitHi() || x < limitLo())
56  {
58  }
59  unsigned int lower = 0;
60  unsigned int upper = m_data.nc() - 2;
62  while (m_data(0, m_searchIndex + 1) < x || m_data(0, m_searchIndex) > x)
63  {
64  if (lower == upper)throw FunctionIndexingError();
65  if (m_data(0, m_searchIndex + 1) < x)//upperLimit<x --> search higher
66  {
67  lower = m_searchIndex;
68  unsigned int new_index = (std::ceil)((float)(m_searchIndex + upper) / 2.0f);
69  if (new_index == m_searchIndex)throw FunctionIndexingError();
70  m_searchIndex = new_index;
71  }
72  else//lowerLimit>x --> search lower
73  {
74  upper = m_searchIndex;
75  unsigned int new_index = (std::floor)((float)(m_searchIndex + lower) / 2.0f);
76  if (new_index == m_searchIndex)throw FunctionIndexingError();
77  m_searchIndex = new_index;
78  }
79  }
80  return m_searchIndex;
81  }
82 
83  virtual CT f(DT x) const override
84  {
85  if (m_data.nc() == 0)throw FunctionNotInitialized();
86  int i = findIndex(x);
87  CT y0 = m_data(1, i);
88  CT y1 = m_data(1, i + 1);
89  return y0 + (y1 - y0)*((x - m_data(0, i)) / (m_data(0, i + 1) - m_data(0, i)));
90  }
91  virtual DT limitHi() const override
92  {
93  return m_data(0, m_data.nc() - 1);
94  }
95  virtual DT limitLo() const override
96  {
97  return m_data(0, 0);
98  }
99  virtual void setLimits(DT lo, DT hi)
100  {
101  throw FunctionNotImplemented();
102  }
104  {
106  }
108  {
109  throw FunctionNotImplemented();
110  }
111  virtual void bound(const DT& xmin, const DT& xmax, CT& ymin, CT& ymax) override
112  {
113  CT y;
114 
115  int imax = findIndex(xmax);
116  y = f(xmax);
117  ymin = y;
118  ymax = y;
119 
120  int imin = findIndex(xmin);
121  y = f(xmin);
122  ymin = adore::mad::min(ymin, y);
123  ymax = adore::mad::max(ymax, y);
124 
125  for (int i = imin + 1; i <= imax; i++)
126  {
127  y = m_data(i);
128  ymin = adore::mad::min(ymin, y);
129  ymax = adore::mad::max(ymax, y);
130  }
131  }
132  };
133 
137  template<typename T, int n>
139  {
140  private:
141  adoreMatrix<T, n + 1, 0> m_data;
142  mutable unsigned int m_searchIndex;
143  public:
144  typedef adoreMatrix<T, n, 1> CT;
145  typedef typename AScalarToN<T,n>::DT DT;
146 
147  adoreMatrix<T, n + 1, 0>& getData()
148  {
149  return m_data;
150  }
151 
152  const adoreMatrix<T, n + 1, 0>& getData()const
153  {
154  return m_data;
155  }
156 
157  //binary search to find index of x, throws an exception on failure
158  unsigned int findIndex(DT x, DT precision = 0.001) const
159  {
160  if (x > limitHi() || x < limitLo())
161  {
162  if(x > limitHi())
163  {
164  if(std::abs(x-limitHi())>precision)
165  {
166  std::cerr<<"LLinearPiecewiseFunctionM out of bounds: findIndex("<<x<<") called, while limitLo()="<<limitLo()<<"and limtiHi()="<<limitHi()<<std::endl;
168  }
169  else
170  {
171  x = limitHi();
172  }
173  }
174  else
175  {
176  if(std::abs(x-limitLo())>precision)
177  {
178  std::cerr<<"LLinearPiecewiseFunctionM out of bounds: findIndex("<<x<<") called, while limitLo()="<<limitLo()<<"and limtiHi()="<<limitHi()<<std::endl;
180  }
181  else
182  {
183  x = limitLo();
184  }
185  }
186  }
187  unsigned int lower = 0;
188  unsigned int upper = m_data.nc() - 2;
190 
191  //look up likely places: last index, next index, previous index, first element, last element
192  if (m_data(0, m_searchIndex) <= x)
193  {
194  if (x < m_data(0, m_searchIndex + 1))
195  {
196  return m_searchIndex;//the same as last time
197  }
198  else
199  {
200  if (m_searchIndex + 2 < (unsigned int)m_data.nc() && x < m_data(0, m_searchIndex + 2))
201  {
202  return ++m_searchIndex;//one to the right
203  }
204  else
205  {
206  if (m_data(0, m_data.nc() - 2) <= x)
207  {
208  return (m_searchIndex = m_data.nc() - 2); //the global last
209  }
210  }
211  }
212  }
213  else
214  {
215  if (m_searchIndex - 1 >= 0 && m_data(0, m_searchIndex - 1) <= x)
216  {
217  return --m_searchIndex;//one to the left
218  }
219  else
220  {
221  if (x < m_data(0, 1))
222  {
223  return (m_searchIndex = 0);//the global first
224  }
225  }
226  }
227 
228  //binary search
229  while (m_data(0, m_searchIndex + 1) < x || m_data(0, m_searchIndex) > x)
230  {
231  if (lower == upper)throw FunctionIndexingError();
232  if (m_data(0, m_searchIndex + 1) < x)//upperLimit<x --> search higher
233  {
234  lower = m_searchIndex;
235  unsigned int new_index = (std::ceil)((float)(m_searchIndex + upper) / 2.0f);
236  if (new_index == m_searchIndex)throw FunctionIndexingError();
237  m_searchIndex = new_index;
238  }
239  else//lowerLimit>x --> search lower
240  {
241  upper = m_searchIndex;
242  unsigned int new_index = (std::floor)((float)(m_searchIndex + lower) / 2.0f);
243  if (new_index == m_searchIndex)throw FunctionIndexingError();
244  m_searchIndex = new_index;
245  }
246  }
247  return m_searchIndex;
248  }
249  public://methods inherited from ALFunction interface
250  //function evaluation returns y of codomain type CT for a value x of domain type DT
251  virtual CT f(DT x) const override
252  {
253  if (m_data.nc() == 0)throw FunctionNotInitialized();
254  int i = findIndex(x);
255  CT y0 = dlib::subm(m_data, dlib::range(1, n), dlib::range(i, i));
256  CT y1 = dlib::subm(m_data, dlib::range(1, n), dlib::range(i + 1, i + 1));
257  return y0 + (y1 - y0)*((x - m_data(0, i)) / (m_data(0, i + 1) - m_data(0, i)));
258  }
259  virtual DT limitHi() const override
260  {
261  if (m_data.nc() == 0)throw FunctionNotInitialized();
262  return m_data(0, m_data.nc() - 1);
263  }
264  virtual DT limitLo() const override
265  {
266  if (m_data.nc() == 0)throw FunctionNotInitialized();
267  return m_data(0, 0);
268  }
269  //reduce or increase the limit of the function
270  virtual void setLimits(DT lo, DT hi) override
271  {
272  throw FunctionNotImplemented();
273  }
275  {
276  throw FunctionNotImplemented();
277  }
278  virtual void bound(const DT& xmin, const DT& xmax, CT& ymin, CT& ymax)override
279  {
280  CT y;
281 
282  int imax = findIndex(xmax);
283  y = f(xmax);
284  ymin = y;
285  ymax = y;
286 
287  int imin = findIndex(xmin);
288  y = f(xmin);
289  ymin = adore::mad::min<T,n,1>(ymin, y);
290  ymax = adore::mad::max<T,n,1>(ymax, y);
291 
292  for (int i = imin + 1; i <= imax; i++)
293  {
294  y = subm(m_data, dlib::range(1, n), dlib::range(i, i));
295  ymin = adore::mad::min<T,n,1>(ymin, y);
296  ymax = adore::mad::max<T,n,1>(ymax, y);
297  }
298  }
299  public: //other useful methods
300 
309  virtual double limit_s_to_bounds(double s) const
310  {
311  // s=adore::mad::bound(m_leftBorderDistance_fct.limitLo(),
312  // m_leftBorderDistance_fct.limitLo()+s,
313  // m_leftBorderDistance_fct.limitHi());
314  return adore::mad::bound(limitLo(),limitLo()+s,limitHi());
315  }
316 
320  virtual void invertDomain()
321  {
322  T old_lo = limitLo();
323  T old_hi = limitHi();
324  m_data = dlib::colm(m_data,m_data.nc()-dlib::range(1,m_data.nc()));
325  set_rowm(m_data,0) = old_hi - rowm(m_data,0) + old_lo;
326  }
330  virtual void startDomainAtZero()
331  {
332  set_rowm(m_data,0) = rowm(m_data,0)-limitLo();
333  }
337  void shiftDomain(DT dx)
338  {
339  set_rowm(m_data,0) = rowm(m_data,0)+dx;
340  }
344  virtual void stretchDomain(DT x0, DT x1)
345  {
346  shiftDomain(-limitLo());
347  DT ratio = (x1-x0)/(limitHi()-limitLo());
348  for(int i = 1; i < m_data.nc(); i++)
349  {
350  m_data(0,i) = m_data(0,i)*ratio;
351  }
352  shiftDomain(x0);
353  }
357  void shiftCodomain(CT dy)
358  {
359  auto rows = dlib::range(1,n);
360  for(int i=0;i<m_data.nc();i++)
361  {
362  dlib::set_subm(m_data,rows,dlib::range(i,i)) = dlib::subm(m_data,rows,dlib::range(i,i)) + dy;
363  }
364  }
368  void shiftCodomain(T dy,int i=0)
369  {
370  set_rowm(m_data,i+1) = rowm(m_data,i+1)+dy;
371  }
372 
376  void rotateXY(double angle, double x0=0.0, double y0=0.0)
377  {
378  for(int i=0 ; i<m_data.nc(); i++)
379  {
380  double x = m_data(1,i);
381  double y = m_data(2,i);
382  m_data(1,i) = (x-x0)*std::cos(angle) - (y-y0)*std::sin(angle) + x0;
383  m_data(2,i) = (x-x0)*std::sin(angle) + (y-y0)*std::cos(angle) + y0;
384  }
385  }
386 
387 
391  virtual T fi(DT x, int row) const override
392  {
393  if (m_data.nc() == 0)throw FunctionNotInitialized();
394  int i = findIndex(x);
395  return m_data(row + 1, i) + (m_data(row + 1, i + 1) - m_data(row + 1, i))*(T)(x - m_data(0, i)) / (T)(m_data(0, i + 1) - m_data(0, i));
396  }
400  virtual T dfidx(DT x, int row)
401  {
402  if (m_data.nc() == 0)throw FunctionNotInitialized();
403  int i = findIndex(x);
404  T dy = m_data(row + 1, i + 1) - m_data(row + 1, i);
405  T dx = m_data(0, i + 1) - m_data(0, i);
406  return dy / dx;
407  }
411  void sample_dfidx(DT* xvec, T* yvec, int count, int row)
412  {
413  if (m_data.nc() == 0)throw FunctionNotInitialized();
414  for (int i = 0; i < count; i++)
415  {
416  yvec[i] = dfidx(xvec[i], row);
417  }
418  }
419  private:
423  class OneDimension :public ALFunction<DT, T>
424  {
425  private:
427  int m_row;
428  public:
430  OneDimension(LLinearPiecewiseFunctionM* parent, int row) :m_parent(parent), m_row(row) {}
431  virtual T f(DT x) const override { return m_parent->fi(x, m_row); }
432  virtual DT limitHi() const override { return m_parent->limitHi(); }
433  virtual DT limitLo() const override { return m_parent->limitLo(); }
434  virtual void setLimits(DT lo, DT hi) { throw FunctionNotImplemented(); }
436  {
437  return new LLinearPiecewiseFunctionS<T>(rowm(m_parent->m_data, 0), rowm(m_parent->m_data, m_row + 1));
438  }
440  {
441  throw FunctionNotImplemented();
442  }
443  virtual void bound(const DT& xmin, const DT& xmax, T& ymin, T& ymax) override
444  {
445  T y;
446 
447  int imax = m_parent->findIndex(xmax);
448  y = m_parent->fi(xmax, m_row);
449  ymin = y;
450  ymax = y;
451 
452  int imin = m_parent->findIndex(xmin);
453  y = m_parent->fi(xmin, m_row);
454  ymin = adore::mad::min(ymin, y);
455  ymax = adore::mad::max(ymax, y);
456 
457  for (int i = imin + 1; i <= imax; i++)
458  {
459  y = m_parent->m_data(m_row + 1, i);
460  ymin = adore::mad::min(ymin, y);
461  ymax = adore::mad::max(ymax, y);
462  }
463  }
464  };
466  public:
470  virtual ALFunction<DT, T>* dimension(int i) override
471  {
472  return &single_dimensions[i];
473  }
474  public:
479  {
480  m_searchIndex = 0;
481  for (int i = 0; i < n; i++)
482  {
483  single_dimensions[i] = OneDimension(this, i);
484  }
485  }
487  {
488  if(&other==this)return *this;
489  this->m_data = other.m_data;
490  this->m_searchIndex = other.m_searchIndex;
491  for (int i = 0; i < n; i++)
492  {
493  single_dimensions[i] = OneDimension(this, i);
494  }
495  return *this;
496  }
501  {
502  this->m_data = other.m_data;
504  for (int i = 0; i < n; i++)
505  {
506  single_dimensions[i] = OneDimension(this, i);
507  }
508  }
512  LLinearPiecewiseFunctionM(const adoreMatrix<T, n + 1, 0>& data)
513  {
514  this->m_data = data;
515  m_searchIndex = 0;
516  for (int i = 0; i < n; i++)
517  {
518  single_dimensions[i] = OneDimension(this, i);
519  }
520  }
524  LLinearPiecewiseFunctionM(const adoreMatrix<T, 1, 0>& xdata,const adoreMatrix<T, n , 0>& ydata)
525  {
526  dlib::set_rowm(this->m_data,0) = xdata;
527  dlib::set_rowm(this->m_data,dlib::range(1l,n+1l)) = ydata;
528  m_searchIndex = 0;
529  for (int i = 0; i < n; i++)
530  {
531  single_dimensions[i] = OneDimension(this, i);
532  }
533  }
537  LLinearPiecewiseFunctionM(const int nc,T value)
538  {
539  this->m_data.set_size(n+1,nc);
540  this->m_data = dlib::ones_matrix<T>(n+1,nc)*value;
541  m_searchIndex = 0;
542  for (int i = 0; i < n; i++)
543  {
544  single_dimensions[i] = OneDimension(this, i);
545  }
546  }
548  virtual ALFunction<DT, CT>* clone() override
549  {
550  return new LLinearPiecewiseFunctionM<T, n>(this->m_data);
551  }
552  //sorry, cannot be virtualized due to c++ limitations, first parameter cannot be removed due to how variadric arguments work, no templated friend method of templated class.
553 /* template<int Nnew>
554  LLinearPiecewiseFunctionM<T, Nnew>* clone(int first, va_list args)
555  {
556  adoreMatrix<T, Nnew + 1, 0> clone_data;
557  clone_data = dlib::zeros_matrix<T>(Nnew + 1, m_data.nc());
558  set_rowm(clone_data, 0) = rowm(m_data, 0);//copy x
559  set_rowm(clone_data, 1) = rowm(m_data, first);//copy first index
560 
561  for (int i = 0; i < Nnew - 1; i++)
562  {
563  int dim = va_arg(args, int );
564  set_rowm(clone_data, i + 2) = rowm(m_data, dim + 1);//copy yi
565  }
566  va_end(args);
567  return new LLinearPiecewiseFunctionM<T, Nnew>(clone_data);
568  }
569  template<int Nnew>
570  LLinearPiecewiseFunctionM<T, Nnew>* clone(int first, ...)
571  {
572  va_list args;
573  va_start(args, first);
574  return clone<Nnew>(first, args);
575  }
576 */
580  void setData(const adoreMatrix<T, n + 1, 0>& data)
581  {
582  this->m_data = data;
583  }
584 
585  public://operators
589  virtual void multiply(adoreMatrix<T, 0, 0> A, int rowi, int rowj) override
590  {
591  assert(A.nc() == A.nr());
592  assert(A.nr() == rowj - rowi + 1);
593  assert(rowi >= 0);
594  assert(rowj < n);
595 
596  set_rowm(m_data, dlib::range(rowi + 1, rowj + 1)) = A * rowm(m_data, dlib::range(rowi + 1, rowj + 1));
597  }
601  virtual void add(adoreMatrix<T, 0, 1> b, int rowi, int rowj) override
602  {
603  assert(b.nr() == rowj - rowi + 1);
604  assert(rowi >= 0);
605  assert(rowj < n);
606  for (int i = 0; i < m_data.nc(); i++)
607  {
608  set_subm(m_data, dlib::range(rowi + 1, rowj + 1), dlib::range(i, i)) = subm(m_data, dlib::range(rowi + 1, rowj + 1), dlib::range(i, i)) + b;
609  }
610  }
611 
612  public://intersection test and other useful operations
617  std::vector<std::pair<T, T> >* getIntersections2d(LLinearPiecewiseFunctionM<T, n>* other, int dim1, int dim2)
618  {
619  std::vector<std::pair<T, T> >* result = new std::vector<std::pair<T, T>>();
620  T a0, b0, c0, d0, a1, b1, c1, d1, e0, e1, f0, f1;
621  T xi0, xi1, xj0, xj1, xi, xj,alpha,beta;
622  for (int i = 0; i < this->m_data.nc() - 1; i++)
623  {
624  xi0 = this->m_data(0, i);
625  xi1 = this->m_data(0, i+1);
626  a0 = this->m_data(dim1 + 1, i);
627  a1 = this->m_data(dim2 + 1, i);
628  b0 = this->m_data(dim1 + 1, i + 1);
629  b1 = this->m_data(dim2 + 1, i + 1);
630  e0 = (b0 - a0) ;
631  e1 = (b1 - a1) ;
632  for (int j = 0; j < other->m_data.nc() - 1; j++)
633  {
634  xj0 = other->m_data(0, j);
635  xj1 = other->m_data(0, j+1);
636  c0 = other->m_data(dim1 + 1, j);
637  c1 = other->m_data(dim2 + 1, j);
638  d0 = other->m_data(dim1 + 1, j + 1);
639  d1 = other->m_data(dim2 + 1, j + 1);
640  f0 = (d0 - c0);
641  f1 = (d1 - c1);
642  if (e1 * f0 - e0 * f1 == 0) continue;//parallel lines
643  alpha = (c0*f1-c1*f0-a0*f1+a1*f0) / (e0*f1-e1*f0);
644  beta = (a0*e1-a1*e0-c0*e1+c1*e0) / (e1*f0-e0*f1);
645  if( 0.0<=alpha && alpha<=1.0
646  && 0.0<=beta && beta<=1.0 )
647  {
648  xi = xi0+alpha*(xi1-xi0);
649  xj = xj0+beta*(xj1-xj0);
650  result->push_back(std::pair<T, T>(xi,xj));//intersection in interval
651  }
652  }
653  }
654  return result;
655  }
656  std::vector<std::pair<T, T> >* getIntersections2d(LLinearPiecewiseFunctionM<T, n>* other)
657  {
658  return getIntersections2d(other, 0, 1);
659  }
664  bool getFirstIntersection2d(LLinearPiecewiseFunctionM<T, n>* other, int dim1, int dim2, std::pair<T,T>& result)
665  {
666  T a0, b0, c0, d0, a1, b1, c1, d1, e0, e1, f0, f1;
667  T xi0, xi1, xj0, xj1, alpha,beta;
668  for (int i = 0; i < this->m_data.nc() - 1; i++)
669  {
670  xi0 = this->m_data(0, i);
671  xi1 = this->m_data(0, i+1);
672  a0 = this->m_data(dim1 + 1, i);
673  a1 = this->m_data(dim2 + 1, i);
674  b0 = this->m_data(dim1 + 1, i + 1);
675  b1 = this->m_data(dim2 + 1, i + 1);
676  e0 = (b0 - a0);
677  e1 = (b1 - a1);
678  for (int j = 0; j < other->m_data.nc() - 1; j++)
679  {
680  xj0 = other->m_data(0, j);
681  xj1 = other->m_data(0, j+1);
682  c0 = other->m_data(dim1 + 1, j);
683  c1 = other->m_data(dim2 + 1, j);
684  d0 = other->m_data(dim1 + 1, j + 1);
685  d1 = other->m_data(dim2 + 1, j + 1);
686  f0 = (d0 - c0);
687  f1 = (d1 - c1);
688  if (std::abs(e1 * f0 - e0 * f1 )< 1.0e-3) continue;//parallel lines
689  alpha = (c0*f1-c1*f0-a0*f1+a1*f0) / (e0*f1-e1*f0);
690  beta = (a0*e1-a1*e0-c0*e1+c1*e0) / (e1*f0-e0*f1);
691  if( 0.0<=alpha && alpha<=1.0
692  && 0.0<=beta && beta<=1.0 )
693  {
694  double dx = a0 + e0*alpha - (c0 + f0*beta);
695  double dy = a1 + e1*alpha - (c1 + f1*beta);
696  if(dx*dx+dy*dy>1e-4){std::cout << "Llinearpiecewise l 635"<<std::endl;continue;}
697  result.first = xi0+alpha*(xi1-xi0);
698  result.second = xj0+beta*(xj1-xj0);
699  return true;//intersection in interval
700  }
701  }
702  }
703  return false;
704  }
705  bool getFirstIntersection2d(LLinearPiecewiseFunctionM<T, n>* other, std::pair<T,T>& result)
706  {
707  return getFirstIntersection2d(other, 0, 1,result);
708  }
709 
716  bool getFirstIntersection1d(T y_cross,int dimension,T& x)
717  {
718  double dir = m_data(dimension,0)>y_cross?1.0:-1.0;
719  for(int i=1;i<m_data.nc();i++)
720  {
721  if( m_data(dimension,i)*dir<y_cross*dir )
722  {
723  x = (y_cross-m_data(dimension,i-1))/(m_data(dimension,i)-m_data(dimension,i-1))*(m_data(0,i)-m_data(0,i-1))+m_data(0,i-1);
724  return true;
725  }
726  }
727  return false;
728  }
729  bool getFirstIntersection1d(T y_cross,T& x)
730  {
731  return getFirstIntersection1d(y_cross,1,x);
732  }
739  bool getFirstIntersection1d(LLinearPiecewiseFunctionM<T, n>* other,int dim,std::pair<T,T>& result)
740  {
741  double xstart = (std::max)(this->limitLo(),other->limitLo());
742  double xend = (std::min)(this->limitHi(),other->limitHi());
743  if(xstart<xend)
744  {
745  int i = this->findIndex(xstart);
746  int j = other->findIndex(xstart);
747  bool direction = this->fi(xstart,dim)<other->fi(xstart,dim);
748  while(i<this->m_data.nc() && j<other->m_data.nc())
749  {
750  double x = adore::mad::bound((std::min)(this->limitLo(),other->limitLo()),(std::max)(this->m_data(0,i),other->m_data(0,j)), (std::min)(this->limitHi(),other->limitHi()));
751  bool new_direction = this->fi(x,dim)<other->fi(x,dim);
752  double x_this = this->getData()(0,i);
753  double x_other = other->getData()(0,j);
754  if( new_direction!=direction )
755  {
756  if(x_this<x_other)
757  {
758  i--;
759  }
760  else
761  {
762  j--;
763  }
764  i = (std::min<unsigned int>)(this->m_data.nc()-1,i);
765  j = (std::min<unsigned int>)(other->m_data.nc()-1,j);
766  double xthis;
767  double xother;
768  adore::mad::intersectLines2(this->m_data(0,i),this->m_data(dim+1,i),this->m_data(0,i+1),this->m_data(dim+1,i+1),
769  other->m_data(0,j),other->m_data(dim+1,j),other->m_data(0,j+1),other->m_data(dim+1,j+1),
770  xthis,xother,0.0);
771  result.first = this->m_data(0,i) + (this->m_data(0,i+1)-this->m_data(0,i))*xthis;
772  result.second = other->m_data(0,j) + (other->m_data(0,j+1)-other->m_data(0,j))*xother;
773  return true;
774  }
775  //double x_this = this->getData()(0,i);
776  //double x_other = other->getData()(0,j);
777  if(x_this<x_other)
778  {
779  i++;
780  }
781  else
782  {
783  j++;
784  }
785  }
786  return false;
787  }
788  else
789  {
790  return false;
791  }
792  }
793 
801  bool getNextIntersectionWithLine2d(T x0, T px, T py, T vx, T vy, int d1, int d2, T& x_result, T& distance,bool extend_fringes=false,bool inside_input_line=false)
802  {
803  int i0 = findIndex(x0);
804  double xa, xb;
805  bool xa_inside;
806  bool xb_inside;
807  for (int i = i0; i < m_data.nc() - 1; i++)
808  {
809  adore::mad::intersectLines(px, py, px + vx, py + vy,
810  m_data(d1 + 1, i), m_data(d2 + 1, i), m_data(d1 + 1, i + 1), m_data(d2 + 1, i + 1),
811  xa, xb, xa_inside, xb_inside);
812  if (xb_inside && (!inside_input_line || xa_inside))
813  {
814  x_result = m_data(0, i) + (m_data(0, i + 1) - m_data(0, i)) * xb;
815  distance = xa;
816  return true;
817  }
818  else
819  {
820  if(extend_fringes)
821  {
822  //intersection before start of domain?
823  if(i==0 && xb<=0)
824  {
825  x_result = limitLo();
826  distance = xa;
827  return true;
828  }
829  if( i==m_data.nc()-1 && xb>1 )
830  {
831  x_result = limitHi();
832  distance = xa;
833  return true;
834  }
835  }
836  }
837  }
838  return false;
839  }
840  bool getNextIntersectionWithLine2d(T x0, T px, T py, T vx, T vy, T& x_result, T& distance, bool extend_fringes=false)
841  {
842  return getNextIntersectionWithLine2d(x0, px, py, vx, vy, 0, 1, x_result, distance,extend_fringes);//for dimension 0 and 1
843  }
844  /*
845  * takes an initial search location x0,
846  * a point px,py and a unit vector vx,vy
847  * returns true if an intersection can be found with |distance|<max_distance
848  * returns also the intersection parameter x_result and the true distance between px,py and the intersection point, with a positive distance indicating in direction of vector and negative distance the opposite
849  */
850  bool getNextIntersectionWithVector2d(T x0, T px, T py, T vx, T vy, T& x_result, T& distance, T max_distance,bool extend_fringes=false)
851  {
852  T s;//parameter for line
853  T px1,py1,vx1,vy1;//shifted+stretched values
854  px1 = px-max_distance*vx;
855  py1 = py-max_distance*vy;
856  vx1 = ((T)2)*max_distance*vx;
857  vy1 = ((T)2)*max_distance*vy;
858  bool rv = getNextIntersectionWithLine2d(x0,px1,py1,vx1,vy1, 0, 1, x_result,s,extend_fringes,true);
859  if(rv)
860  {
861  distance = ((T)2)*max_distance*s-max_distance;
862  }
863  else
864  {
865  distance = max_distance;
866  }
867  return rv;
868  }
869 
870 
880  int countIntersectionsWithRay2d(T px, T py, int d1, int d2, T ex, T ey,bool inverted = false)
881  {
882  double count = 0;
883  double rx,ry;//lower point
884  double sx,sy;//higher point
885  double x0;//x-value at intersection of x-axis
886 
887  for (int i = 0; i < m_data.nc() - 1; i++)
888  {
889  //order points i and i+1 by y-value
890  if(m_data(d2,i)<m_data(d2,i+1))
891  {
892  rx = m_data(d1,i)-px; ry=m_data(d2,i)-py;
893  sx = m_data(d1,i+1)-px; sy=m_data(d2,i+1)-py;
894  }
895  else
896  {
897  if(m_data(d2,i+1)<m_data(d2,i))
898  {
899  rx = m_data(d1,i+1)-px; ry=m_data(d2,i+1)-py;
900  sx = m_data(d1,i)-px; sy=m_data(d2,i)-py;
901  }
902  else
903  {
904  continue;//no delta y
905  }
906  }
907  if(ry<(T)0 && (T)0<sy)//is there a zero transition?
908  {
909  //find intersection with x-axis
910  x0 = rx-ry*(sx-rx)/(sy-ry);
911  if(x0>(T)0)
912  {
913  count++;
914  }
915  }
916  }
917 
918  //extension point
919  int i=(inverted)?0:m_data.nc()-1;
920  if(m_data(d2,i)<ey)
921  {
922  rx = m_data(d1,i)-px; ry=m_data(d2,i)-py;
923  sx = ex-px; sy=ey-py;
924  }
925  else
926  {
927  if(ey<m_data(d2,i))
928  {
929  rx = ex-px; ry=ey-py;
930  sx = m_data(d1,i)-px; sy=m_data(d2,i)-py;
931  }
932  else
933  {
934  return count;//no delta y
935  }
936  }
937  if(ry<(T)0 && (T)0<sy)//is there a zero transition?
938  {
939  //find intersection with x-axis
940  x0 = rx-ry*(sx-rx)/(sy-ry);
941  if(x0>(T)0)
942  {
943  count++;
944  }
945  }
946 
947 
948  return count;
949  }
950 
955  double getPositionOfPoint(T px, T py, int d1, int d2, T& d_tangential_min, T& d_normal_min)
956  {
957  T DIST_GUARD = 1e99;
958  T d_tangential_i;
959  T d_normal_i;
960  d_normal_min = DIST_GUARD;
961  d_tangential_min = DIST_GUARD;
962  T xmin = limitLo();
963 
964  for (int i = 0; i < m_data.nc() - 1; i++)
965  {
966 // comparePointWithLine(m_data(d1, 0), m_data(d2, 0), m_data(d1, 1), m_data(d2, 1), px, py, d_tangential_i, d_normal_i);
967  comparePointWithLine(m_data(d1, i), m_data(d2, i), m_data(d1, i+1), m_data(d2, i+1), px, py, d_tangential_i, d_normal_i);
968  if ((T)0 <= d_tangential_i && d_tangential_i <= (T)1)//in interval
969  {
970  if ((T)0 <= d_tangential_min && d_tangential_min <= (T)1)//previously in interval
971  {
972  if ((std::abs)(d_normal_i) < (std::abs)(d_normal_min))//smaller normal distance
973  {
974  d_normal_min = d_normal_i;
975  d_tangential_min = d_tangential_i;
976  xmin = m_data(0,i) + (m_data(0,i+1)-m_data(0,i)) * d_tangential_min;
977  xmin = (std::max)(limitLo(),(std::min)(xmin,limitHi()));
978  }
979  }
980  else//previously not in interval
981  {
982  d_normal_min = d_normal_i;
983  d_tangential_min = d_tangential_i;
984  xmin = m_data(0,i) + (m_data(0,i+1)-m_data(0,i)) * d_tangential_min;
985  xmin = (std::max)(limitLo(),(std::min)(xmin,limitHi()));
986  }
987  }
988  else//not in interval
989  {
990  if (d_tangential_min < (T)0 || (T)1 < d_tangential_min)//previously not in interval
991  {
992  if ((std::abs)(d_tangential_i - (T)0.5) < (std::abs)(d_tangential_min - (T)0.5))//smaller distance to center of interval
993  {
994  d_normal_min = d_normal_i;
995  d_tangential_min = d_tangential_i;
996  if(d_tangential_min<(T)0)
997  {
998  xmin = limitLo();
999  }
1000  else
1001  {
1002  xmin = limitHi();
1003  }
1004  }
1005  }
1006  }
1007  }
1008  return xmin;
1009  }
1010 
1014  double getClosestParameter(T px, T py, int d1, int d2,T& n_min) const
1015  {
1016  T rel,x_min=limitLo();
1017  T d, d_min = 1e99,normal;
1018  for(int i=0;i<m_data.nc()-1;i++)
1019  {
1020  d = getDistancePointToLine(m_data(d1, i), m_data(d2, i), m_data(d1, i+1), m_data(d2, i+1), px, py, rel,normal);
1021  if( d<d_min )
1022  {
1023  d_min = d;
1024  n_min = normal;
1025  x_min = m_data(0,i) + (m_data(0,i+1)-m_data(0,i)) * rel;
1026  }
1027  }
1028  x_min = (std::max)(limitLo(),(std::min)(x_min,limitHi()));
1029  return x_min;
1030  }
1034  double getClosestParameter_local(T px, T py, int d1, int d2,T x0,T* n_min=nullptr) const
1035  {
1036  int i0 = std::max(0,(int)findIndex(x0));
1037  T rel,x_min=limitLo();
1038  T d, d_min = 1e99,normal;
1039  d_min = getDistancePointToLine(m_data(d1, i0), m_data(d2, i0), m_data(d1, i0+1), m_data(d2, i0+1), px, py, rel,normal);
1040  x_min = m_data(0,i0) + (m_data(0,i0+1)-m_data(0,i0)) * rel;
1041 
1042  for(int i=i0+1;i<m_data.nc()-1;i++)
1043  {
1044  if( std::abs(m_data(d1,i+1)-m_data(d1,i))<1e-10
1045  && std::abs(m_data(d2,i+1)-m_data(d2,i))<1e-10 )continue;
1046  d = getDistancePointToLine(m_data(d1, i), m_data(d2, i), m_data(d1, i+1), m_data(d2, i+1), px, py, rel,normal);
1047  if( d<d_min )
1048  {
1049  d_min = d;
1050  x_min = m_data(0,i) + (m_data(0,i+1)-m_data(0,i)) * rel;
1051  if(n_min!=nullptr)*n_min=normal;
1052  }
1053  else
1054  {
1055  break;
1056  }
1057  }
1058  x_min = (std::max)(limitLo(),(std::min)(x_min,limitHi()));
1059  return x_min;
1060  }
1061 
1062  double getClosestParameter(T px, T py, int d1, int d2) const
1063  {
1064  T d_min;
1065  return getClosestParameter( px, py, d1, d2, d_min);
1066  }
1067 
1068 
1075  bool isPointEnclosed(LLinearPiecewiseFunctionM<T, n>* other, T px, T py, int d1, int d2, bool invert_this = false, bool invert_other = true)
1076  {
1077 
1078  //extension point for this and other (making the connection between the two line sequences)
1079  double ex0,ey0;
1080  double ex1,ey1;
1081  if(invert_other)
1082  {
1083  ex0 = other->m_data(d1,other->m_data.nc()-1);
1084  ey0 = other->m_data(d2,other->m_data.nc()-1);
1085  }
1086  else
1087  {
1088  ex0 = other->m_data(d1,0);
1089  ey0 = other->m_data(d2,0);
1090  }
1091  if(invert_this)
1092  {
1093  ex1 = this->m_data(d1,this->m_data.nc()-1);
1094  ey1 = this->m_data(d2,this->m_data.nc()-1);
1095  }
1096  else
1097  {
1098  ex1 = this->m_data(d1,0);
1099  ey1 = this->m_data(d2,0);
1100  }
1101 
1102  //sum the number of intersections with an arbitrary ray starting at px,py
1103  int count = 0;
1104  count += this->countIntersectionsWithRay2d(px,py,d1,d2,ex0,ey0,invert_this);
1105  count += other->countIntersectionsWithRay2d(px,py,d1,d2,ex1,ey1,invert_other);
1106 
1107  return (count%2)!=0;
1108  }
1109 
1113  T getXAfterNPoints(T xstart, int N)
1114  {
1115  xstart = (std::max)(xstart,limitLo());
1116  int i0 = this->findIndex(xstart);
1117  int i1 = (std::min)(i0 + N - 1, (int) m_data.nc()-1);
1118  return m_data(0, i1);
1119  }
1120 
1124  void writePointsToArray(int i,int j, T* m)
1125  {
1126  for(int k=i;k<=j;k++)
1127  {
1128  for(int d = 0; d<n; d++)
1129  {
1130  m[(k-i)*n+d] = m_data(d+1,k);
1131  }
1132  }
1133  }
1137  void writePointsToArray(int i,int j, int d, T* m)
1138  {
1139  for(int k=i;k<=j;k++)
1140  {
1141  m[k-i] = m_data(d+1,k);
1142  }
1143  }
1144 
1148  int export_points(adoreMatrix<double,0,0>& target, double x0, double x1,double precision)
1149  {
1150  x0 = (std::max)(x0,limitLo());
1151  x1 = (std::min)(x1,limitHi());
1152  int i = findIndex(x0);
1153  int j = findIndex(x1);
1154  //if the distance between x0 and m_data(:,i) is low, use m_data(:,i) as first point
1155  if((std::abs)(m_data(0,i)-x0)<precision)
1156  {
1157  set_subm(target,dlib::range(0,n),dlib::range(0,0)) = subm(m_data,dlib::range(0,n),dlib::range(i,i));
1158  i ++;
1159  }
1160  else
1161  {
1162  //if the distance between x0 and m_data(:,i+1) is low, use m_data(:,i+1) as first point
1163  int l =(std::min)(i+1,(int)m_data.nc());
1164  if((std::abs)(m_data(0,l)-x0)<precision)
1165  {
1166  set_subm(target,dlib::range(0,n),dlib::range(0,0)) = subm(m_data,dlib::range(0,n),dlib::range(l,l));
1167  i+=2;
1168  }
1169  //otherwise, evaluate f(x0) and use this intermediate point as first point in target
1170  else
1171  {
1172  target(0,0) = x0;
1173  set_subm(target,dlib::range(1,n),dlib::range(0,0)) = f(x0);
1174  i++;
1175  }
1176  }
1177  //set intermediate points, if i<=j
1178  if(i<=j)
1179  {
1180  set_subm(target,dlib::range(0,n),dlib::range(1,j-i+1)) = subm(m_data,dlib::range(0,n),dlib::range(i,j));
1181  }
1182  if(x1 - m_data(0,j)>precision || i>j)
1183  {
1184  target(0,j-i+2) = x1;
1185  set_subm(target,dlib::range(1,n),dlib::range(j-i+2,j-i+2)) = f(x1);
1186  return j-i+3;
1187  }
1188  else
1189  {
1190  return j-i+2;
1191  }
1192  }
1193 
1194  };
1195 
1201  template<typename T, int N, int k>
1203  {
1204  public:
1205  typedef typename AScalarToN<T,N>::DT DT;
1207  private:
1208  T m_data[N*k];
1212  inline void idx(DT x, int dim, int & i, int & j) const
1213  {
1214  int c = (std::max)((std::min)(0, (int)(std::floor)((x - m_x1) / m_dx)), k - 1);
1215  i = c*N + dim;
1216  j = (c + 1)*N + dim;
1217  }
1218  inline DT xval(int i)
1219  {
1220  return m_x0 + m_dx * i;
1221  }
1222  public:
1223  typedef adoreMatrix<T, N, 1> CT;
1224  virtual CT f(DT x) const override
1225  {
1226  CT m;
1227  for (int i = 0; i < N; i++)m(i, 1) = fi(x, i);
1228  return m;
1229  }
1230  virtual T fi(DT x, int dim) const override
1231  {
1232  int i, j;
1233  idx(x, dim, i, j);
1234  return m_data[i] + (x - xval(i)) / m_dx * (m_data[j] - m_data[i]);
1235  }
1236  virtual DT limitHi() const override
1237  {
1238  return m_x1;
1239  }
1240  virtual DT limitLo()const override
1241  {
1242  return m_x0;
1243  }
1244  //virtual void invertDirection()override
1245  //{
1246  // throw FunctionNotImplemented();
1247  //}
1248  virtual void setLimits(DT lo, DT hi)override
1249  {
1250  throw FunctionNotImplemented();
1251  }
1253  {
1254  throw FunctionNotImplemented();
1255  }
1256  virtual void bound(const DT& xmin, const DT& xmax, CT& ymin, CT& ymax) override
1257  {
1258  CT y;
1259 
1260  int imax, jmax;
1261  idx(xmax, 0, imax, jmax);
1262  y = f(xmax);
1263  ymin = y;
1264  ymax = y;
1265 
1266  int imin, jmin;
1267  idx(xmin, 0, imin, jmin);
1268  y = f(xmin);
1269  ymin = adore::mad::min(ymin, y);
1270  ymax = adore::mad::max(ymax, y);
1271 
1272  for (int i = jmin; i <= imax; i += N)
1273  {
1274  for (int j = 0; j < N; j++)
1275  {
1276  T yj = m_data[i*N + j];
1277  ymin(j) = (std::min)(ymin(j), yj);
1278  ymax(j) = (std::max)(ymax(j), yj);
1279  }
1280  }
1281  }
1282  private: //scalar versions of this function, for example evaluating to CT double not matrix
1283  class OneDimension :public ALFunction<DT, T>
1284  {
1285  private:
1287  int m_row;
1289  public:
1291  OneDimension(LLinearPiecewiseFunctionA* parent, int row) :m_parent(parent), m_row(row) {}
1292  virtual T f(DT x) override { return m_parent->fi(x, m_row); }
1293  virtual DT limitHi() const override { return m_parent->limitHi(); }
1294  virtual DT limitLo() const override { return m_parent->limitLo(); }
1295  virtual void setLimits(DT lo, DT hi)override { throw FunctionNotImplemented(); }
1296  virtual SUBFUN* clone()override { throw FunctionNotImplemented(); }
1297  virtual SUBFUN* create_derivative()override { throw FunctionNotImplemented(); }
1298  virtual void bound(const DT& xmin, const DT& xmax, T& ymin, T& ymax) override
1299  {
1300  T y;
1301 
1302  int imax, jmax;
1303  m_parent->idx(xmax, m_row, imax, jmax);
1304  y = m_parent->fi(xmax, m_row);
1305  ymin = y;
1306  ymax = y;
1307 
1308  int imin, jmin;
1309  m_parent->idx(xmin, m_row, imin, jmin);
1310  y = m_parent->fi(xmin, m_row);
1311  ymin = (std::min)(ymin, y);
1312  ymax = (std::max)(ymax, y);
1313 
1314  for (int i = jmin; i <= imax; i += N)
1315  {
1316  y = m_parent->m_data[i*N + m_row];
1317  ymin = (std::min)(ymin, y);
1318  ymax = (std::max)(ymax, y);
1319  }
1320  }
1321  };
1323  public: //get access to a scalar version of this function
1324  virtual SUBFUN* dimension(int i) override
1325  {
1326  return &single_dimensions[i];
1327  }
1329  {
1330  m_x0 = x0;
1331  m_x1 = x1;
1332  m_dx = (x1 - x0) / (T)k;
1333  memcpy(m_data, data, sizeof(T)*N*k);
1334  for (int i = 0; i < N; i++)
1335  {
1336  single_dimensions[i] = OneDimension(this, i);
1337  }
1338  }
1340  virtual ALFunction<DT, CT>* clone() override
1341  {
1342  return new LLinearPiecewiseFunctionA<T, N, k>(this->m_data, m_x0, m_x1);
1343  }
1344 
1348  virtual void multiply(adoreMatrix<T, 0, 0> A, int row_start, int row_end) override
1349  {
1350  assert(A.nc() == A.nr());
1351  assert(A.nr() == row_end - row_start + 1);
1352  assert(row_start >= 0);
1353  assert(row_end < N);
1354 
1355  T buf[N];
1356  for (int col = 0; col < k; col++)
1357  {
1358  for (int i = row_start; i <= row_end; i++)
1359  {
1360  buf[i] = (T)0;
1361  for (int j = row_start; j <= row_end; j++)buf[i] += A(i - row_start, j - row_start)*m_data[col*N + j];
1362  }
1363  for (int i = row_start; i <= row_end; i++)
1364  {
1365  m_data[col*N + i] = buf[i];
1366  }
1367  }
1368  }
1369  virtual void add(adoreMatrix<T, 0, 1> b, int row_start, int row_end) override
1370  {
1371  assert(b.nr() == row_end - row_start + 1);
1372  assert(row_start >= 0);
1373  assert(row_end < N);
1374  for (int col = 0; col < k; col++)
1375  {
1376  for (int i = row_start; i <= row_end; i++)
1377  {
1378  m_data[col*N + i] += b(i - row_start, 0);
1379  }
1380  }
1381  }
1382  };
1383 
1384  template<typename T, int dout,int din>
1386  {
1387  adoreMatrix<T> m_out;
1388  m_out.set_size(dout+1,fin.getData().nc());
1389  dlib::set_rowm(m_out,0) = dlib::rowm(fin.getData(),0);
1390  for(int i=0;i<dout;i++)
1391  {
1392  dlib::set_rowm(m_out,i+1) = dlib::rowm(fin.getData(),dimensions[i]+1);
1393  }
1394  return LLinearPiecewiseFunctionM<T,dout>(m_out);
1395  }
1396 
1404  template<typename T,int n_dfun,int n_base,int n_normal,int n_target>
1406  {
1407  static const int it = 0;//maybe move this to template
1408  static const int ix = 1;//maybe move this to template
1409  static const int iy = 2;//maybe move this to template
1410  if(base->getData().nc()!=dfun->getData().nc())
1411  {
1412  dfun->getData().set_size(dfun->getData().nr(),base->getData().nc());
1413  }
1414  dlib::set_rowm(dfun->getData(),it) = dlib::rowm(base->getData(),it);//set dfun's sampling equal to base's
1415  T s = target->limitLo();
1416  for(int j=0;j<dfun->getData().nc();j++)
1417  {
1418  double dj;
1419  target->getNextIntersectionWithVector2d(s, base->getData()(ix,j),
1420  base->getData()(iy,j),
1421  normal->getData()(ix,j),
1422  normal->getData()(iy,j),
1423  s,dj,guard);
1424  if( guardfun!=0 && dj==guard )
1425  {
1426  dj = guardfun->f(adore::mad::bound(guardfun->limitLo(),s,guardfun->limitHi()));
1427  }
1428  dfun->getData()(id,j) = dj;
1429  }
1430  }
1431  }
1432 }
Definition: alfunction.h:74
Definition: alfunction.h:214
T DT
Definition: alfunction.h:216
adoreMatrix< T, N, 1 > CT
Definition: alfunction.h:217
Definition: alfunction.h:54
Definition: alfunction.h:38
Definition: alfunction.h:46
Definition: alfunction.h:30
Definition: llinearpiecewisefunction.h:1284
OneDimension()
Definition: llinearpiecewisefunction.h:1290
int m_row
Definition: llinearpiecewisefunction.h:1287
virtual SUBFUN * create_derivative() override
Definition: llinearpiecewisefunction.h:1297
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:1294
virtual void bound(const DT &xmin, const DT &xmax, T &ymin, T &ymax) override
Definition: llinearpiecewisefunction.h:1298
virtual SUBFUN * clone() override
Definition: llinearpiecewisefunction.h:1296
ALFunction< DT, T > SUBFUN
Definition: llinearpiecewisefunction.h:1288
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:1293
OneDimension(LLinearPiecewiseFunctionA *parent, int row)
Definition: llinearpiecewisefunction.h:1291
LLinearPiecewiseFunctionA * m_parent
Definition: llinearpiecewisefunction.h:1286
virtual void setLimits(DT lo, DT hi) override
Definition: llinearpiecewisefunction.h:1295
virtual T f(DT x) override
Definition: llinearpiecewisefunction.h:1292
Definition: llinearpiecewisefunction.h:1203
virtual void add(adoreMatrix< T, 0, 1 > b, int row_start, int row_end) override
Definition: llinearpiecewisefunction.h:1369
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:1236
LLinearPiecewiseFunctionA(T *data, T x0, T x1)
Definition: llinearpiecewisefunction.h:1328
void idx(DT x, int dim, int &i, int &j) const
Definition: llinearpiecewisefunction.h:1212
adoreMatrix< T, N, 1 > CT
Definition: llinearpiecewisefunction.h:1223
DT m_x0
Definition: llinearpiecewisefunction.h:1209
virtual ~LLinearPiecewiseFunctionA()
Definition: llinearpiecewisefunction.h:1339
virtual void setLimits(DT lo, DT hi) override
Definition: llinearpiecewisefunction.h:1248
virtual SUBFUN * dimension(int i) override
Definition: llinearpiecewisefunction.h:1324
DT m_x1
Definition: llinearpiecewisefunction.h:1210
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:1240
OneDimension single_dimensions[N]
Definition: llinearpiecewisefunction.h:1322
virtual ALFunction< DT, CT > * clone() override
Definition: llinearpiecewisefunction.h:1340
virtual ALFunction< DT, CT > * create_derivative() override
Definition: llinearpiecewisefunction.h:1252
AScalarToN< T, N >::DT DT
Definition: llinearpiecewisefunction.h:1205
virtual void multiply(adoreMatrix< T, 0, 0 > A, int row_start, int row_end) override
Definition: llinearpiecewisefunction.h:1348
DT xval(int i)
Definition: llinearpiecewisefunction.h:1218
DT m_dx
Definition: llinearpiecewisefunction.h:1211
T m_data[N *k]
Definition: llinearpiecewisefunction.h:1208
AScalarToN< T, N >::SUBFUN SUBFUN
Definition: llinearpiecewisefunction.h:1206
virtual void bound(const DT &xmin, const DT &xmax, CT &ymin, CT &ymax) override
Definition: llinearpiecewisefunction.h:1256
virtual T fi(DT x, int dim) const override
Definition: llinearpiecewisefunction.h:1230
virtual CT f(DT x) const override
Definition: llinearpiecewisefunction.h:1224
Definition: llinearpiecewisefunction.h:424
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:432
OneDimension(LLinearPiecewiseFunctionM *parent, int row)
Definition: llinearpiecewisefunction.h:430
int m_row
Definition: llinearpiecewisefunction.h:427
virtual ALFunction< DT, T > * create_derivative() override
Definition: llinearpiecewisefunction.h:439
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:433
OneDimension()
Definition: llinearpiecewisefunction.h:429
virtual T f(DT x) const override
Definition: llinearpiecewisefunction.h:431
virtual void bound(const DT &xmin, const DT &xmax, T &ymin, T &ymax) override
Definition: llinearpiecewisefunction.h:443
virtual void setLimits(DT lo, DT hi)
Definition: llinearpiecewisefunction.h:434
LLinearPiecewiseFunctionM * m_parent
Definition: llinearpiecewisefunction.h:426
virtual ALFunction< DT, T > * clone()
Definition: llinearpiecewisefunction.h:435
Definition: llinearpiecewisefunction.h:139
void writePointsToArray(int i, int j, int d, T *m)
Definition: llinearpiecewisefunction.h:1137
T getXAfterNPoints(T xstart, int N)
Definition: llinearpiecewisefunction.h:1113
double getPositionOfPoint(T px, T py, int d1, int d2, T &d_tangential_min, T &d_normal_min)
Definition: llinearpiecewisefunction.h:955
virtual ALFunction< DT, CT > * clone() override
Definition: llinearpiecewisefunction.h:548
virtual CT f(DT x) const override
Definition: llinearpiecewisefunction.h:251
virtual void bound(const DT &xmin, const DT &xmax, CT &ymin, CT &ymax) override
Definition: llinearpiecewisefunction.h:278
unsigned int m_searchIndex
Definition: llinearpiecewisefunction.h:142
void setData(const adoreMatrix< T, n+1, 0 > &data)
Definition: llinearpiecewisefunction.h:580
bool getFirstIntersection2d(LLinearPiecewiseFunctionM< T, n > *other, int dim1, int dim2, std::pair< T, T > &result)
Definition: llinearpiecewisefunction.h:664
double getClosestParameter_local(T px, T py, int d1, int d2, T x0, T *n_min=nullptr) const
Definition: llinearpiecewisefunction.h:1034
double getClosestParameter(T px, T py, int d1, int d2, T &n_min) const
Definition: llinearpiecewisefunction.h:1014
bool getNextIntersectionWithLine2d(T x0, T px, T py, T vx, T vy, int d1, int d2, T &x_result, T &distance, bool extend_fringes=false, bool inside_input_line=false)
Definition: llinearpiecewisefunction.h:801
bool getFirstIntersection1d(T y_cross, int dimension, T &x)
Definition: llinearpiecewisefunction.h:716
virtual void startDomainAtZero()
Definition: llinearpiecewisefunction.h:330
adoreMatrix< T, n, 1 > CT
Definition: llinearpiecewisefunction.h:144
OneDimension single_dimensions[n]
Definition: llinearpiecewisefunction.h:465
virtual void stretchDomain(DT x0, DT x1)
Definition: llinearpiecewisefunction.h:344
std::vector< std::pair< T, T > > * getIntersections2d(LLinearPiecewiseFunctionM< T, n > *other)
Definition: llinearpiecewisefunction.h:656
void sample_dfidx(DT *xvec, T *yvec, int count, int row)
Definition: llinearpiecewisefunction.h:411
bool getFirstIntersection1d(T y_cross, T &x)
Definition: llinearpiecewisefunction.h:729
LLinearPiecewiseFunctionM(const int nc, T value)
Definition: llinearpiecewisefunction.h:537
virtual ALFunction< DT, T > * dimension(int i) override
Definition: llinearpiecewisefunction.h:470
LLinearPiecewiseFunctionM & operator=(const LLinearPiecewiseFunctionM &other)
Definition: llinearpiecewisefunction.h:486
const adoreMatrix< T, n+1, 0 > & getData() const
Definition: llinearpiecewisefunction.h:152
virtual T fi(DT x, int row) const override
Definition: llinearpiecewisefunction.h:391
LLinearPiecewiseFunctionM()
Definition: llinearpiecewisefunction.h:478
virtual void invertDomain()
Definition: llinearpiecewisefunction.h:320
std::vector< std::pair< T, T > > * getIntersections2d(LLinearPiecewiseFunctionM< T, n > *other, int dim1, int dim2)
Definition: llinearpiecewisefunction.h:617
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
void rotateXY(double angle, double x0=0.0, double y0=0.0)
Definition: llinearpiecewisefunction.h:376
bool isPointEnclosed(LLinearPiecewiseFunctionM< T, n > *other, T px, T py, int d1, int d2, bool invert_this=false, bool invert_other=true)
Definition: llinearpiecewisefunction.h:1075
int export_points(adoreMatrix< double, 0, 0 > &target, double x0, double x1, double precision)
Definition: llinearpiecewisefunction.h:1148
LLinearPiecewiseFunctionM(const adoreMatrix< T, n+1, 0 > &data)
Definition: llinearpiecewisefunction.h:512
bool getNextIntersectionWithLine2d(T x0, T px, T py, T vx, T vy, T &x_result, T &distance, bool extend_fringes=false)
Definition: llinearpiecewisefunction.h:840
LLinearPiecewiseFunctionM(const LLinearPiecewiseFunctionM &other)
Definition: llinearpiecewisefunction.h:500
virtual T dfidx(DT x, int row)
Definition: llinearpiecewisefunction.h:400
LLinearPiecewiseFunctionM(const adoreMatrix< T, 1, 0 > &xdata, const adoreMatrix< T, n, 0 > &ydata)
Definition: llinearpiecewisefunction.h:524
virtual void add(adoreMatrix< T, 0, 1 > b, int rowi, int rowj) override
Definition: llinearpiecewisefunction.h:601
virtual void setLimits(DT lo, DT hi) override
Definition: llinearpiecewisefunction.h:270
unsigned int findIndex(DT x, DT precision=0.001) const
Definition: llinearpiecewisefunction.h:158
void shiftCodomain(CT dy)
Definition: llinearpiecewisefunction.h:357
virtual ~LLinearPiecewiseFunctionM()
Definition: llinearpiecewisefunction.h:547
AScalarToN< T, n >::DT DT
Definition: llinearpiecewisefunction.h:145
bool getFirstIntersection1d(LLinearPiecewiseFunctionM< T, n > *other, int dim, std::pair< T, T > &result)
Definition: llinearpiecewisefunction.h:739
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:259
bool getNextIntersectionWithVector2d(T x0, T px, T py, T vx, T vy, T &x_result, T &distance, T max_distance, bool extend_fringes=false)
Definition: llinearpiecewisefunction.h:850
void writePointsToArray(int i, int j, T *m)
Definition: llinearpiecewisefunction.h:1124
virtual void multiply(adoreMatrix< T, 0, 0 > A, int rowi, int rowj) override
Definition: llinearpiecewisefunction.h:589
virtual ALFunction< DT, CT > * create_derivative() override
Definition: llinearpiecewisefunction.h:274
adoreMatrix< T, n+1, 0 > m_data
Definition: llinearpiecewisefunction.h:141
void shiftDomain(DT dx)
Definition: llinearpiecewisefunction.h:337
int countIntersectionsWithRay2d(T px, T py, int d1, int d2, T ex, T ey, bool inverted=false)
Definition: llinearpiecewisefunction.h:880
virtual double limit_s_to_bounds(double s) const
shifts s to be in between limitLo and limitHi
Definition: llinearpiecewisefunction.h:309
void shiftCodomain(T dy, int i=0)
Definition: llinearpiecewisefunction.h:368
double getClosestParameter(T px, T py, int d1, int d2) const
Definition: llinearpiecewisefunction.h:1062
bool getFirstIntersection2d(LLinearPiecewiseFunctionM< T, n > *other, std::pair< T, T > &result)
Definition: llinearpiecewisefunction.h:705
Definition: llinearpiecewisefunction.h:32
unsigned int findIndex(DT x) const
Definition: llinearpiecewisefunction.h:53
virtual void bound(const DT &xmin, const DT &xmax, CT &ymin, CT &ymax) override
Definition: llinearpiecewisefunction.h:111
LLinearPiecewiseFunctionS(const adoreMatrix< T, 2, 0 > &data)
Definition: llinearpiecewisefunction.h:39
virtual CT f(DT x) const override
Definition: llinearpiecewisefunction.h:83
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:91
virtual ALFunction< DT, CT > * clone()
Definition: llinearpiecewisefunction.h:103
LLinearPiecewiseFunctionS(const adoreMatrix< T, 1, 0 > &x, const adoreMatrix< T, 1, 0 > &y)
Definition: llinearpiecewisefunction.h:44
unsigned int m_searchIndex
Definition: llinearpiecewisefunction.h:37
adoreMatrix< T, 2, 0 > m_data
Definition: llinearpiecewisefunction.h:34
T CT
Definition: llinearpiecewisefunction.h:35
virtual ALFunction< DT, CT > * create_derivative() override
Definition: llinearpiecewisefunction.h:107
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:95
T DT
Definition: llinearpiecewisefunction.h:36
virtual void setLimits(DT lo, DT hi)
Definition: llinearpiecewisefunction.h:99
bool intersectLines(T a, T b, T c, T d, T e, T f, T g, T h, T &x0, T &x1, bool &x0_inside, bool &x1_inside)
Definition: adoremath.h:522
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
LLinearPiecewiseFunctionM< T, dout > getSubFunction(LLinearPiecewiseFunctionM< T, din > &fin, int dimensions[dout])
Definition: llinearpiecewisefunction.h:1385
void defineDistanceMap2d(LLinearPiecewiseFunctionM< T, n_dfun > *dfun, int id, LLinearPiecewiseFunctionM< T, n_base > *base, LLinearPiecewiseFunctionM< T, n_normal > *normal, LLinearPiecewiseFunctionM< T, n_target > *target, T guard, LLinearPiecewiseFunctionM< T, 1 > *guardfun=0)
Definition: llinearpiecewisefunction.h:1405
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
double getDistancePointToLine(T a, T b, T c, T d, T e, T f, T &rel, T &n)
Definition: adoremath.h:265
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
bool intersectLines2(T a, T b, T c, T d, T e, T f, T g, T h, T &x0, T &x1, T eps)
Definition: adoremath.h:546
void comparePointWithLine(T a, T b, T c, T d, T e, T f, T &d_tangential, T &d_normal)
Definition: adoremath.h:229
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
y
Definition: adore_set_goal.py:31
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
Definition: areaofeffectconverter.h:20