126 template<
typename Iterator>
127 void update(
double position_on_current,Iterator current_lf, Iterator first_lf, Iterator last_lf)
141 auto gate_region =
getGateRegion(current_lf, first_lf, last_lf);
142 if(gate_region.first==std::next(last_lf)||gate_region.second==std::next(last_lf))
return;
146 for(Iterator it = gate_region.first;it!=std::next(gate_region.second);it++)
152 d += (*it)->getLength();
153 if(it==gate_region.second)
159 for(Iterator it = first_lf;it!=std::next(last_lf);it++)
161 if(it==current_lf)
break;
162 d += (*it)->getLength();
170 int upstream_count = 0;
171 int gate_source_count = 0;
172 int gate_target_count = 0;
173 int downstream_count = 0;
175 for(Iterator it = first_lf;it!=gate_region.first;it++)
181 for(Iterator it = gate_region.first;it!=std::next(gate_region.second);it++)
184 gate_source_count ++;
186 if(adjacent_lane.first==
nullptr||adjacent_lane.second==
nullptr)
return;
188 gate_target_count ++;
195 bool inverted =
false;
222 template <
typename Iterator>
225 std::pair<Iterator,Border*> result = std::make_pair(last_lf,
nullptr);
227 for(Iterator current = first_lf;current!=std::next(last_lf,1);current++)
229 Border* currentBorder = *current;
231 if(adjacent_lane.first==
nullptr||adjacent_lane.second==
nullptr)
240 auto cost_query =
borderCostMap_->find(adjacent_lane.second->m_id);
244 cost = cost_query->second;
250 result = std::make_pair(current,adjacent_lane.second);
268 template<
typename Iterator>
269 std::pair<Iterator,Iterator>
getGateRegion(Iterator current_lf, Iterator first_lf, Iterator last_lf)
271 auto best_gate = findBestGateEntryPoint<Iterator>(current_lf,last_lf);
272 if(best_gate.second==
nullptr)
return std::make_pair(std::next(last_lf,1),std::next(last_lf,1));
273 auto gate_start = best_gate;
274 for(Iterator candidate = std::prev(best_gate.first);candidate!=std::prev(first_lf);candidate--)
277 if(lct.first==
nullptr||lct.second==
nullptr)
break;
279 if(lct.second->isContinuousPredecessorOf(gate_start.second))
281 gate_start.first = candidate;
282 gate_start.second = lct.second;
289 auto gate_end = best_gate;
290 for(Iterator candidate = std::next(best_gate.first);candidate!=std::next(last_lf);candidate++)
293 if(lct.first==
nullptr||lct.second==
nullptr)
break;
295 if(lct.second->isContinuousSuccessorOf(gate_end.second))
297 gate_end.first = candidate;
298 gate_end.second = lct.second;
305 return std::make_pair(gate_start.first,gate_end.first);
This class chooses the successor with the lowest cost until an upper limit on distance is reached.
Definition: borderaccumulator.h:136
virtual void getNextBorder(Border *&border, bool &inverted) override
Get the next border.
Definition: borderaccumulator.h:169
void setContinueOnIncreasingCost(bool value)
set continuation on increasing cost
Definition: borderaccumulator.h:165
Definition: bordercostmap.h:31
efficiently store borders in boost R-tree
Definition: borderset.h:99
std::pair< Border *, Border * > getLaneChangeTarget(Border *current_right, bool direction_left)
computes a pair of borders (left,right) suitable for lane-changing from given source lane
Definition: borderset.h:1336
bool borderTypeValid(Border *b)
check whether border type is in allowed types of set
Definition: borderset.h:204
Selects Borders from BorderSet required for LaneChangeView construction.
Definition: lanechangeborders.h:30
std::pair< Iterator, Iterator > getGateRegion(Iterator current_lf, Iterator first_lf, Iterator last_lf)
computes the gate region for a lane change Supply iterators for a list or vector of Border*,...
Definition: lanechangeborders.h:269
void setLookAhead(double value)
Definition: lanechangeborders.h:85
std::vector< Border * > gate_target_borders_
Definition: lanechangeborders.h:44
bool lc_direction_left_
Definition: lanechangeborders.h:34
void update(double position_on_current, Iterator current_lf, Iterator first_lf, Iterator last_lf)
collects all borders relevant for lane change view in object-variable vectors
Definition: lanechangeborders.h:127
double downstream_length_
Definition: lanechangeborders.h:39
double getRemainingInGate()
Definition: lanechangeborders.h:69
bool isLCDirectionLeft()
Definition: lanechangeborders.h:57
std::vector< Border * > gate_source_borders_
Definition: lanechangeborders.h:45
double lookahead_
Definition: lanechangeborders.h:35
bool isValid()
Definition: lanechangeborders.h:61
double getLookAhead()
Definition: lanechangeborders.h:81
double getLookBehind()
Definition: lanechangeborders.h:89
BorderCostMap * borderCostMap_
Definition: lanechangeborders.h:33
double getNavCostIncrease()
Definition: lanechangeborders.h:101
bool continueOnIncreasingCost_
Definition: lanechangeborders.h:42
double progress_in_gate_
Definition: lanechangeborders.h:37
BorderSet * borderSet_
Definition: lanechangeborders.h:32
std::vector< Border * > upstream_borders_
Definition: lanechangeborders.h:47
LaneChangeBorders(bool lc_direction_left, BorderSet *borderSet, BorderCostMap *borderCostMap)
Definition: lanechangeborders.h:49
std::vector< Border * > downstream_borders_
Definition: lanechangeborders.h:46
double lookbehind_
Definition: lanechangeborders.h:36
double getDistanceToCurrent()
Definition: lanechangeborders.h:77
double distance_to_current_
Definition: lanechangeborders.h:40
void setConinueOnIncreasingCost(bool value)
Definition: lanechangeborders.h:97
bool valid_
Definition: lanechangeborders.h:41
double getProgressInGate()
Definition: lanechangeborders.h:65
double remaining_in_gate_
Definition: lanechangeborders.h:38
std::pair< Iterator, Border * > findBestGateEntryPoint(Iterator first_lf, Iterator last_lf)
computes the best gate entry point for a lane change Supply iterators for lists or vectors of Border*...
Definition: lanechangeborders.h:223
void setLookBehind(double value)
Definition: lanechangeborders.h:93
double getDownstreamLength()
Definition: lanechangeborders.h:73
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
Definition: areaofeffectconverter.h:20
The border struct contains data of the smallest.
Definition: border.h:62
double getLength()
Get the length of the border.
Definition: border.h:703
Struct to organize navigation cost.
Definition: navigationcost.h:26
static double maximum_cost()
Definition: navigationcost.h:30
double getCombinedCost()
Get combined cost.
Definition: navigationcost.h:76