ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
linearconstraintset.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 <vector>
17 #include <set>
18 #include <map>
21 
22 namespace adore
23 {
24  namespace mad
25  {
30  {
31  private:
32  struct Line
33  {
34  double x0;
35  mutable double y0,x1,y1;//declared mutable to make changes while using const iterator (not affecting sorting)
36  Line(double x0,double y0,double x1,double y1):x0(x0),y0(y0),x1(x1),y1(y1){}
37  inline bool operator<(const Line& rhs) const //compare linex by first x coordinate
38  {
39  return x0<rhs.x0;//sort by x0
40  //return x1<rhs.x1;//sort by x1
41  }
42  inline double operator()(double x)
43  {
44  return (x-x0)*(y1-y0)/(x1-x0)+y0;
45  }
46  };
47  std::vector<Line> I;//initial set
48  public:
49  enum Direction
50  {
51  LOWER=-1,
52  UPPER=1
53  };
54 
61  bool hasConstraints() const
62  {
63  return I.size() > 0;
64  }
65 
69  void insert(double x0,double y0,double x1,double y1)
70  {
71  I.push_back(Line(x0,y0,x1,y1));
72  }
76  void clear()
77  {
78  I.clear();
79  }
85  void bound(function_type_scalar* fun,Direction direction)
86  {
87  double d = (double)direction;
88  double eps = 1e-5;
89 
90  std::set<double> brakepoints;//brakepoints of resulting function
91  for( auto i = I.begin();i!=I.end(); i++)
92  {
93  brakepoints.emplace(i->x0);
94  brakepoints.emplace(i->x1);
95  for( auto j=i;j!=I.end(); j++)
96  {
97  if(i==j)continue;
98  double i_intersect;
99  double j_intersect;
100  if(adore::mad::intersectLines2( i->x0, i->y0, i->x1, i->y1,
101  j->x0, j->y0, j->x1, j->y1,
102  i_intersect,
103  j_intersect,
104  1.0e-6))
105  {
106  brakepoints.emplace(i->x0 + (i->x1-i->x0) * i_intersect);
107  }
108  }
109  }
110  //std::sort(brakepoints.begin(),brakepoints.end());
111 
112  std::vector<std::pair<double,double>> points;
113  for( auto b = brakepoints.begin();b!=brakepoints.end();b++)
114  {
115  double xl = *b-eps;
116  double xu = *b+eps;
117  bool lfound=false;
118  bool ufound=false;
119  double yl = 0;
120  double yu = 0;
121  for(auto i=I.begin();i!=I.end();i++)
122  {
123  if(i->x0<=xl && i->x1>=xl)
124  {
125  double yi = (*i)(xl);
126  if(!lfound || d*yi>d*yl)
127  {
128  yl=yi;
129  lfound = true;
130  }
131  }
132  if(i->x0<=xu && i->x1>=xu)
133  {
134  double yi = (*i)(xu);
135  if(!ufound || d*yi>d*yu)
136  {
137  yu=yi;
138  ufound = true;
139  }
140  }
141  }
142  if(lfound)points.push_back(std::make_pair(xl,yl));
143  if(ufound)points.push_back(std::make_pair(xu,yu));
144  }
145 
146  fun->getData().set_size(2,points.size());
147  for(unsigned int i=0;i<points.size();i++)
148  {
149  fun->getData()(0,i) = points[i].first;
150  fun->getData()(1,i) = points[i].second;
151  }
152  }
153 
157  std::vector<Line> getPossiblyIntersectingSet() const
158  {
159  return I;
160  }
161 
162  };
163 
164  }
165 }
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
Definition: linearconstraintset.h:30
void clear()
Definition: linearconstraintset.h:76
std::vector< Line > I
Definition: linearconstraintset.h:47
Direction
Definition: linearconstraintset.h:50
@ LOWER
Definition: linearconstraintset.h:51
@ UPPER
Definition: linearconstraintset.h:52
void bound(function_type_scalar *fun, Direction direction)
Definition: linearconstraintset.h:85
bool hasConstraints() const
utility function, returns if the set contain any constraints
Definition: linearconstraintset.h:61
std::vector< Line > getPossiblyIntersectingSet() const
Definition: linearconstraintset.h:157
void insert(double x0, double y0, double x1, double y1)
Definition: linearconstraintset.h:69
const double eps
Definition: cubic_piecewise_function.cpp:16
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
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
Definition: linearconstraintset.h:33
double operator()(double x)
Definition: linearconstraintset.h:42
double y0
Definition: linearconstraintset.h:35
double x1
Definition: linearconstraintset.h:35
double y1
Definition: linearconstraintset.h:35
double x0
Definition: linearconstraintset.h:34
bool operator<(const Line &rhs) const
Definition: linearconstraintset.h:37
Line(double x0, double y0, double x1, double y1)
Definition: linearconstraintset.h:36