[Pedestrians] [AUTO-7842][AUTO-7924] Removed NavSystem from pedestria…

…n waypoint behaviour.

Assets/Scripts/Api/Commands/PedestrianFollowWaypoints.cs

@@ -66,7 +66,7 @@ public void Execute(JSONNode args)
                 }
 
                 var waypointBehaviour = ped.SetBehaviour<PedestrianWaypointBehaviour>();
-                waypointBehaviour.FollowWaypoints(wp, loop, waypointsPathType);
+                waypointBehaviour.SetFollowWaypoints(wp, loop, waypointsPathType);
                 api.RegisterAgentWithWaypoints(ped.gameObject);
                 api.SendResult(this);
             }

Assets/Scripts/Api/Waypoint.cs

@@ -81,7 +81,7 @@ Vector3 IWaypoint.Position
             set => Position = value;
         }
 
-        Vector3 IWaypoint.Angle { get; set; }
+        public Vector3 Angle { get; set; }
 
         IWaypoint IWaypoint.Clone()
         {

Assets/Scripts/Controllers/Pedestrians/PedestrianAutomaticBehaviour.cs

@@ -5,14 +5,23 @@
  *
  */
 
+using System.Collections.Generic;
 using Simulator.Map;
+using Simulator.Utilities;
 using UnityEngine;
 using UnityEngine.AI;
 
 public class PedestrianAutomaticBehaviour : PedestrianBehaviourBase
 {
+    public NavMeshObstacle NavMeshObstacle { get; set; }
+    public NavMeshAgent Agent { get; set; }
+    public NavMeshPath Path { get; set; }
+    public Vector3 NextTargetPos { get; set; }
     private Vector3[] Corners = new Vector3[] { };
     private float LinearSpeed = 1.0f;
+    private float AngularSpeed = 10.0f;
+    private Vector3 LastRBPosition;
+    private Quaternion LastRBRotation;
 
     public override void PhysicsUpdate()
     {
@@ -29,17 +38,178 @@ public override void PhysicsUpdate()
         }
 
         EvaluateDistanceFromFocus();
+        PEDTurn();
+        PEDMove();
     }
 
     public override void SetSpeed(float speed)
     {
         LinearSpeed = speed;
     }
 
-    public override void Init(int seed) { }
+    public override void Init(int seed)
+    {
+        Path = new NavMeshPath();
+        Agent = GetComponent<NavMeshAgent>();
+        NavMeshObstacle = GetComponent<NavMeshObstacle>();
+        Agent.avoidancePriority = RandomGenerator.Next(1, 100); // set to 0 for no avoidance
+        Agent.updatePosition = false;
+        Agent.updateRotation = false;
+        Agent.Warp(RB.position);
+        Agent.transform.rotation = Quaternion.identity;
+    }
+
+    public override void InitAPI(PedestrianManager.PedSpawnData data)
+    {
+        Path = new NavMeshPath();
+        Agent = GetComponent<NavMeshAgent>();
+        Agent.avoidancePriority = 0;
+        Agent.updatePosition = false;
+        Agent.updateRotation = false;
+        Agent.Warp(RB.position);
+        Agent.transform.rotation = RB.rotation;
+        GetComponent<NavMeshObstacle>().enabled = false;
+    }
 
     public override void OnAgentCollision(GameObject go) { }
 
+    public override void Reset()
+    {
+        Path.ClearCorners();
+    }
+
+    private void OnCollisionEnter(Collision collision)
+    {
+        if ((LayerMask.GetMask("Agent", "NPC", "Pedestrian", "Obstacle") & 1 << collision.gameObject.layer) != 0)
+        {
+            GetNextPath(false);
+        }
+    }
+
+    public void WalkRandomly()
+    {
+        Agent.avoidancePriority = RandomGenerator.Next(1, 100);
+
+        var position = Agent.transform.position;
+        MapPedestrianLane closest = null;
+        float closestDistance = float.MaxValue;
+        int closestIndex = 0;
+
+        foreach (var path in SimulatorManager.Instance.MapManager.pedestrianLanes)
+        {
+            for (int i = 0; i < path.mapWorldPositions.Count; i++)
+            {
+                float distance = Vector3.SqrMagnitude(position - path.mapWorldPositions[i]);
+                if (distance < closestDistance)
+                {
+                    closest = path;
+                    closestIndex = i;
+                    closestDistance = distance;
+                }
+            }
+        }
+        controller.Targets = closest.mapWorldPositions;
+        controller.MapPath = closest;
+
+        controller.NextTargetIndex = closestIndex;
+    }
+
+    private void PEDTurn()
+    {
+        if (controller.CurrentTurn != Vector3.zero)
+        {
+            RB.MoveRotation(Quaternion.Slerp(RB.rotation, Quaternion.LookRotation(controller.CurrentTurn), AngularSpeed * Time.fixedDeltaTime));
+        }
+        else
+        {
+            RB.angularVelocity = Vector3.zero;
+        }
+
+        var euler1 = LastRBRotation.eulerAngles;
+        var euler2 = RB.rotation.eulerAngles;
+        var diff = euler2 - euler1;
+        for (int i = 0; i < 3; i++)
+        {
+            diff[i] = (diff[i] + 180) % 360 - 180;
+        }
+        controller.CurrentAngularVelocity = diff / Time.fixedDeltaTime * Mathf.Deg2Rad;
+    }
+
+    private void PEDMove()
+    {
+        if (controller.MovementSpeed != 0f)
+        {
+            RB.MovePosition(RB.position + transform.forward * (controller.MovementSpeed * Time.fixedDeltaTime));
+        }
+        else
+        {
+            RB.velocity = Vector3.zero;
+        }
+
+        var previousVelocity = controller.CurrentVelocity;
+        controller.CurrentVelocity = (RB.position - LastRBPosition) / Time.fixedDeltaTime;
+        controller.CurrentAcceleration = controller.CurrentVelocity - previousVelocity;
+        LastRBPosition = RB.position;
+    }
+
+    public bool IsPathReady()
+    {
+        if (Path.corners.Length == 0 || Path.status != NavMeshPathStatus.PathComplete)
+        {
+            return false;
+        }
+        else
+        {
+            return true;
+        }
+    }
+
+    public void GetNextPath(bool GetNextTarget = true)
+    {
+        if (GetNextTarget)
+        {
+            NextTargetPos = GetRandomTargetPosition(controller.NextTargetIndex);
+        }
+        if (NavMeshObstacle != null)
+        {
+            NavMeshObstacle.enabled = false;
+        }
+        Agent.enabled = true;
+        Agent.CalculatePath(NextTargetPos, Path);
+        Agent.enabled = false;
+        if (NavMeshObstacle != null)
+        {
+            NavMeshObstacle.enabled = true;
+        }
+    }
+
+    private Vector3 GetRandomTargetPosition(int index)
+    {
+        Vector3 tempV = Vector3.zero;
+        if (controller.Targets.Count > 0 && index >= 0 && index < controller.Targets.Count)
+        {
+            tempV = controller.Targets[index];
+        }
+
+        int count = 0;
+        bool isInNavMesh = false;
+        while (!isInNavMesh && count < 10000)
+        {
+            Vector3 randomPoint = tempV + RandomGenerator.InsideUnitSphere() * controller.TargetRange;
+            NavMeshHit hit;
+            if (NavMesh.SamplePosition(randomPoint, out hit, 1.0f, NavMesh.AllAreas))
+            {
+                tempV = hit.position;
+                isInNavMesh = true;
+                count = 10000;
+            }
+
+            count++;
+        }
+
+        return tempV;
+    }
+
     private void EvaluateSidewalk()
     {
         if (IsRandomIdle())
@@ -115,12 +285,12 @@ private bool EvaluateOnRoadForward()
 
     private void EvaluateNextTarget()
     {
-        if (!controller.IsPathReady())
+        if (!IsPathReady())
         {
-            controller.GetNextPath();
+            GetNextPath();
         }
 
-        Corners = controller.Path.corners;
+        Corners = Path.corners;
         Vector3 targetPos = RB.position;
         if (controller.CurrentWP < Corners.Length)
         {
@@ -138,7 +308,7 @@ private void EvaluateNextTarget()
             controller.CurrentWP++;
             if (controller.CurrentWP >= Corners.Length)
             {
-                controller.Path.ClearCorners();
+                Path.ClearCorners();
                 controller.CurrentTargetIndex = controller.NextTargetIndex;
                 controller.NextTargetIndex = controller.GetNextTargetIndex(controller.CurrentTargetIndex);
                 controller.CurrentWP = 0;