19 #include <boost/geometry.hpp>
20 #include <boost/geometry/geometries/point.hpp>
21 #include <boost/geometry/geometries/box.hpp>
37 typedef boost::geometry::model::point<double,3,boost::geometry::cs::cartesian>
boost_point;
38 typedef boost::geometry::model::box<boost_point>
boost_box;
40 adoreMatrix<double,3,1>
to_;
47 set(0.0,0.0,0.0,0.0,0.0,0.0);
53 void set(
double f1,
double f2,
double f3,
double t1,
double t2,
double t3)
66 void set(
const std::string& s,
bool coordinates_in_UTM=
true)
69 double f1,f2,f3,t1,t2,t3;
72 is>> f1 >> sep >> f2 >> sep >> f3 >> sep >> t1 >> sep >> t2 >> sep >> t3;
73 set(f1,f2,f3,t1,t2,t3);
75 std::cout<<s<<std::endl;
98 && this->
to_(0)==other.
to_(0)
99 && this->
to_(1)==other.
to_(1)
100 && this->
to_(2)==other.
to_(2);
122 void set(
const std::string& s,
bool coordinates_in_UTM=
true)
124 std::size_t pos = s.find(
">");
125 if(pos!=std::string::npos)
127 high_.
set(s.substr(0,pos),coordinates_in_UTM);
128 low_.
set(s.substr(pos+1,s.length()-pos-1),coordinates_in_UTM);
134 if(pos!=std::string::npos)
136 low_.
set(s.substr(0,pos),coordinates_in_UTM);
137 high_.
set(s.substr(pos+1,s.length()-pos-1),coordinates_in_UTM);
150 if(this->unary_!=other.
unary_)
return false;
152 if(this->unary_ && !this->low_.
equals(other.
low_))
return false;
161 l.min_corner().get<0>();
162 const double min0 = (
std::min)(l.min_corner().get<0>(),h.min_corner().get<0>());
163 const double min1 = (
std::min)(l.min_corner().get<1>(),h.min_corner().get<1>());
164 const double min2 = (
std::min)(l.min_corner().get<2>(),h.min_corner().get<2>());
165 const double max0 = (
std::max)(l.max_corner().get<0>(),h.max_corner().get<0>());
166 const double max1 = (
std::max)(l.max_corner().get<1>(),h.max_corner().get<1>());
167 const double max2 = (
std::max)(l.max_corner().get<2>(),h.max_corner().get<2>());
205 template<
typename value_type,
typename Tfirst>
211 return boost::geometry::equals<Tfirst,Tfirst>(v1.first, v2.first) && v1.second == v2.second;
216 boost::geometry::index::quadratic<16>,
217 boost::geometry::index::indexable<idxRegion2Precedence>,
220 template<
typename T1,
typename T2>
241 std::ifstream
file(filename);
244 std::cout<<
"failed to open file\n";
247 while(std::getline(
file,line))
249 if (line.empty() || line.at(0) ==
'#')
256 rule->high_.from_(0)<<
","<<
257 rule->high_.from_(1)<<
","<<
258 rule->high_.from_(2)<<
";"<<
259 rule->high_.to_(0)<<
","<<
260 rule->high_.to_(1)<<
","<<
266 rule->low_.from_(0)<<
","<<
267 rule->low_.from_(1)<<
","<<
268 rule->low_.from_(2)<<
";"<<
269 rule->low_.to_(0)<<
","<<
270 rule->low_.to_(1)<<
","<<
284 myrule->
set(s,use_UTM_coordinates);
302 std::cout<<
"copying...";
306 auto rule = pair.second;
315 for(
auto it =
precedenceRT_.qbegin(boost::geometry::index::intersects(
330 if(it->second->equals(rule))
return true;
347 static const double guard = 1.0e99;
348 auto it =
precedenceRT_.qbegin(boost::geometry::index::intersects(
361 static const double guard = 1.0e99;
362 auto it =
precedenceRT_.qbegin(boost::geometry::index::intersects(
383 static const double guard = 1.0e99;
384 auto it =
precedenceRT_.qbegin(boost::geometry::index::disjoint(
397 std::vector<PrecedenceRule*> removeset;
400 removeset.push_back(it.current()->second);
PrecedenceSet contains PrecedenceRules, indexed by the area they affect.
Definition: precedence.h:179
std::pair< PriorityRoute::boost_box, PrecedenceRule * > idxRegion2Precedence
Definition: precedence.h:214
Region2PrecedenceRT * getAllRules()
returns all rules
Definition: precedence.h:374
void eraseRule(PrecedenceRule *rule)
removes a rule from the container
Definition: precedence.h:337
adore::mad::AFeed< PrecedenceRule > * ruleFeed_
Definition: precedence.h:181
PrecedenceSet()
Definition: precedence.h:184
void update(double radius, double x, double y)
Definition: precedence.h:189
itRegion2PrecedenceRT getRulesOutsideRegion(double x0, double y0, double x1, double y1)
returns a subset of rules outside of a region
Definition: precedence.h:381
bool contains(const PrecedenceRule &rule)
check whether a rule is contained
Definition: precedence.h:313
void refocus(double x0, double y0, double x1, double y1)
removes all rules outside of a region
Definition: precedence.h:395
itRegion2PrecedenceRT getRulesInRegion(double x0, double y0, double x1, double y1) const
returns a subset of rules in a region
Definition: precedence.h:345
itRegion2PrecedenceRT getAllRulesIt() const
returns all rules
Definition: precedence.h:359
void insertRule(const PrecedenceRule &rule)
inserts a copy of the given rule into container
Definition: precedence.h:291
Region2PrecedenceRT precedenceRT_
Definition: precedence.h:232
itpair< Region2PrecedenceRT::const_query_iterator, Region2PrecedenceRT::const_query_iterator > itRegion2PrecedenceRT
Definition: precedence.h:229
void init(PrecedenceSet *other)
initialize by copying entries
Definition: precedence.h:300
bool readFile(const std::string &filename)
reads a set of precedence rules from a file
Definition: precedence.h:238
boost::geometry::index::rtree< idxRegion2Precedence, boost::geometry::index::quadratic< 16 >, boost::geometry::index::indexable< idxRegion2Precedence >, my_equal< idxRegion2Precedence, PriorityRoute::boost_box > > Region2PrecedenceRT
Definition: precedence.h:219
PrecedenceRule * parseRule(const std::string &s, bool use_UTM_coordinates=true)
creates a precedence rule from string and adds it to container.
Definition: precedence.h:281
PrecedenceSet(adore::mad::AFeed< PrecedenceRule > *ruleFeed)
Definition: precedence.h:185
Definition: com_patterns.h:29
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
y
Definition: adore_set_goal.py:31
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
r
Definition: adore_suppress_lanechanges.py:209
string file
Definition: adore_suppress_lanechanges.py:181
Definition: areaofeffectconverter.h:20
The PrecedenceRule defines a precedence relationship between two routes. Vehicles on the low_ priorit...
Definition: precedence.h:108
PrecedenceRule()
Definition: precedence.h:114
bool equals(const PrecedenceRule &other) const
Definition: precedence.h:148
bool unary_
Definition: precedence.h:112
void set(const std::string &s, bool coordinates_in_UTM=true)
reads PrecedenceRule's two PriorityRoutes from the input string exemplary valid string "0....
Definition: precedence.h:122
PriorityRoute low_
Definition: precedence.h:110
PriorityRoute high_
Definition: precedence.h:111
PriorityRoute::boost_box getBoostBox()
Definition: precedence.h:156
Definition: precedence.h:222
itpair(T1 first, T2 second)
Definition: precedence.h:225
T1 first
Definition: precedence.h:223
T2 second
Definition: precedence.h:224
T1 & current()
Definition: precedence.h:226
T2 & end()
Definition: precedence.h:227
custom equal test for iterators
Definition: precedence.h:207
result_type operator()(value_type const &v1, value_type const &v2) const
Definition: precedence.h:209
bool result_type
Definition: precedence.h:208
PriorityRoute implicitly references a route between two coordinates. The coordinates should be chosen...
Definition: precedence.h:35
bool equals(const PriorityRoute &other) const
returns true if all values are equal with other PriorityRoute
Definition: precedence.h:93
adoreMatrix< double, 3, 1 > to_
Definition: precedence.h:40
boost::geometry::model::point< double, 3, boost::geometry::cs::cartesian > boost_point
Definition: precedence.h:37
adoreMatrix< double, 3, 1 > from_
Definition: precedence.h:39
bool coordinates_in_UTM_
Definition: precedence.h:41
void set(double f1, double f2, double f3, double t1, double t2, double t3)
set the two coordinates
Definition: precedence.h:53
void set(const std::string &s, bool coordinates_in_UTM=true)
reads PriorityRoute's two coordinates from a string exemplary valid string "0.0,3....
Definition: precedence.h:66
PriorityRoute()
empty constructor sets coordinates to 0.0^3
Definition: precedence.h:45
boost_box getBoostBox()
returns a box in boost format, which encompasses start/end points
Definition: precedence.h:80
boost::geometry::model::box< boost_point > boost_box
Definition: precedence.h:38