ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
localboxset.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 
15 
16 #pragma once
17 #include <adore/mad/adoremath.h>
18 #include <boost/geometry.hpp>
19 #include <boost/geometry/geometries/point.hpp>
20 #include <boost/geometry/geometries/box.hpp>
21 
22 namespace adore
23 {
24  namespace env
25  {
34  template<typename TObject,typename TBoxGen,typename TComparator>
36  {
37  public:
38  typedef boost::geometry::model::point<double,3,boost::geometry::cs::cartesian> Tboost_point;
39  typedef boost::geometry::model::box<Tboost_point> Tboost_box;
40  typedef std::pair<Tboost_box,TObject*> TBoxPointerPair;
41  struct EqualFunctor
42  {
43  TComparator compare_;
44  bool operator() (TBoxPointerPair const& v1, TBoxPointerPair const& v2) const
45  {
46  return boost::geometry::equals<Tboost_box,Tboost_box>(v1.first, v2.first)&&(v1.second == v2.second);
47  }
48  };
49  typedef boost::geometry::index::rtree< TBoxPointerPair,
50  boost::geometry::index::quadratic<16>,
51  boost::geometry::index::indexable<TBoxPointerPair>,
53  typedef typename Trtree::const_query_iterator Trtreeit;
54  struct itpair
55  {
58  Trtreeit& current(){return first;}
59  Trtreeit& end(){return second;}
60  };
61  private:
63  TBoxGen boxgen_;
64  TComparator compare_;
65  public:
70  void insert(const TObject& object)
71  {
72  TObject* old = find(object);
73  if(old !=0)
74  {
75  remove(old);
76  }
77  TObject* copy = new TObject(object);
78  Tboost_box box = boxgen_(copy);
79  TBoxPointerPair pair = std::make_pair(box,copy);
80  data_.insert(pair);
81  }
86  TObject* find(const TObject& object)
87  {
88  Tboost_box box = boxgen_(&object);
89  for(auto it = data_.qbegin(boost::geometry::index::intersects(box));
90  it!= data_.qend();
91  it++)
92  {
93  if(compare_(it->second,&object))return it->second;
94  }
95  return 0;
96  }
100  bool contains(const TObject& object)
101  {
102  return find(object)!=0;
103  }
107  void remove(TObject* object)
108  {
109  data_.remove(std::make_pair(boxgen_(object),object));
110  delete object;
111  }
115  int size()const{return data_.size();}
116 
121  itpair getObjectsInRegion(double x0,double y0,double x1,double y1)
122  {
123  static const double guard = 1.0e99;
124  auto it = data_.qbegin(boost::geometry::index::intersects(
125  Tboost_box(
126  Tboost_point(x0,y0,-guard),
127  Tboost_point(x1,y1,+guard)
128  )
129  ));
130  return itpair(it,data_.qend());
131  }
136  itpair getObjectsOutsideRegion(double x0,double y0,double x1,double y1)
137  {
138  static const double guard = 1.0e99;
139  auto it = data_.qbegin(boost::geometry::index::disjoint(
140  Tboost_box(
141  Tboost_point(x0,y0,-guard),
142  Tboost_point(x1,y1,+guard)
143  )
144  ));
145  return itpair(it,data_.qend());
146  }
150  void refocus(double x0,double y0,double x1,double y1)
151  {
152  std::vector<TObject*> removeset;
153  for(auto itp = getObjectsOutsideRegion(x0,y0,x1,y1);
154  itp.current()!=itp.end();
155  itp.current()++)
156  {
157  removeset.push_back(itp.current()->second);
158  }
159  for(auto obj: removeset)remove(obj);
160  }
161  };
162  }
163 }
Definition: localboxset.h:36
boost::geometry::model::point< double, 3, boost::geometry::cs::cartesian > Tboost_point
Definition: localboxset.h:38
boost::geometry::model::box< Tboost_point > Tboost_box
Definition: localboxset.h:39
Trtree data_
Definition: localboxset.h:62
itpair getObjectsInRegion(double x0, double y0, double x1, double y1)
Definition: localboxset.h:121
TBoxGen boxgen_
Definition: localboxset.h:63
void insert(const TObject &object)
Definition: localboxset.h:70
boost::geometry::index::rtree< TBoxPointerPair, boost::geometry::index::quadratic< 16 >, boost::geometry::index::indexable< TBoxPointerPair >, EqualFunctor > Trtree
Definition: localboxset.h:52
std::pair< Tboost_box, TObject * > TBoxPointerPair
Definition: localboxset.h:40
int size() const
Definition: localboxset.h:115
bool contains(const TObject &object)
Definition: localboxset.h:100
void refocus(double x0, double y0, double x1, double y1)
Definition: localboxset.h:150
itpair getObjectsOutsideRegion(double x0, double y0, double x1, double y1)
Definition: localboxset.h:136
Trtree::const_query_iterator Trtreeit
Definition: localboxset.h:53
TComparator compare_
Definition: localboxset.h:64
TObject * find(const TObject &object)
Definition: localboxset.h:86
void remove(TObject *object)
Definition: localboxset.h:107
x0
Definition: adore_set_goal.py:25
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: localboxset.h:42
TComparator compare_
Definition: localboxset.h:43
bool operator()(TBoxPointerPair const &v1, TBoxPointerPair const &v2) const
Definition: localboxset.h:44
Definition: localboxset.h:55
Trtreeit second
Definition: localboxset.h:56
Trtreeit & end()
Definition: localboxset.h:59
Trtreeit first
Definition: localboxset.h:56
itpair(Trtreeit first, Trtreeit second)
Definition: localboxset.h:57
Trtreeit & current()
Definition: localboxset.h:58