CARLA
CollisionStage.cpp
Go to the documentation of this file.
1 
2 #include "carla/geom/Math.h"
3 
6 
8 
9 namespace carla {
10 namespace traffic_manager {
11 
12 using Point2D = bg::model::point<double, 2, bg::cs::cartesian>;
14 
15 using namespace constants::Collision;
17 
19  const std::vector<ActorId> &vehicle_id_list,
20  const SimulationState &simulation_state,
21  const BufferMap &buffer_map,
22  const TrackTraffic &track_traffic,
23  const Parameters &parameters,
24  CollisionFrame &output_array,
25  cc::DebugHelper &debug_helper,
26  RandomGeneratorMap &random_devices)
27  : vehicle_id_list(vehicle_id_list),
28  simulation_state(simulation_state),
29  buffer_map(buffer_map),
30  track_traffic(track_traffic),
31  parameters(parameters),
32  output_array(output_array),
33  debug_helper(debug_helper),
34  random_devices(random_devices) {}
35 
36 void CollisionStage::Update(const unsigned long index) {
37  ActorId obstacle_id = 0u;
38  bool collision_hazard = false;
39  float available_distance_margin = std::numeric_limits<float>::infinity();
40 
41  const ActorId ego_actor_id = vehicle_id_list.at(index);
42  if (simulation_state.ContainsActor(ego_actor_id)) {
43  const cg::Location ego_location = simulation_state.GetLocation(ego_actor_id);
44  const Buffer &ego_buffer = buffer_map.at(ego_actor_id);
45  const unsigned long look_ahead_index = GetTargetWaypoint(ego_buffer, JUNCTION_LOOK_AHEAD).second;
46 
47  ActorIdSet overlapping_actors = track_traffic.GetOverlappingVehicles(ego_actor_id);
48  std::vector<ActorId> collision_candidate_ids;
49 
50  // Run through vehicles with overlapping paths and filter them;
51  float collision_radius_square = SQUARE(MAX_COLLISION_RADIUS);
52  for (ActorId overlapping_actor_id : overlapping_actors) {
53  // If actor is within maximum collision avoidance and vertical overlap range.
54  const cg::Location &overlapping_actor_location = simulation_state.GetLocation(overlapping_actor_id);
55  if (overlapping_actor_id != ego_actor_id
56  && cg::Math::DistanceSquared(overlapping_actor_location, ego_location) < collision_radius_square
57  && std::abs(ego_location.z - overlapping_actor_location.z) < VERTICAL_OVERLAP_THRESHOLD) {
58  collision_candidate_ids.push_back(overlapping_actor_id);
59  }
60  }
61 
62  // Sorting collision candidates in accending order of distance to current vehicle.
63  std::sort(collision_candidate_ids.begin(), collision_candidate_ids.end(),
64  [this, &ego_location](const ActorId &a_id_1, const ActorId &a_id_2) {
65  const cg::Location &e_loc = ego_location;
66  const cg::Location &loc_1 = simulation_state.GetLocation(a_id_1);
67  const cg::Location &loc_2 = simulation_state.GetLocation(a_id_2);
68  return (cg::Math::DistanceSquared(e_loc, loc_1) < cg::Math::DistanceSquared(e_loc, loc_2));
69  });
70 
71  // Check every actor in the vicinity if it poses a collision hazard.
72  for (auto iter = collision_candidate_ids.begin();
73  iter != collision_candidate_ids.end() && !collision_hazard;
74  ++iter) {
75  const ActorId other_actor_id = *iter;
76  const ActorType other_actor_type = simulation_state.GetType(other_actor_id);
77 
78  if (parameters.GetCollisionDetection(ego_actor_id, other_actor_id)
79  && buffer_map.find(ego_actor_id) != buffer_map.end()
80  && simulation_state.ContainsActor(other_actor_id)) {
81  std::pair<bool, float> negotiation_result = NegotiateCollision(ego_actor_id,
82  other_actor_id,
83  look_ahead_index);
84  if (negotiation_result.first) {
85  if ((other_actor_type == ActorType::Vehicle
86  && parameters.GetPercentageIgnoreVehicles(ego_actor_id) <= random_devices.at(ego_actor_id).next())
87  || (other_actor_type == ActorType::Pedestrian
88  && parameters.GetPercentageIgnoreWalkers(ego_actor_id) <= random_devices.at(ego_actor_id).next())) {
89  collision_hazard = true;
90  obstacle_id = other_actor_id;
91  available_distance_margin = negotiation_result.second;
92  }
93  }
94  }
95  }
96  }
97 
98  CollisionHazardData &output_element = output_array.at(index);
99  output_element.hazard_actor_id = obstacle_id;
100  output_element.hazard = collision_hazard;
101  output_element.available_distance_margin = available_distance_margin;
102 }
103 
104 void CollisionStage::RemoveActor(const ActorId actor_id) {
105  collision_locks.erase(actor_id);
106 }
107 
109  collision_locks.clear();
110 }
111 
113 
114  const float velocity = cg::Math::Dot(simulation_state.GetVelocity(actor_id), simulation_state.GetHeading(actor_id));
115  float bbox_extension;
116  // Using a linear function to calculate boundary length.
117  bbox_extension = BOUNDARY_EXTENSION_RATE * velocity + BOUNDARY_EXTENSION_MINIMUM;
118  // If a valid collision lock present, change boundary length to maintain lock.
119  if (collision_locks.find(actor_id) != collision_locks.end()) {
120  const CollisionLock &lock = collision_locks.at(actor_id);
121  float lock_boundary_length = static_cast<float>(lock.distance_to_lead_vehicle + LOCKING_DISTANCE_PADDING);
122  // Only extend boundary track vehicle if the leading vehicle
123  // if it is not further than velocity dependent extension by MAX_LOCKING_EXTENSION.
124  if ((lock_boundary_length - lock.initial_lock_distance) < MAX_LOCKING_EXTENSION) {
125  bbox_extension = lock_boundary_length;
126  }
127  }
128 
129  return bbox_extension;
130 }
131 
133  const ActorType actor_type = simulation_state.GetType(actor_id);
134  const cg::Vector3D heading_vector = simulation_state.GetHeading(actor_id);
135 
136  float forward_extension = 0.0f;
137  if (actor_type == ActorType::Pedestrian) {
138  // Extend the pedestrians bbox to "predict" where they'll be and avoid collisions.
139  forward_extension = simulation_state.GetVelocity(actor_id).Length() * WALKER_TIME_EXTENSION;
140  }
141 
142  cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id);
143 
144  float bbox_x = dimensions.x;
145  float bbox_y = dimensions.y;
146 
147  const cg::Vector3D x_boundary_vector = heading_vector * (bbox_x + forward_extension);
148  const auto perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f).MakeSafeUnitVector(EPSILON);
149  const cg::Vector3D y_boundary_vector = perpendicular_vector * (bbox_y + forward_extension);
150 
151  // Four corners of the vehicle in top view clockwise order (left-handed system).
152  const cg::Location location = simulation_state.GetLocation(actor_id);
153  LocationVector bbox_boundary = {
154  location + cg::Location(x_boundary_vector - y_boundary_vector),
155  location + cg::Location(-1.0f * x_boundary_vector - y_boundary_vector),
156  location + cg::Location(-1.0f * x_boundary_vector + y_boundary_vector),
157  location + cg::Location(x_boundary_vector + y_boundary_vector),
158  };
159 
160  return bbox_boundary;
161 }
162 
164  LocationVector geodesic_boundary;
165 
166  if (geodesic_boundary_map.find(actor_id) != geodesic_boundary_map.end()) {
167  geodesic_boundary = geodesic_boundary_map.at(actor_id);
168  } else {
169  const LocationVector bbox = GetBoundary(actor_id);
170 
171  if (buffer_map.find(actor_id) != buffer_map.end()) {
172  float bbox_extension = GetBoundingBoxExtention(actor_id);
173  const float specific_lead_distance = parameters.GetDistanceToLeadingVehicle(actor_id);
174  bbox_extension = std::max(specific_lead_distance, bbox_extension);
175  const float bbox_extension_square = SQUARE(bbox_extension);
176 
177  LocationVector left_boundary;
178  LocationVector right_boundary;
179  cg::Vector3D dimensions = simulation_state.GetDimensions(actor_id);
180  const float width = dimensions.y;
181  const float length = dimensions.x;
182 
183  const Buffer &waypoint_buffer = buffer_map.at(actor_id);
184  const TargetWPInfo target_wp_info = GetTargetWaypoint(waypoint_buffer, length);
185  const SimpleWaypointPtr boundary_start = target_wp_info.first;
186  const uint64_t boundary_start_index = target_wp_info.second;
187 
188  // At non-signalized junctions, we extend the boundary across the junction
189  // and in all other situations, boundary length is velocity-dependent.
190  SimpleWaypointPtr boundary_end = nullptr;
191  SimpleWaypointPtr current_point = waypoint_buffer.at(boundary_start_index);
192  bool reached_distance = false;
193  for (uint64_t j = boundary_start_index; !reached_distance && (j < waypoint_buffer.size()); ++j) {
194  if (boundary_start->DistanceSquared(current_point) > bbox_extension_square || j == waypoint_buffer.size() - 1) {
195  reached_distance = true;
196  }
197  if (boundary_end == nullptr
198  || cg::Math::Dot(boundary_end->GetForwardVector(), current_point->GetForwardVector()) < COS_10_DEGREES
199  || reached_distance) {
200 
201  const cg::Vector3D heading_vector = current_point->GetForwardVector();
202  const cg::Location location = current_point->GetLocation();
203  cg::Vector3D perpendicular_vector = cg::Vector3D(-heading_vector.y, heading_vector.x, 0.0f);
204  perpendicular_vector = perpendicular_vector.MakeSafeUnitVector(EPSILON);
205  // Direction determined for the left-handed system.
206  const cg::Vector3D scaled_perpendicular = perpendicular_vector * width;
207  left_boundary.push_back(location + cg::Location(scaled_perpendicular));
208  right_boundary.push_back(location + cg::Location(-1.0f * scaled_perpendicular));
209 
210  boundary_end = current_point;
211  }
212 
213  current_point = waypoint_buffer.at(j);
214  }
215 
216  // Reversing right boundary to construct clockwise (left-hand system)
217  // boundary. This is so because both left and right boundary vectors have
218  // the closest point to the vehicle at their starting index for the right
219  // boundary,
220  // we want to begin at the farthest point to have a clockwise trace.
221  std::reverse(right_boundary.begin(), right_boundary.end());
222  geodesic_boundary.insert(geodesic_boundary.end(), right_boundary.begin(), right_boundary.end());
223  geodesic_boundary.insert(geodesic_boundary.end(), bbox.begin(), bbox.end());
224  geodesic_boundary.insert(geodesic_boundary.end(), left_boundary.begin(), left_boundary.end());
225  } else {
226 
227  geodesic_boundary = bbox;
228  }
229 
230  geodesic_boundary_map.insert({actor_id, geodesic_boundary});
231  }
232 
233  return geodesic_boundary;
234 }
235 
237 
238  traffic_manager::Polygon boundary_polygon;
239  for (const cg::Location &location : boundary) {
240  bg::append(boundary_polygon.outer(), Point2D(location.x, location.y));
241  }
242  bg::append(boundary_polygon.outer(), Point2D(boundary.front().x, boundary.front().y));
243 
244  return boundary_polygon;
245 }
246 
248  const ActorId other_actor_id) {
249 
250 
251  std::pair<ActorId, ActorId> key_parts;
252  if (reference_vehicle_id < other_actor_id) {
253  key_parts = {reference_vehicle_id, other_actor_id};
254  } else {
255  key_parts = {other_actor_id, reference_vehicle_id};
256  }
257 
258  uint64_t actor_id_key = 0u;
259  actor_id_key |= key_parts.first;
260  actor_id_key <<= 32;
261  actor_id_key |= key_parts.second;
262 
263  GeometryComparison comparision_result{-1.0, -1.0, -1.0, -1.0};
264 
265  if (geometry_cache.find(actor_id_key) != geometry_cache.end()) {
266 
267  comparision_result = geometry_cache.at(actor_id_key);
268  double mref_veh_other = comparision_result.reference_vehicle_to_other_geodesic;
269  comparision_result.reference_vehicle_to_other_geodesic = comparision_result.other_vehicle_to_reference_geodesic;
270  comparision_result.other_vehicle_to_reference_geodesic = mref_veh_other;
271  } else {
272 
273  const Polygon reference_polygon = GetPolygon(GetBoundary(reference_vehicle_id));
274  const Polygon other_polygon = GetPolygon(GetBoundary(other_actor_id));
275 
276  const Polygon reference_geodesic_polygon = GetPolygon(GetGeodesicBoundary(reference_vehicle_id));
277 
278  const Polygon other_geodesic_polygon = GetPolygon(GetGeodesicBoundary(other_actor_id));
279 
280  const double reference_vehicle_to_other_geodesic = bg::distance(reference_polygon, other_geodesic_polygon);
281  const double other_vehicle_to_reference_geodesic = bg::distance(other_polygon, reference_geodesic_polygon);
282  const auto inter_geodesic_distance = bg::distance(reference_geodesic_polygon, other_geodesic_polygon);
283  const auto inter_bbox_distance = bg::distance(reference_polygon, other_polygon);
284 
285  comparision_result = {reference_vehicle_to_other_geodesic,
286  other_vehicle_to_reference_geodesic,
287  inter_geodesic_distance,
288  inter_bbox_distance};
289 
290  geometry_cache.insert({actor_id_key, comparision_result});
291  }
292 
293  return comparision_result;
294 }
295 
296 std::pair<bool, float> CollisionStage::NegotiateCollision(const ActorId reference_vehicle_id,
297  const ActorId other_actor_id,
298  const uint64_t reference_junction_look_ahead_index) {
299  // Output variables for the method.
300  bool hazard = false;
301  float available_distance_margin = std::numeric_limits<float>::infinity();
302 
303  const cg::Location reference_location = simulation_state.GetLocation(reference_vehicle_id);
304  const cg::Location other_location = simulation_state.GetLocation(other_actor_id);
305 
306  // Ego and other vehicle heading.
307  const cg::Vector3D reference_heading = simulation_state.GetHeading(reference_vehicle_id);
308  // Vector from ego position to position of the other vehicle.
309  cg::Vector3D reference_to_other = other_location - reference_location;
310  reference_to_other = reference_to_other.MakeSafeUnitVector(EPSILON);
311 
312  // Other vehicle heading.
313  const cg::Vector3D other_heading = simulation_state.GetHeading(other_actor_id);
314  // Vector from other vehicle position to ego position.
315  cg::Vector3D other_to_reference = reference_location - other_location;
316  other_to_reference = other_to_reference.MakeSafeUnitVector(EPSILON);
317 
318  float reference_vehicle_length = simulation_state.GetDimensions(reference_vehicle_id).x * SQUARE_ROOT_OF_TWO;
319  float other_vehicle_length = simulation_state.GetDimensions(other_actor_id).x * SQUARE_ROOT_OF_TWO;
320 
321  float inter_vehicle_distance = cg::Math::DistanceSquared(reference_location, other_location);
322  float ego_bounding_box_extension = GetBoundingBoxExtention(reference_vehicle_id);
323  float other_bounding_box_extension = GetBoundingBoxExtention(other_actor_id);
324  // Calculate minimum distance between vehicle to consider collision negotiation.
325  float inter_vehicle_length = reference_vehicle_length + other_vehicle_length;
326  float ego_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length);
327  float cross_detection_range = SQUARE(ego_bounding_box_extension + inter_vehicle_length + other_bounding_box_extension);
328 
329  // Conditions to consider collision negotiation.
330  bool other_vehicle_in_ego_range = inter_vehicle_distance < ego_detection_range;
331  bool other_vehicles_in_cross_detection_range = inter_vehicle_distance < cross_detection_range;
332  float reference_heading_to_other_dot = cg::Math::Dot(reference_heading, reference_to_other);
333  bool other_vehicle_in_front = reference_heading_to_other_dot > 0;
334  const Buffer &reference_vehicle_buffer = buffer_map.at(reference_vehicle_id);
335  SimpleWaypointPtr closest_point = reference_vehicle_buffer.front();
336  bool ego_inside_junction = closest_point->CheckJunction();
337  TrafficLightState reference_tl_state = simulation_state.GetTLS(reference_vehicle_id);
338  bool ego_at_traffic_light = reference_tl_state.at_traffic_light;
339  bool ego_stopped_by_light = reference_tl_state.tl_state != TLS::Green && reference_tl_state.tl_state != TLS::Off;
340  SimpleWaypointPtr look_ahead_point = reference_vehicle_buffer.at(reference_junction_look_ahead_index);
341  bool ego_at_junction_entrance = !closest_point->CheckJunction() && look_ahead_point->CheckJunction();
342 
343  // Conditions to consider collision negotiation.
344  if (!(ego_at_junction_entrance && ego_at_traffic_light && ego_stopped_by_light)
345  && ((ego_inside_junction && other_vehicles_in_cross_detection_range)
346  || (!ego_inside_junction && other_vehicle_in_front && other_vehicle_in_ego_range))) {
347  GeometryComparison geometry_comparison = GetGeometryBetweenActors(reference_vehicle_id, other_actor_id);
348 
349  // Conditions for collision negotiation.
350  bool geodesic_path_bbox_touching = geometry_comparison.inter_geodesic_distance < 0.1;
351  bool vehicle_bbox_touching = geometry_comparison.inter_bbox_distance < 0.1;
352  bool ego_path_clear = geometry_comparison.other_vehicle_to_reference_geodesic > 0.1;
353  bool other_path_clear = geometry_comparison.reference_vehicle_to_other_geodesic > 0.1;
354  bool ego_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic < geometry_comparison.other_vehicle_to_reference_geodesic;
355  bool other_path_priority = geometry_comparison.reference_vehicle_to_other_geodesic > geometry_comparison.other_vehicle_to_reference_geodesic;
356  bool ego_angular_priority = reference_heading_to_other_dot< cg::Math::Dot(other_heading, other_to_reference);
357 
358  // Whichever vehicle's path is farthest away from the other vehicle gets priority to move.
359  bool lower_priority = !ego_path_priority && (other_path_priority || !ego_angular_priority);
360  bool blocked_by_other_or_lower_priority = !ego_path_clear || (other_path_clear && lower_priority);
361  bool yield_pre_crash = !vehicle_bbox_touching && blocked_by_other_or_lower_priority;
362  bool yield_post_crash = vehicle_bbox_touching && !ego_angular_priority;
363 
364  if (geodesic_path_bbox_touching && (yield_pre_crash || yield_post_crash)) {
365 
366  hazard = true;
367 
368  const float reference_lead_distance = parameters.GetDistanceToLeadingVehicle(reference_vehicle_id);
369  const float specific_distance_margin = std::max(reference_lead_distance, BOUNDARY_EXTENSION_MINIMUM);
370  available_distance_margin = static_cast<float>(std::max(geometry_comparison.reference_vehicle_to_other_geodesic
371  - static_cast<double>(specific_distance_margin), 0.0));
372 
373  ///////////////////////////////////// Collision locking mechanism /////////////////////////////////
374  // The idea is, when encountering a possible collision,
375  // we should ensure that the bounding box extension doesn't decrease too fast and loose collision tracking.
376  // This enables us to smoothly approach the lead vehicle.
377 
378  // When possible collision found, check if an entry for collision lock present.
379  if (collision_locks.find(reference_vehicle_id) != collision_locks.end()) {
380  CollisionLock &lock = collision_locks.at(reference_vehicle_id);
381  // Check if the same vehicle is under lock.
382  if (other_actor_id == lock.lead_vehicle_id) {
383  // If the body of the lead vehicle is touching the reference vehicle bounding box.
384  if (geometry_comparison.other_vehicle_to_reference_geodesic < 0.1) {
385  // Distance between the bodies of the vehicles.
386  lock.distance_to_lead_vehicle = geometry_comparison.inter_bbox_distance;
387  } else {
388  // Distance from reference vehicle body to other vehicle path polygon.
390  }
391  } else {
392  // If possible collision with a new vehicle, re-initialize with new lock entry.
393  lock = {geometry_comparison.inter_bbox_distance, geometry_comparison.inter_bbox_distance, other_actor_id};
394  }
395  } else {
396  // Insert and initialize lock entry if not present.
397  collision_locks.insert({reference_vehicle_id,
398  {geometry_comparison.inter_bbox_distance,
399  geometry_comparison.inter_bbox_distance,
400  other_actor_id}});
401  }
402  }
403  }
404 
405  // If no collision hazard detected, then flush collision lock held by the vehicle.
406  if (!hazard && collision_locks.find(reference_vehicle_id) != collision_locks.end()) {
407  collision_locks.erase(reference_vehicle_id);
408  }
409 
410  return {hazard, available_distance_margin};
411 }
412 
414  geodesic_boundary_map.clear();
415  geometry_cache.clear();
416 }
417 
419  cg::Location one_meter_up(0.0f, 0.0f, 1.0f);
420  for (uint64_t i = 0u; i < boundary.size(); ++i) {
422  boundary[i] + one_meter_up,
423  boundary[i+1 == boundary.size()? 0: i+1] + one_meter_up,
424  0.1f, {255u, 255u, 0u}, 0.05f);
425  }
426 }
427 
428 } // namespace traffic_manager
429 } // namespace carla
bool GetCollisionDetection(const ActorId &reference_actor_id, const ActorId &other_actor_id) const
Method to query collision avoidance rule between a pair of vehicles.
Definition: Parameters.cpp:166
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
GeometryComparison GetGeometryBetweenActors(const ActorId reference_vehicle_id, const ActorId other_actor_id)
Polygon GetPolygon(const LocationVector &boundary)
void DrawBoundary(const LocationVector &boundary)
std::vector< cg::Location > LocationVector
LocationVector GetBoundary(const ActorId actor_id)
float GetBoundingBoxExtention(const ActorId actor_id)
LocationVector GetGeodesicBoundary(const ActorId actor_id)
void reverse(I begin, I end)
Definition: pugixml.cpp:7358
TrafficLightState GetTLS(const ActorId actor_id) const
This class holds the state of all the vehicles in the simlation.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
cg::Vector3D GetDimensions(const ActorId actor_id) const
std::unordered_map< carla::rpc::ActorId, RandomGenerator > RandomGeneratorMap
std::unordered_set< ActorId > ActorIdSet
ActorType GetType(const ActorId actor_id) const
std::deque< std::shared_ptr< SimpleWaypoint > > Buffer
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:66
std::unordered_map< carla::ActorId, Buffer > BufferMap
void DrawLine(const geom::Location &begin, const geom::Location &end, float thickness=0.1f, Color color=Color{255u, 0u, 0u}, float life_time=-1.0f, bool persistent_lines=true)
Definition: DebugHelper.cpp:38
float Length() const
Definition: geom/Vector3D.h:49
cg::Location GetLocation(const ActorId actor_id) const
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
geom::Location Location
Definition: rpc/Location.h:14
#define SQUARE(a)
This file contains various constants used in traffic manager arranged into sensible namespaces for re...
Definition: Constants.h:11
void Update(const unsigned long index) override
carla::ActorId ActorId
bool ContainsActor(ActorId actor_id) const
void RemoveActor(const ActorId actor_id) override
ActorIdSet GetOverlappingVehicles(ActorId actor_id) const
float GetPercentageIgnoreVehicles(const ActorId &actor_id) const
Method to get % to ignore any vehicle.
Definition: Parameters.cpp:260
float GetDistanceToLeadingVehicle(const ActorId &actor_id) const
Method to query distance to leading vehicle for a given vehicle.
Definition: Parameters.cpp:215
cg::Vector3D GetHeading(const ActorId actor_id) const
const SimulationState & simulation_state
bg::model::point< double, 2, bg::cs::cartesian > Point2D
static auto Dot(const Vector3D &a, const Vector3D &b)
Definition: Math.h:58
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
bg::model::polygon< bg::model::d2::point_xy< double > > Polygon
float GetPercentageIgnoreWalkers(const ActorId &actor_id) const
Method to get % to ignore any walker.
Definition: Parameters.cpp:249
const std::vector< ActorId > & vehicle_id_list
std::pair< SimpleWaypointPtr, uint64_t > TargetWPInfo
Method to return the wayPoints from the waypoint Buffer by using target point distance.
cg::Vector3D GetVelocity(const ActorId actor_id) const
Vector3D MakeSafeUnitVector(const float epsilon) const
Definition: geom/Vector3D.h:60
void sort(I begin, I end, const Pred &pred)
Definition: pugixml.cpp:7444
CollisionStage(const std::vector< ActorId > &vehicle_id_list, const SimulationState &simulation_state, const BufferMap &buffer_map, const TrackTraffic &track_traffic, const Parameters &parameters, CollisionFrame &output_array, cc::DebugHelper &debug_helper, RandomGeneratorMap &random_devices)
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
Definition: road/Map.cpp:31
std::vector< CollisionHazardData > CollisionFrame
std::pair< bool, float > NegotiateCollision(const ActorId reference_vehicle_id, const ActorId other_actor_id, const uint64_t reference_junction_look_ahead_index)