ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
curvature.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 
17 #include "alfunction.h"
18 #include <math.h>
19 
20 namespace adore
21 {
22  namespace mad
23  {
30  template<typename T>
31  ALFunction<T, T>* sample_curvature(ALFunction<T, adoreMatrix<T, 2, 1>>* path, adoreMatrix<T, 1, 0> samples)
32  {
35  adoreMatrix<T, 1, 0> values;
36  values = dlib::zeros_matrix<T>(1, samples.nc());
37  adoreMatrix<T, 2, 1> dpx, ddpx;
38  T v;
39  for (int i = 0; i < samples.nc(); i++)
40  {
41  dpx = dp->f(samples(i));
42  ddpx = ddp->f(samples(i));
43  v = sqrt(dpx(0)*dpx(0) + dpx(1)*dpx(1));
44  values(i) = (dpx(0)*ddpx(1) - ddpx(0)*dpx(1)) / (v*v*v);
45  }
46  delete dp;
47  delete ddp;
48  return new LLinearPiecewiseFunctionS<T>(samples, values);
49  }
50 
51 
55  template<int d1,int d2,typename T,int nd>
57  {
58  public:
60  private:
61  TPath* p;
66  public:
71  :p(p)
72  {
73  dp = p->create_derivative();
76  owner_of_dpddp=true;
77  }
82  :p(0),dp(dp),ddp(ddp),dddp(dddp)
83  {
84  owner_of_dpddp=false;
85  }
87  {
88  if(owner_of_dpddp)
89  {
90  delete dp;
91  delete ddp;
92  delete dddp;
93  }
94  }
95  virtual void setLimits(T lo, T hi)
96  {
97  /*
98  * Curvature2d retrieves limits from reference path
99  */
100  throw FunctionNotImplemented();
101  }
102  virtual T limitHi()
103  {
104  return dp->limitHi();
105  }
106  virtual T limitLo()
107  {
108  return dp->limitLo();
109  }
113  virtual T f(T x) const
114  {
115  auto dq = dp->f(x);
116  auto ddq = dp->f(x);
117  auto dddq = dp->f(x);
118  return (dddq(d2)*dq(d1)-dddq(d1)*dq(d2)) - (ddq(d2)*dq(d1)-ddq(d1)*dq(d2))*(ddq(d1)*dq(d1)+ddq(d2)*dq(d2))/(dq(d1)*dq(d1)+dq(d2)*dq(d2));
119  }
121  {
122  if(owner_of_dpddp)
123  {
125  }
126  else
127  {
129  }
130  }
132  {
133  throw FunctionNotImplemented();
134  }
135  virtual void bound(const T& xmin, const T& xmax, T& ymin, T& ymax)
136  {
137  //this function could be implemented
138  throw FunctionNotImplemented();
139  }
140  };
141 
142 
146  template<int d1,int d2,typename T,int nd>
147  class Curvature2d:public ALFunction<T,T>
148  {
149  public:
151  private:
156  public:
161  :p(p)
162  {
163  dp = p->create_derivative();
164  ddp = dp->create_derivative();
165  owner_of_dpddp=true;
166  }
171  :p(0),dp(dp),ddp(ddp)
172  {
173  owner_of_dpddp=false;
174  }
176  {
177  if(owner_of_dpddp)
178  {
179  delete dp;
180  delete ddp;
181  }
182  }
183  virtual void setLimits(T lo, T hi)
184  {
185  /*
186  * Curvature2d retrieves limits from reference path
187  */
188  throw FunctionNotImplemented();
189  }
190  virtual T limitHi()
191  {
192  return dp->limitHi();
193  }
194  virtual T limitLo()
195  {
196  return dp->limitLo();
197  }
201  virtual T f(T x) const
202  {
203  auto dq = dp->f(x);
204  auto ddq = ddp->f(x);
205  return (ddq(d2)*dq(d1)-ddq(d1)*dq(d2)) / sqrt(dq(d1)*dq(d1)+dq(d2)*dq(d2));
206  }
208  {
209  if(owner_of_dpddp)
210  {
211  return new Curvature2d<T>(p);
212  }
213  else
214  {
215  return new Curvature2d<T>(dp,ddp);
216  }
217  }
219  {
221  else throw FunctionNotInitialized();
222  }
223  virtual void bound(const T& xmin, const T& xmax, T& ymin, T& ymax)
224  {
225  //this function could be implemented
226  throw FunctionNotImplemented();
227  }
228  };
229  }
230 }
virtual CT f(DT x) const =0
virtual DT limitHi() const =0
virtual DT limitLo() const =0
virtual ALFunction< DT, CT > * create_derivative()=0
Definition: curvature.h:148
virtual T limitHi()
Definition: curvature.h:190
bool owner_of_dpddp
Definition: curvature.h:155
TPath * p
Definition: curvature.h:152
virtual ALFunction< T, T > * clone()
Definition: curvature.h:207
TPath * dp
Definition: curvature.h:153
virtual T f(T x) const
Definition: curvature.h:201
virtual void setLimits(T lo, T hi)
Definition: curvature.h:183
virtual T limitLo()
Definition: curvature.h:194
Curvature2d(TPath *p)
Definition: curvature.h:160
virtual ALFunction< T, T > * create_derivative()
Definition: curvature.h:218
TPath * ddp
Definition: curvature.h:154
ALFunction< T, adoreMatrix< T, nd, 1 > > TPath
Definition: curvature.h:150
~Curvature2d()
Definition: curvature.h:175
Curvature2d(TPath *dp, TPath *ddp)
Definition: curvature.h:170
virtual void bound(const T &xmin, const T &xmax, T &ymin, T &ymax)
Definition: curvature.h:223
Definition: alfunction.h:38
Definition: alfunction.h:46
Definition: llinearpiecewisefunction.h:32
virtual T limitHi()
Definition: curvature.h:102
virtual void bound(const T &xmin, const T &xmax, T &ymin, T &ymax)
Definition: curvature.h:135
TPath * dddp
Definition: curvature.h:64
virtual T limitLo()
Definition: curvature.h:106
TPath * ddp
Definition: curvature.h:63
TPath * p
Definition: curvature.h:61
ALFunction< T, adoreMatrix< T, nd, 1 > > TPath
Definition: curvature.h:59
TPath * dp
Definition: curvature.h:62
~PartialCurvatureDerivative2d()
Definition: curvature.h:86
bool owner_of_dpddp
Definition: curvature.h:65
PartialCurvatureDerivative2d(TPath *dp, TPath *ddp, TPath *dddp)
Definition: curvature.h:81
virtual void setLimits(T lo, T hi)
Definition: curvature.h:95
virtual ALFunction< T, T > * clone()
Definition: curvature.h:120
virtual T f(T x) const
Definition: curvature.h:113
PartialCurvatureDerivative2d(TPath *p)
Definition: curvature.h:70
virtual ALFunction< T, T > * create_derivative()
Definition: curvature.h:131
ALFunction< T, T > * sample_curvature(ALFunction< T, adoreMatrix< T, 2, 1 >> *path, adoreMatrix< T, 1, 0 > samples)
Definition: curvature.h:31
x
Definition: adore_set_goal.py:30
Definition: areaofeffectconverter.h:20