diff --git a/unity/Assets/Scripts/BaseFPSAgentController.cs b/unity/Assets/Scripts/BaseFPSAgentController.cs index e691c093c9..2088eae0b6 100644 --- a/unity/Assets/Scripts/BaseFPSAgentController.cs +++ b/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 startRotations, + float allowedError, + IEnumerable 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 rotations, + string objectType = null, + string objectId = null, + float allowedError = DefaultAllowedErrorInShortestPath, + IEnumerable 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 { 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 initialRotations, + float allowedError, + bool visualize = false, + IEnumerable 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,