Merge pull request #337 from tier4/fix/RJD-1050-fix-performance-npc-v…

…ehicle fix(NPCVehicleCognitionStep): add isCloseEachOther check
tier4 · Aug 22, 2024 · b02ce2a · b02ce2a
2 parents f01e671 + 39ca328
commit b02ce2a
Showing 1 changed file with 23 additions and 0 deletions.
diff --git 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;