ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
occupancycylinder.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
15 #include <adore/mad/adoremath.h>
17 namespace adore
18 {
19  namespace mad
20  {
25  {
26  double rxy_;
27  double x_,y_;
28  double t0_,t1_;
29  double z0_,z1_;
31  OccupancyCylinder(double rxy,double x,double y,double t0,double t1,double z0 = 0.0,double z1 = 3.0)
32  :rxy_(rxy),x_(x),y_(y),t0_(t0),t1_(t1),z0_(z0),z1_(z1){}
37  inline bool intersects(const OccupancyCylinder& other)const
38  {
39  if(!adore::mad::overlaps(t0_,t1_,other.t0_,other.t1_))return false;
40  if(!adore::mad::overlaps(z0_,z1_,other.z0_,other.z1_))return false;
41  const double dx = other.x_-x_;
42  const double dy = other.y_-y_;
43  const double R = other.rxy_+rxy_;
44  return dx*dx+dy*dy<=R*R;
45  }
49  void bound(const OccupancyCylinder& other)
50  {
51  const double dx = other.x_-x_;
52  const double dy = other.y_-y_;
53  const double R = (std::sqrt)(dx*dx+dy*dy) + other.rxy_;
54  rxy_ = (std::max)(rxy_,R);
55  t0_ = (std::min)(t0_,other.t0_);
56  t1_ = (std::max)(t1_,other.t1_);
57  z0_ = (std::min)(z0_,other.z0_);
58  z1_ = (std::max)(z1_,other.z1_);
59  }
60  };
61 
63  {
64  typedef std::pair<std::pair<int,int>,OccupancyCylinder> IndexedVolumeType;
65  typedef std::vector<IndexedVolumeType> VolumeVector;
70  IndexedVolumeType operator()(const VolumeVector& base,int i0,int i1)const
71  {
73  iv.second = base[i0].second;
74  iv.second.x_ = 0.5 * (base[i0].second.x_+base[i1].second.x_);
75  iv.second.y_ = 0.5 * (base[i0].second.y_+base[i1].second.y_);
76  for(int i=i0+1;i<=i1;i++)iv.second.bound(base[i].second);
77  iv.first.first=i0;
78  iv.first.second=i1;
79  return iv;
80  }
81  };
82 
83  class OccupancyCylinderTree:public adore::mad::VectorBasedVolumeTree<OccupancyCylinder,OccupancyCylinderBoundingFunctor>
84  {
85  private:
90  {
91  double operator()(const OccupancyCylinder& a,const OccupancyCylinder& b)const
92  {
93  const double guard = 1.e10;
94  if(!adore::mad::overlaps(a.t0_,a.t1_,b.t0_,b.t1_))return guard;
95  if(!adore::mad::overlaps(a.z0_,a.z1_,b.z0_,b.z1_))return guard;
96  const double dx = a.x_-b.x_;
97  const double dy = a.y_-b.y_;
98  return std::sqrt(dx*dx+dy*dy)-a.rxy_-b.rxy_;
99  }
100  };
105  {
106  double max_time_;
107  EarliestOverlapFunctor(double max_time):max_time_(max_time){}
108  double operator()(const OccupancyCylinder& a,const OccupancyCylinder& b)const
109  {
110  if(!adore::mad::overlaps(a.t0_,a.t1_,b.t0_,b.t1_))return max_time_;
111  if(!adore::mad::overlaps(a.z0_,a.z1_,b.z0_,b.z1_))return max_time_;
112  const double dx = a.x_-b.x_;
113  const double dy = a.y_-b.y_;
114  const double d = std::sqrt(dx*dx+dy*dy)-a.rxy_-b.rxy_;
115  if(d<0.0)
116  {
117  return (std::max)(a.t0_,b.t0_);
118  }
119  else
120  {
121  return max_time_;
122  }
123  }
124  };
130  {
131  double operator()(const OccupancyCylinder& a,const OccupancyCylinder& b)const
132  {
133  const double guard = 1.e10;
134  if(!adore::mad::overlaps(a.z0_,a.z1_,b.z0_,b.z1_))return guard;
135  const double dx = a.x_-b.x_;
136  const double dy = a.y_-b.y_;
137  const double R = a.rxy_+b.rxy_;
138  if(dx*dx+dy*dy>R*R)return guard;
139  if(a.t0_<=b.t0_ && a.t1_>=b.t1_)return -(b.t1_-b.t0_);//b is included in a
140  if(b.t0_<=a.t0_ && b.t1_>=a.t1_)return -(a.t1_-a.t0_);//a is included in b
141  if(a.t0_<=b.t0_ && b.t0_<=a.t1_)return -(a.t1_-b.t0_);//b starts in a
142  if(b.t0_<=a.t0_ && a.t0_<=b.t1_)return -(b.t1_-a.t0_);//a starts in b
143  //no overlap between intervals
144  if(a.t1_<=b.t0_)return b.t0_-a.t1_;//a ends before b: return time gap from a to b
145  return a.t0_-b.t1_;//b ends before a: return time gap from b to a
146  }
147  };
148 
149  public:
153  bool collidesWith(const OccupancyCylinderTree& other)const
154  {
156  const double collision_distance = 0.0001;
157  double result;
158  return compute_min(other,collision_distance,result,f);
159  }
166  bool getPostEncroachmentTime(const OccupancyCylinderTree& other,double max_time,double& result)const
167  {
169  bool rv = compute_min(other,max_time,result,f);
170  if(rv)result=(std::max)(result,0.0);
171  return rv;
172  }
179  bool getEarliestCollisionTime(const OccupancyCylinderTree& other,double max_time,double& result)const
180  {
181  EarliestOverlapFunctor f(max_time);
182  bool rv = compute_min(other,max_time,result,f);
183  return rv;
184  }
185  };
186 
187  }
188 }
Definition: occupancycylinder.h:84
bool getEarliestCollisionTime(const OccupancyCylinderTree &other, double max_time, double &result) const
Definition: occupancycylinder.h:179
bool collidesWith(const OccupancyCylinderTree &other) const
Definition: occupancycylinder.h:153
bool getPostEncroachmentTime(const OccupancyCylinderTree &other, double max_time, double &result) const
Definition: occupancycylinder.h:166
Definition: vectorbasedvolumetree.h:30
bool compute_min(const VectorBasedVolumeTree< OtherVolumeType, OtherFunctorType > &other, double cutoff, double &result_value, const MetricFunctor &f) const
Definition: vectorbasedvolumetree.h:161
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
bool overlaps(const T &a0, const T &a1, const T &b0, const T &b1)
Definition: adoremath.h:655
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x
Definition: adore_set_goal.py:30
z0
Definition: adore_set_goal.py:27
y
Definition: adore_set_goal.py:31
z1
Definition: adore_set_pose.py:30
Definition: areaofeffectconverter.h:20
Definition: occupancycylinder.h:63
std::vector< IndexedVolumeType > VolumeVector
Definition: occupancycylinder.h:65
std::pair< std::pair< int, int >, OccupancyCylinder > IndexedVolumeType
Definition: occupancycylinder.h:64
IndexedVolumeType operator()(const VolumeVector &base, int i0, int i1) const
Definition: occupancycylinder.h:70
EarliestOverlapFunctor(double max_time)
Definition: occupancycylinder.h:107
double operator()(const OccupancyCylinder &a, const OccupancyCylinder &b) const
Definition: occupancycylinder.h:108
double max_time_
Definition: occupancycylinder.h:106
double operator()(const OccupancyCylinder &a, const OccupancyCylinder &b) const
Definition: occupancycylinder.h:131
double operator()(const OccupancyCylinder &a, const OccupancyCylinder &b) const
Definition: occupancycylinder.h:91
Definition: occupancycylinder.h:25
OccupancyCylinder(double rxy, double x, double y, double t0, double t1, double z0=0.0, double z1=3.0)
Definition: occupancycylinder.h:31
double rxy_
Definition: occupancycylinder.h:26
double t0_
Definition: occupancycylinder.h:28
double y_
Definition: occupancycylinder.h:27
double z1_
Definition: occupancycylinder.h:29
double t1_
Definition: occupancycylinder.h:28
double z0_
Definition: occupancycylinder.h:29
bool intersects(const OccupancyCylinder &other) const
Definition: occupancycylinder.h:37
double x_
Definition: occupancycylinder.h:27
OccupancyCylinder()
Definition: occupancycylinder.h:30
void bound(const OccupancyCylinder &other)
Definition: occupancycylinder.h:49