From 1576f1ff77fb16e7174fad014e3283718c7a4bb5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 9 Dec 2024 15:09:10 +0100 Subject: [PATCH 1/8] Fix traffic light distance calculation --- .../Steps/NPCVehicleDecisionStep.cs | 94 ++++++++++++++++++- 1 file changed, 91 insertions(+), 3 deletions(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs index 46e989bab..076446099 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs +++ b/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 = DistanceToClosestTrafficLane(state); switch (state.TrafficLightPassability) { case TrafficLightPassability.GREEN: @@ -109,6 +108,95 @@ private static float CalculateTrafficLightDistance(NPCVehicleInternalState state return onlyGreaterThan(distanceToStopPointByTrafficLight, 0); } + private static float DistanceToClosestTrafficLane(NPCVehicleInternalState state) + { + if (state.TrafficLightLane is null && !state.FollowingLanes.Contains(state.TrafficLightLane)) + { + return float.MaxValue; + } + + var distance = 0f; + var vehiclePosition = state.FrontCenterPosition; + bool startAddingWholeLanesDistance = false; + + for (var i = 0; i < state.FollowingLanes.Count; i++) + { + (var laneFollowingProgress, var laneLenght) = + GetLaneFollowingProgressAndLaneLength(vehiclePosition, state.FollowingLanes[i]); + 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 + { + //when distance was calculated once partially/for whole lane it keeps calculating whole lanes + //until meeting traffic light lane + distance += laneLenght; + } + + //if traffic lane, stop computing + if (state.FollowingLanes[i] == state.TrafficLightLane) + { + break; + } + } + return distance; + } + + public static (float, float) GetLaneFollowingProgressAndLaneLength(Vector3 position, TrafficLane lane) + { + if (lane is null) + { + return (-1f, -1f); + } + + float lengthToPointOnLane = 0.0f; + float wholeLaneLength = 0.0f; + float eps = 0.01f; + Vector2 positionXZ = new Vector2(position.x, position.z); + for (var i = 0; i < lane.Waypoints.Length - 1; i++) + { + Vector2 segmentStart = new Vector2(lane.Waypoints[i].x, lane.Waypoints[i].z); + Vector2 segmentEnd = new Vector2(lane.Waypoints[i + 1].x, lane.Waypoints[i + 1].z); + Vector2 pointOnSegment = ClosestPointOnSegment(segmentStart, segmentEnd, positionXZ); + float distanceFromStart = Vector2.Distance(segmentStart, pointOnSegment); + if (distanceFromStart > eps) + { + lengthToPointOnLane += distanceFromStart; + } + + wholeLaneLength += Vector2.Distance(segmentStart, segmentEnd); + } + + return ((lengthToPointOnLane / wholeLaneLength), wholeLaneLength); + } + + public static Vector2 ClosestPointOnSegment(Vector2 segmentStart, Vector2 segmentStop, Vector2 pointOfInterest) + { + Vector2 segmentVector = segmentStop - segmentStart; + Vector2 segmentStartToPoiVector = pointOfInterest - segmentStart; + + float segmentMagnitude = segmentVector.sqrMagnitude; + float dotProduct = Vector2.Dot(segmentStartToPoiVector, segmentVector); + float argument = dotProduct / segmentMagnitude; + + float t = Mathf.Clamp01(argument); + + return segmentStart + t * segmentVector; + } + private static float CalculateYieldingDistance(NPCVehicleInternalState state) { var distanceToStopPointByRightOfWay = float.MaxValue; @@ -156,4 +244,4 @@ public void ShowGizmos(IReadOnlyList states) } } } -} +} \ No newline at end of file From e8bd005547c69f8332e5a11acb899a592ac435fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Thu, 5 Dec 2024 18:25:39 +0100 Subject: [PATCH 2/8] Move GetLaneFollowingProgressAndLaneLength to RandomTrafficUtils --- .../Steps/NPCVehicleDecisionStep.cs | 49 ++------------- .../RandomTraffic/Utils/RandomTrafficUtils.cs | 59 +++++++++++++++++++ 2 files changed, 64 insertions(+), 44 deletions(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs index 076446099..44a700683 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs @@ -121,8 +121,11 @@ private static float DistanceToClosestTrafficLane(NPCVehicleInternalState state) for (var i = 0; i < state.FollowingLanes.Count; i++) { - (var laneFollowingProgress, var laneLenght) = - GetLaneFollowingProgressAndLaneLength(vehiclePosition, state.FollowingLanes[i]); + RandomTrafficUtils.GetLaneFollowingProgressAndLaneLength( + vehiclePosition, + state.FollowingLanes[i], + out var laneFollowingProgress, + out var laneLenght); if (!startAddingWholeLanesDistance) { //vehicle is before first not-skipped lane @@ -155,48 +158,6 @@ private static float DistanceToClosestTrafficLane(NPCVehicleInternalState state) return distance; } - public static (float, float) GetLaneFollowingProgressAndLaneLength(Vector3 position, TrafficLane lane) - { - if (lane is null) - { - return (-1f, -1f); - } - - float lengthToPointOnLane = 0.0f; - float wholeLaneLength = 0.0f; - float eps = 0.01f; - Vector2 positionXZ = new Vector2(position.x, position.z); - for (var i = 0; i < lane.Waypoints.Length - 1; i++) - { - Vector2 segmentStart = new Vector2(lane.Waypoints[i].x, lane.Waypoints[i].z); - Vector2 segmentEnd = new Vector2(lane.Waypoints[i + 1].x, lane.Waypoints[i + 1].z); - Vector2 pointOnSegment = ClosestPointOnSegment(segmentStart, segmentEnd, positionXZ); - float distanceFromStart = Vector2.Distance(segmentStart, pointOnSegment); - if (distanceFromStart > eps) - { - lengthToPointOnLane += distanceFromStart; - } - - wholeLaneLength += Vector2.Distance(segmentStart, segmentEnd); - } - - return ((lengthToPointOnLane / wholeLaneLength), wholeLaneLength); - } - - public static Vector2 ClosestPointOnSegment(Vector2 segmentStart, Vector2 segmentStop, Vector2 pointOfInterest) - { - Vector2 segmentVector = segmentStop - segmentStart; - Vector2 segmentStartToPoiVector = pointOfInterest - segmentStart; - - float segmentMagnitude = segmentVector.sqrMagnitude; - float dotProduct = Vector2.Dot(segmentStartToPoiVector, segmentVector); - float argument = dotProduct / segmentMagnitude; - - float t = Mathf.Clamp01(argument); - - return segmentStart + t * segmentVector; - } - private static float CalculateYieldingDistance(NPCVehicleInternalState state) { var distanceToStopPointByRightOfWay = float.MaxValue; diff --git a/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs b/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs index 627d80d87..354edd29b 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs @@ -79,5 +79,64 @@ public static T GetRandomElement(IList source) ? source[Random.Range(0, source.Count)] : default; } + + /// + /// Calculates where on the lane the position currently is. + /// Helps to track progress of lane following functionality. + /// + /// position for which the progress is calculated + /// lane on which the progress is calculated + /// Out parameter. Represents distance from LaneStart to position projection on segment, relative to whole lane length [0-1 range] + /// Out parameter. Represents + /// + 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; + } + + /// + /// Calculates closest point on segment to point of interest. + /// If point of interest cannot be projected to segment, returns closest end of segment. + /// + /// segment start position + /// segment end position + /// point of interest + /// closest point on segment to point of interest + 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; + } } } From 30dc32bb72e107892f563c842846ccf1dd813ca0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Fri, 6 Dec 2024 13:21:29 +0100 Subject: [PATCH 3/8] Fix Distance to interseciton calculation --- .../NPCVehicle/NPCVehicleInternalState.cs | 52 ++++++++++++++++++- .../Steps/NPCVehicleDecisionStep.cs | 52 +------------------ 2 files changed, 52 insertions(+), 52 deletions(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs index a1c1e2d3a..eb9bce2fc 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs @@ -111,7 +111,7 @@ public float DistanceToNextLane public float DistanceToIntersection => FirstLaneWithIntersection == null ? float.MaxValue - : SignedDistanceToPointOnLane(FirstLaneWithIntersection.StopLine?.CenterPoint ?? FirstLaneWithIntersection.Waypoints[0]); + : DistanceToClosestTrafficLane(); public bool ObstructedByVehicleBehindIntersection => DistanceToIntersection > DistanceToFrontVehicle; @@ -129,6 +129,56 @@ public float SignedDistanceToPointOnLane(Vector3 point) var distance = Vector3.Distance(position, point); return hasPassedThePoint ? -distance : distance; } + + public float DistanceToClosestTrafficLane() + { + 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 + { + //when distance was calculated once partially/for whole lane it keeps calculating whole lanes + //until meeting traffic light lane + distance += laneLenght; + } + + //if traffic lane, stop computing + if (FollowingLanes[i] == TrafficLightLane) + { + break; + } + } + return distance; + } public TrafficLane CurrentFollowingLane => FollowingLanes.FirstOrDefault(); diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs index 44a700683..30019a98f 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs @@ -91,7 +91,7 @@ private static float CalculateTrafficLightDistance(NPCVehicleInternalState state var distanceToStopPointByTrafficLight = float.MaxValue; if (state.TrafficLightLane != null) { - var distanceToStopLine = DistanceToClosestTrafficLane(state); + var distanceToStopLine = state.DistanceToClosestTrafficLane(); switch (state.TrafficLightPassability) { case TrafficLightPassability.GREEN: @@ -108,56 +108,6 @@ private static float CalculateTrafficLightDistance(NPCVehicleInternalState state return onlyGreaterThan(distanceToStopPointByTrafficLight, 0); } - private static float DistanceToClosestTrafficLane(NPCVehicleInternalState state) - { - if (state.TrafficLightLane is null && !state.FollowingLanes.Contains(state.TrafficLightLane)) - { - return float.MaxValue; - } - - var distance = 0f; - var vehiclePosition = state.FrontCenterPosition; - bool startAddingWholeLanesDistance = false; - - for (var i = 0; i < state.FollowingLanes.Count; i++) - { - RandomTrafficUtils.GetLaneFollowingProgressAndLaneLength( - vehiclePosition, - state.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 - { - //when distance was calculated once partially/for whole lane it keeps calculating whole lanes - //until meeting traffic light lane - distance += laneLenght; - } - - //if traffic lane, stop computing - if (state.FollowingLanes[i] == state.TrafficLightLane) - { - break; - } - } - return distance; - } - private static float CalculateYieldingDistance(NPCVehicleInternalState state) { var distanceToStopPointByRightOfWay = float.MaxValue; From 677dd2ccf6ba4d9026c59adf6f0aa7f283c8dbf0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 9 Dec 2024 13:27:51 +0100 Subject: [PATCH 4/8] Implement DistanceToNextLane calculcation along route --- .../NPCVehicle/NPCVehicleInternalState.cs | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs index eb9bce2fc..abb1f87f1 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs @@ -106,9 +106,7 @@ 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 : DistanceToClosestTrafficLane(); @@ -129,6 +127,22 @@ public float SignedDistanceToPointOnLane(Vector3 point) var distance = Vector3.Distance(position, 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 DistanceToClosestTrafficLane() { From 42b83e1483f2cb1128e241e2ea7368fe2e1e0964 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 9 Dec 2024 14:51:26 +0100 Subject: [PATCH 5/8] Delete comment --- .../Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs index abb1f87f1..e467ac6fe 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs @@ -115,7 +115,6 @@ public float DistanceToIntersection private int routeIndex = 0; - // TODO: Calculate distance along the lane public float SignedDistanceToPointOnLane(Vector3 point) { var position = FrontCenterPosition; From b06c3e6fe019bb7e3e0a28c7feb0248309264cf8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 9 Dec 2024 15:19:16 +0100 Subject: [PATCH 6/8] Apply linting --- .../Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs | 2 +- Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs index e467ac6fe..ed9fe6aad 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs @@ -142,7 +142,7 @@ private float CalculateDistanceToNextLane() out var laneLenght); return (1f - laneFollowingProgress) * laneLenght; } - + public float DistanceToClosestTrafficLane() { if (TrafficLightLane is null && !FollowingLanes.Contains(TrafficLightLane)) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs b/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs index 354edd29b..cac2d6b69 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/Utils/RandomTrafficUtils.cs @@ -79,7 +79,7 @@ public static T GetRandomElement(IList source) ? source[Random.Range(0, source.Count)] : default; } - + /// /// Calculates where on the lane the position currently is. /// Helps to track progress of lane following functionality. From 13bb851a17045ce7cc62ba53747997cfbee1b9e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 9 Dec 2024 15:30:41 +0100 Subject: [PATCH 7/8] Fix function naming --- .../RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs index 30019a98f..07516b9c2 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleDecisionStep.cs @@ -91,7 +91,7 @@ private static float CalculateTrafficLightDistance(NPCVehicleInternalState state var distanceToStopPointByTrafficLight = float.MaxValue; if (state.TrafficLightLane != null) { - var distanceToStopLine = state.DistanceToClosestTrafficLane(); + var distanceToStopLine = state.DistanceToClosestTrafficLightLane(); switch (state.TrafficLightPassability) { case TrafficLightPassability.GREEN: From 570bc82fcac3356c69488f9462552dc5822dd665 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 9 Dec 2024 16:07:16 +0100 Subject: [PATCH 8/8] Fix function naming --- .../RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs index ed9fe6aad..91513665c 100644 --- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs +++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/NPCVehicleInternalState.cs @@ -109,7 +109,7 @@ public float DistanceToNextLane => CalculateDistanceToNextLane(); public float DistanceToIntersection => FirstLaneWithIntersection == null ? float.MaxValue - : DistanceToClosestTrafficLane(); + : DistanceToClosestTrafficLightLane(); public bool ObstructedByVehicleBehindIntersection => DistanceToIntersection > DistanceToFrontVehicle; @@ -143,7 +143,7 @@ private float CalculateDistanceToNextLane() return (1f - laneFollowingProgress) * laneLenght; } - public float DistanceToClosestTrafficLane() + public float DistanceToClosestTrafficLightLane() { if (TrafficLightLane is null && !FollowingLanes.Contains(TrafficLightLane)) { @@ -179,12 +179,9 @@ public float DistanceToClosestTrafficLane() } else { - //when distance was calculated once partially/for whole lane it keeps calculating whole lanes - //until meeting traffic light lane distance += laneLenght; } - //if traffic lane, stop computing if (FollowingLanes[i] == TrafficLightLane) { break;