From be69727d09ff9e4ae33aa163ee6794ad7ad40702 Mon Sep 17 00:00:00 2001
From: Dawid Moszynski <dawid.moszynski@robotec.ai>
Date: Thu, 25 Jul 2024 12:25:09 +0200
Subject: [PATCH] fix(NPCVehicleCognitionStep): add isCloseEachOther check

---
 .../Steps/NPCVehicleCognitionStep.cs          | 23 +++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs
index f1b4d043d..e344efdda 100644
--- a/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs
+++ b/Assets/AWSIM/Scripts/RandomTraffic/NPCVehicle/Steps/NPCVehicleCognitionStep.cs
@@ -4,6 +4,7 @@
 using Unity.Jobs;
 using UnityEngine;
 using UnityEngine.Profiling;
+using GeometryUtility = AWSIM.Lanelet.GeometryUtility;
 
 namespace AWSIM.TrafficSimulation
 {
@@ -281,6 +282,8 @@ private struct RightOfWayCheckJob
             public static float maximumOverrunStopPointForLaneRules = 1f;
 
             public static float differenceOrientationDegreesImplyingPerpendicularRoad = 35f;
+            
+            public static float minimumDistanceBetweenNPCs = 70f;
 
             // In
             public Transform EGOTransform;
@@ -402,6 +405,11 @@ public void Execute()
                 }
             }
 
+            static private bool isCloseEachOther(NPCVehicleInternalState refState, NPCVehicleInternalState otherState)
+            {
+                return GeometryUtility.Distance2D(refState.Vehicle.transform.position, otherState.Vehicle.transform.position) < minimumDistanceBetweenNPCs;
+            }
+
             static private bool isYieldingDueToRules(NPCVehicleInternalState refState)
             {
                 return refState.YieldPhase == NPCVehicleYieldPhase.LEFT_HAND_RULE_ENTERING_INTERSECTION ||
@@ -479,6 +487,9 @@ static private bool isLeftHandRuleEnteringIntersection(NPCVehicleInternalState r
             {
                 foreach (var otherState in states)
                 {
+                    if (!isCloseEachOther(refState, otherState))
+                        continue;
+
                     if (!shouldBeConsideredForYielding(refState, otherState))
                         continue;
 
@@ -505,6 +516,9 @@ static private bool isLeftHandRuleOnIntersection(NPCVehicleInternalState refStat
             {
                 foreach (var otherState in states)
                 {
+                    if (!isCloseEachOther(refState, otherState))
+                        continue;
+
                     if (!shouldBeConsideredForYielding(refState, otherState))
                         continue;
 
@@ -539,6 +553,9 @@ static private bool isIntersectionBusy(NPCVehicleInternalState refState, IReadOn
             {
                 foreach (var otherState in states)
                 {
+                    if (!isCloseEachOther(refState, otherState))
+                        continue;
+
                     if (!shouldBeConsideredForYielding(refState, otherState))
                         continue;
 
@@ -572,6 +589,9 @@ static private bool isSomeVehicleForcingPriority(NPCVehicleInternalState refStat
             {
                 foreach (var otherState in states)
                 {
+                    if (!isCloseEachOther(refState, otherState))
+                        continue;
+
                     if (!shouldBeConsideredForYielding(refState, otherState))
                         continue;
 
@@ -630,6 +650,9 @@ static bool IsLaneDominatedByAny(TrafficLane lane, IReadOnlyList<NPCVehicleInter
                     dominatingVehicle = null;
                     foreach (var otherState in states)
                     {
+                        if (!isCloseEachOther(refState, otherState))
+                            continue;
+
                         if (!shouldBeConsideredForYielding(refState, otherState))
                             continue;