Merge pull request #373 from tier4/fix/traffic-distance-calculation

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

Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs

@@ -106,18 +106,15 @@ public float DistanceToCurrentWaypoint
             => SignedDistanceToPointOnLane(CurrentWaypoint);
 
         public float DistanceToNextLane
-            => CurrentFollowingLane?.Waypoints?.Any() != true ? float.MaxValue
-            : SignedDistanceToPointOnLane(CurrentFollowingLane.Waypoints.Last());
-
+            => CalculateDistanceToNextLane();
         public float DistanceToIntersection
             => FirstLaneWithIntersection == null ? float.MaxValue
-            : SignedDistanceToPointOnLane(FirstLaneWithIntersection.StopLine?.CenterPoint ?? FirstLaneWithIntersection.Waypoints[0]);
+            : DistanceToClosestTrafficLightLane();
 
         public bool ObstructedByVehicleBehindIntersection => DistanceToIntersection > DistanceToFrontVehicle;
 
         private int routeIndex = 0;
 
-        // TODO: Calculate distance along the lane
         public float SignedDistanceToPointOnLane(Vector3 point)
         {
             var position = FrontCenterPosition;
@@ -130,6 +127,69 @@ public float SignedDistanceToPointOnLane(Vector3 point)
             return hasPassedThePoint ? -distance : distance;
         }
 
+        private float CalculateDistanceToNextLane()
+        {
+            var nextLane = CurrentFollowingLane;
+            if (nextLane == null || nextLane.Waypoints == null || nextLane.Waypoints.Length == 0)
+            {
+                return float.MaxValue;
+            }
+            var vehiclePosition = FrontCenterPosition;
+            RandomTrafficUtils.GetLaneFollowingProgressAndLaneLength(
+                vehiclePosition,
+                nextLane,
+                out var laneFollowingProgress,
+                out var laneLenght);
+            return (1f - laneFollowingProgress) * laneLenght;
+        }
+
+        public float DistanceToClosestTrafficLightLane()
+        {
+            if (TrafficLightLane is null && !FollowingLanes.Contains(TrafficLightLane))
+            {
+                return float.MaxValue;
+            }
+
+            var distance = 0f;
+            var vehiclePosition = FrontCenterPosition;
+            bool startAddingWholeLanesDistance = false;
+
+            for (var i = 0; i < FollowingLanes.Count; i++)
+            {
+                RandomTrafficUtils.GetLaneFollowingProgressAndLaneLength(
+                    vehiclePosition,
+                    FollowingLanes[i],
+                    out var laneFollowingProgress,
+                    out var laneLenght);
+                if (!startAddingWholeLanesDistance)
+                {
+                    //vehicle is before first not-skipped lane
+                    if (laneFollowingProgress <= 0f)
+                    {
+                        distance += laneLenght;
+                        startAddingWholeLanesDistance = true;
+                    }
+                    //vehicle is on lane
+                    else if (laneFollowingProgress < 1f)
+                    {
+                        var progressToLaneEnd = (1 - laneFollowingProgress);
+                        distance += laneLenght * progressToLaneEnd;
+                        startAddingWholeLanesDistance = true;
+                    }
+                }
+                else
+                {
+                    distance += laneLenght;
+                }
+
+                if (FollowingLanes[i] == TrafficLightLane)
+                {
+                    break;
+                }
+            }
+            return distance;
+        }
+
         public TrafficLane CurrentFollowingLane => FollowingLanes.FirstOrDefault();
 
         public TrafficLane FirstLaneWithIntersection => FollowingLanes.FirstOrDefault(lane => lane.intersectionLane == true);

Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs

@@ -91,8 +91,7 @@ private static float CalculateTrafficLightDistance(NPCVehicleInternalState state
             var distanceToStopPointByTrafficLight = float.MaxValue;
             if (state.TrafficLightLane != null)
             {
-                var distanceToStopLine =
-                    state.SignedDistanceToPointOnLane(state.TrafficLightLane.StopLine.CenterPoint);
+                var distanceToStopLine = state.DistanceToClosestTrafficLightLane();
                 switch (state.TrafficLightPassability)
                 {
                     case TrafficLightPassability.GREEN:
@@ -156,4 +155,4 @@ public void ShowGizmos(IReadOnlyList<NPCVehicleInternalState> states)
             }
         }
     }
-}
+}

Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs

@@ -79,5 +79,64 @@ public static T GetRandomElement<T>(IList<T> source)
                 ? source[Random.Range(0, source.Count)]
                 : default;
         }
+
+        /// <summary>
+        /// Calculates where on the lane the position currently is.
+        /// Helps to track progress of lane following functionality.
+        /// </summary>
+        /// <param name="position">position for which the progress is calculated</param>
+        /// <param name="lane">lane on which the progress is calculated</param>
+        /// <param name="progress">Out parameter. Represents distance from LaneStart to position projection on segment, relative to whole lane length [0-1 range]</param>
+        /// <param name="laneLength">Out parameter. Represents</param>
+        /// <returns></returns>
+        public static void GetLaneFollowingProgressAndLaneLength(Vector3 position, TrafficLane lane, out float progress, out float laneLength)
+        {
+            if (lane is null)
+            {
+                progress = -1f;
+                laneLength = -1f;
+                return;
+            }
+
+            float lengthToPointOnLane = 0.0f;
+            laneLength = 0.0f;
+            float eps = 0.01f;
+            for (var i = 0; i < lane.Waypoints.Length - 1; i++)
+            {
+                Vector3 segmentStart = lane.Waypoints[i];
+                Vector3 segmentEnd = lane.Waypoints[i+1];
+                Vector3 pointOnSegment = ClosestPointOnSegment(segmentStart, segmentEnd, position);
+                float distanceFromStart = Vector3.Distance(segmentStart, pointOnSegment);
+                if (distanceFromStart > eps)
+                {
+                    lengthToPointOnLane += distanceFromStart;
+                }
+
+                laneLength += Vector3.Distance(segmentStart, segmentEnd);
+            }
+            progress = lengthToPointOnLane / laneLength;
+        }
+
+        /// <summary>
+        /// Calculates closest point on segment to point of interest.
+        /// If point of interest cannot be projected to segment, returns closest end of segment.
+        /// </summary>
+        /// <param name="segmentStart">segment start position</param>
+        /// <param name="segmentStop">segment end position</param>
+        /// <param name="pointOfInterest">point of interest</param>
+        /// <returns>closest point on segment to point of interest</returns>
+        public static Vector3 ClosestPointOnSegment(Vector3 segmentStart, Vector3 segmentStop, Vector3 pointOfInterest)
+        {
+            Vector3 segmentVector = segmentStop - segmentStart;
+            Vector3 segmentStartToPoiVector = pointOfInterest - segmentStart;
+
+            float segmentMagnitude = segmentVector.sqrMagnitude;
+            float dotProduct = Vector3.Dot(segmentStartToPoiVector, segmentVector);
+            float argument = dotProduct / segmentMagnitude;
+
+            float t = Mathf.Clamp01(argument);
+
+            return segmentStart + t * segmentVector;
+        }
     }
 }