Multiple rotations to GetShortestPath

unity/Assets/Scripts/BaseFPSAgentController.cs

@@ -678,7 +678,7 @@ public Vector3[] getReachablePositions(
                                 );
                             }
 #if UNITY_EDITOR
-                            Debug.DrawLine(p, newPosition, Color.cyan, 100000f);
+                            // Debug.DrawLine(p, newPosition, Color.cyan, 100000f);
 #endif
                         }
                     }
@@ -6329,6 +6329,28 @@ public UnityEngine.AI.NavMeshPath getShortestPath(
             );
         }
 
+        private void getShortestPath(
+            string objectType,
+            string objectId,
+            Vector3 startPosition,
+            IEnumerable<Quaternion> startRotations,
+            float allowedError,
+            IEnumerable<int> navMeshIds = null,
+            bool sampleFromNavmesh = true
+        ) {
+            SimObjPhysics sop = getSimObjectFromTypeOrId(objectType, objectId);
+            var path = GetSimObjectNavMeshTargets(
+                sop,
+                startPosition,
+                startRotations,
+                allowedError,
+                navMeshIds: navMeshIds,
+                sampleFromNavmesh: sampleFromNavmesh
+            );
+            // VisualizePath(startPosition, path);
+            actionFinishedEmit(success: true, actionReturn: path);
+        }
+
         private void getShortestPath(
             string objectType,
             string objectId,
@@ -6371,6 +6393,26 @@ public void GetShortestPath(
             );
         }
 
+        public void GetShortestPath(
+            Vector3 position,
+            List<Vector3> rotations,
+            string objectType = null,
+            string objectId = null,
+            float allowedError = DefaultAllowedErrorInShortestPath,
+            IEnumerable<int> navMeshIds = null,
+            bool sampleFromNavmesh = true
+        ) {
+            getShortestPath(
+                objectType: objectType,
+                objectId: objectId,
+                startPosition: position,
+                startRotations: rotations.Select(r => Quaternion.Euler(r)),
+                allowedError: allowedError,
+                navMeshIds: navMeshIds,
+                sampleFromNavmesh: sampleFromNavmesh
+            );
+        }
+
         // public void GetShortestPathNew(
         //     Vector3 position,
         //     Vector3 rotation,
@@ -6405,7 +6447,7 @@ public void GetShortestPath(
                 objectType,
                 objectId,
                 position,
-                Quaternion.Euler(Vector3.zero),
+                Quaternion.identity,
                 allowedError,
                 navMeshIds,
                 sampleFromNavmesh: sampleFromNavmesh
@@ -6754,21 +6796,27 @@ public bool getReachablePositionToObjectVisible(
                 "Procedural3",
                 "Procedural0"
             );
+
+            var rotations = new List<float> { 0.0f, 90.0f, 180.0f, 270f }; //, 180.0f, 360f};
             int stepsTaken = 0;
             pos = Vector3.negativeInfinity;
             while (pointsQueue.Count != 0) {
                 stepsTaken += 1;
                 Vector3 p = pointsQueue.Dequeue();
+
                 if (!goodPoints.Contains(p)) {
+                    var preRotation = transform.rotation;
+                    // foreach (var rotation in rotations) {
                     goodPoints.Add(p);
                     transform.position = p;
                     var rot = transform.rotation;
+                    // transform.rotation = Quaternion.Euler(new Vector3(0.0f, rotation, 0.0f));
                     // make sure to rotate just the Camera, not the whole agent
                     m_Camera.transform.LookAt(targetSOP.transform, transform.up);
 
                     bool isVisible = IsInteractable(targetSOP);
 
-                    transform.rotation = rot;
+                    // transform.rotation = rot;
 
                     if (isVisible) {
                         pos = p;
@@ -6834,13 +6882,15 @@ public bool getReachablePositionToObjectVisible(
                         if (shouldEnqueue) {
                             pointsQueue.Enqueue(newPosition);
 #if UNITY_EDITOR
-                            Debug.DrawLine(p, newPosition, Color.cyan, 100000f);
+                            Debug.DrawLine(p, newPosition, Color.green, 100000f);
 #endif
 #if UNITY_EDITOR
-                            Debug.DrawLine(p, newPosition, Color.cyan, 10f);
+                            // Debug.DrawLine(p, newPosition, Color.cyan, 10f);
 #endif
                         }
                     }
+                    // }
+                    // transform.rotation = preRotation;
                 }
                 if (stepsTaken > Math.Floor(maxStepCount / (gridSize * gridSize))) {
                     throw new InvalidOperationException(
@@ -6857,6 +6907,43 @@ public bool getReachablePositionToObjectVisible(
             return false;
         }
 
+        private UnityEngine.AI.NavMeshPath GetSimObjectNavMeshTargets(
+            SimObjPhysics targetSOP,
+            Vector3 initialPosition,
+            IEnumerable<Quaternion> initialRotations,
+            float allowedError,
+            bool visualize = false,
+            IEnumerable<int> navMeshIds = null,
+            bool sampleFromNavmesh = true
+        ) {
+            var path = new UnityEngine.AI.NavMeshPath();
+            InvalidOperationException lastException = null;
+            foreach (var rotation in initialRotations) {
+                lastException = null;
+                try {
+                    path = GetSimObjectNavMeshTarget(
+                        targetSOP: targetSOP,
+                        initialPosition: initialPosition,
+                        initialRotation: rotation,
+                        allowedError: allowedError,
+                        visualize: visualize,
+                        navMeshIds: navMeshIds,
+                        sampleFromNavmesh: sampleFromNavmesh
+                    );
+                } catch (InvalidOperationException e) {
+                    lastException = e;
+                }
+
+                if (path.status == NavMeshPathStatus.PathComplete) {
+                    return path;
+                }
+            }
+            if (lastException != null) {
+                throw lastException;
+            }
+            return path;
+        }
+
         private UnityEngine.AI.NavMeshPath GetSimObjectNavMeshTarget(
             SimObjPhysics targetSOP,
             Vector3 initialPosition,