ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
velocityprediction.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 implementation and API
13  ********************************************************************************/
14 #pragma once
16 
17 namespace adore
18 {
19 namespace env
20 {
22  {
23  public:
24  void predict_s_tva(double s0,double v0,double a0,double delay,double a1,double vbound,double t0,double dt,double t1,adore::mad::LLinearPiecewiseFunctionM<double,3>& result)
25  {
26  predict_t_sva(s0,v0,a0,delay,a1,vbound,t0,dt,t1,result);
27  int n = result.getData().nc();
28  for(int i=0;i<n;i++)
29  {
30  double tmp = result.getData()(0,i);
31  result.getData()(0,i)=result.getData()(1,i);
32  result.getData()(1,i)=tmp;
33  if(i>0 && result.getData()(0,i)<=result.getData()(0,i-1))//restore monotony
34  {
35  result.getData()(0,i)=result.getData()(0,i-1)+0.00001;
36  }
37  }
38  }
39  void predict_s_tva(double s0,double v0,double a0,double vbound,double t0,double dt,double t1,adore::mad::LLinearPiecewiseFunctionM<double,3>& result)
40  {
41  predict_t_sva(s0,v0,a0,vbound,t0,dt,t1,result);
42  int n = result.getData().nc();
43  for(int i=0;i<n;i++)
44  {
45  double tmp = result.getData()(0,i);
46  result.getData()(0,i)=result.getData()(1,i);
47  result.getData()(1,i)=tmp;
48  if(i>0 && result.getData()(0,i)<=result.getData()(0,i-1))//restore monotony
49  {
50  result.getData()(0,i)=result.getData()(0,i-1)+0.00001;
51  }
52  }
53  }
54  void predict_t_sva(double s0,double v0,double a0,double vbound,double t0,double dt,double t1,adore::mad::LLinearPiecewiseFunctionM<double,3>& result)
55  {
56  int n = (std::ceil)((t1-t0)/dt);
57  result.getData().set_size(3+1,n);
58  double s = s0;
59  double v = v0;
60  double a = a0;
61  double t = t0;
62  for(int i=0;i<n;i++)
63  {
64  result.getData()(0,i)=t;
65  result.getData()(1,i)=s;
66  result.getData()(2,i)=v;
67  result.getData()(3,i)=a;
68  double dti = dt;
69  if(t+dti>t1)dti=t1-t;
70  t+=dti;
71  if( a>0.0 && v+a*dti>vbound )
72  {
73  a = 0.0;
74  v = vbound;
75  }
76  else if( a<0.0 && v+a*dti<0.0 )
77  {
78  a = 0.0;
79  v = 0.0;
80  }
81  s+=(std::max)(0.0,v*dti+0.5*a*dti*dti);
82  v+=a*dti;
83  }
84  }
85  void predict_t_sva(double s0,double v0,double a0,double delay,double a1,double vbound,double t0,double dt,double t1,adore::mad::LLinearPiecewiseFunctionM<double,3>& result)
86  {
87  int n = (std::ceil)((t1-t0)/dt);
88  result.getData().set_size(3+1,n);
89  double s = s0;
90  double v = v0;
91  double a = delay>dt?a0:a1;
92  double t = t0;
93  for(int i=0;i<n;i++)
94  {
95 
96  result.getData()(0,i)=t;
97  result.getData()(1,i)=s;
98  result.getData()(2,i)=v;
99  result.getData()(3,i)=a;
100  double dti = dt;
101  if(t+dti>t1)dti=t1-t;
102  t+=dti;
103  a = (t+dti<t0+delay) ? a0 : a1;
104  if( a>0.0 && v+a*dti>vbound )
105  {
106  a = 0.0;
107  v = vbound;
108  }
109  else if( a<0.0 && v+a*dti<0.0 )
110  {
111  a = 0.0;
112  v = 0.0;
113  }
114  s+=(std::max)(0.0,v*dti+0.5*a*dti*dti);
115  v+=a*dti;
116  }
117  }
118  };
119 }
120 }
Definition: velocityprediction.h:22
void predict_t_sva(double s0, double v0, double a0, double vbound, double t0, double dt, double t1, adore::mad::LLinearPiecewiseFunctionM< double, 3 > &result)
Definition: velocityprediction.h:54
void predict_t_sva(double s0, double v0, double a0, double delay, double a1, double vbound, double t0, double dt, double t1, adore::mad::LLinearPiecewiseFunctionM< double, 3 > &result)
Definition: velocityprediction.h:85
void predict_s_tva(double s0, double v0, double a0, double vbound, double t0, double dt, double t1, adore::mad::LLinearPiecewiseFunctionM< double, 3 > &result)
Definition: velocityprediction.h:39
void predict_s_tva(double s0, double v0, double a0, double delay, double a1, double vbound, double t0, double dt, double t1, adore::mad::LLinearPiecewiseFunctionM< double, 3 > &result)
Definition: velocityprediction.h:24
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
Definition: areaofeffectconverter.h:20