Fix: calculate distance to next lane and intersection using NPC route instead of 3D distance

[kielczykowski-rai]

Description

The Random Traffic calculated the distance to the closest traffic light lane in 3D space. The distance did not take the NPC route into account. This can cause situations where NPC stops moving along a lane, since currently it passes a TrafficLight, that it will reach in the future on its route. (Internal evidence 1, Internal evidence 2)

The PR solves Internal issue 1 and Internal issue 2

Details

The PR introduces:

• calculating a distance to next lane using FollowingLanes in NPCVehicleInternalState
• calculating a distance to Intersection using TrafficLightLane and FollowingLanes in NPCVehicleInternalState

Part of the implementation is extracted to RandomTrafficUtils for better code reusage in the future.

The currently implemented SignedDistanceToPointOnLane was left intact for the following situations:

• DistanceToTargetPoint and DistanceToCurrentWaypoint calculate the distance from Vehicle front to TargetPoint and CurrentWaypoint (green arrow on the image below). As I checked both attributes reference the closest waypoint on a TrafficLane. Calculating the distance in this manner is robust, performing and calculates the distance between the actual position of an NPC and the next waypoint, making it a good solution as is.
  
• checking if NPC yields in cognition step (link) covers checking if NPC went past a yield point (negative distance), which cannot be calculated using FollowingLanes since when NPC enters new lane, the lane gets deleted from the list. In this scenario we do not have access to the previous lanes. Even if we had this would increase computation time to analyze previous lanes and would add significant overhead to the calculation process
• yield distance calculation (link) is calculated a couple of times during simulation with 20 NPCs at a scene. It happens mostly when an NPC is approaching to stop at red light before intersection. This is a similar situation to the one from 1st point of this list (DistanceToTargetPoint and DistanceToCurrentWaypoint) - calculating distance projected on TrafficLane could require much computation time for a small benefit of calculation accuracy
  

[mackierx111]

thank you! LGTM