basic FSM

Assets/CollisionTags.SHConfig

@@ -1,13 +1,13 @@
-0 StaticObject 1100000000000000
+0 StaticObject 1101000000000000
-1 Player 1100000000000000
+1 Player 1101000000000000
 2 Food 1000000000000000
 3 Breakable 1100000000000000
 4 ScoringWallCollider 0110000000000000
 5 Homeowner 1100000000000000
 6 Camera 0010000000000000
-7 StaticWithCameraCollision 1110000000000000
+7 StaticWithCameraCollision 1111000000000000
-8 9 0000000000000000
+8 Floor 1100000000000000
-9 10 0000000000000000
+9 Navigation 0001000000000000
 10 11 0000000000000000
 11 12 0000000000000000
 12 13 0000000000000000

Assets/Scenes/Level2_AITest.shade.shmeta

@@ -0,0 +1,3 @@
+Name: Level2_AITest
+ID: 86300248
+Type: 5

Assets/Scripts/Gameplay/AIBehaviour/AIRework/AILineOfSight.cs

@@ -0,0 +1,107 @@
+using SHADE;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework
+{
+    public class AILineOfSight:Script
+    {
+        public GameObject player;
+
+        public float range = 2.0f;
+        [Tooltip("Angle between player and forward to be within sight")]
+        public float angle = 30.0f;
+
+        [Tooltip("For debug")]
+        public float angleBetween = 0.0f;
+
+        [Tooltip("For debug")]
+        public float distance = 0.0f;
+
+        public float heightLimit = 1.0f;
+
+        public Vector3 rayOffset;
+
+        
+        public bool withinRange;
+        public bool withinSight;
+
+
+        public Vector3 lastFoundPos;
+        public float lastFoundTimer = 0.0f;
+
+        protected override void update()
+        {
+            if (player == GameObject.Null)
+                return;
+
+            Transform transform = GetComponent<Transform>();
+            Transform playerTransform = player.GetComponent<Transform>();
+            Collider playerCollider = player.GetComponent<Collider>();
+
+            withinRange = false;
+            withinSight = false;
+            
+
+            if(transform && playerTransform && playerCollider)
+            {
+                Vector3 pos = transform.GlobalPosition + rayOffset;
+                Vector3 playerPos = playerTransform.GlobalPosition ;
+                Vector3 d = playerPos - pos;
+                distance = d.GetMagnitude();
+                if(distance < range)
+                {
+                    Vector3 fwdHorizontal = transform.Forward;
+                    fwdHorizontal.y = 0;
+                    fwdHorizontal.Normalise();
+
+                    Vector3 dHorizontal = d;
+                    dHorizontal.y = 0;
+                    
+
+                    float dot = Vector3.Dot(fwdHorizontal, dHorizontal);
+                    angleBetween = SHADE.Math.Rad2Deg * MathF.Acos(dot / (dHorizontal.GetMagnitude()));
+                    if (angleBetween < angle && playerPos.y < pos.y + heightLimit)
+                    {
+                        withinRange = true;
+                        withinSight = true;
+                        Ray sightRay = new Ray(pos, d.GetNormalised());
+                        List<RaycastHit> hitResults = Physics.Raycast(sightRay, distance,false, (ushort)65535);
+
+                        foreach(RaycastHit hit in hitResults)
+                        {
+                            if (hit.Hit && hit.Other != player)
+                            {
+                                Debug.Log("AI LOS: HIT OTHER");
+                                withinSight = false;
+                                break;
+                            }
+                        }
+                    }
+                }
+
+                if (withinSight == true)
+                {
+                    lastFoundPos = playerTransform.GlobalPosition;
+                    lastFoundTimer = 0.0f;
+                }
+                else
+                {
+                    lastFoundTimer += Time.DeltaTimeF;
+                }
+
+            }
+
+
+            
+
+
+        }
+
+
+
+    }
+}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/AILineOfSight.cs.shmeta

@@ -0,0 +1,3 @@
+Name: AILineOfSight
+ID: 152074687
+Type: 9

Assets/Scripts/Gameplay/AIBehaviour/AIRework/BaseState.cs

@@ -1,38 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
-using SHADE;
-
-namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework
-{
-    public abstract class BaseState
-    {
-        protected string stateName = "Base State";
-        protected StateMachine machine;
-
-        protected BaseState(StateMachine stateMachine)
-        {
-            machine = stateMachine;
-        }
-
-        public virtual void OnEnter()
-        {
-
-        }
-
-        public abstract void Update();
-
-        public virtual void OnExit()
-        {
-
-        }
-
-
-        public string GetStateName()
-        {
-            return stateName;
-        }
-    }
-}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/HomeOwnerAI.cs

@@ -1,17 +1,97 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
+using System.Net;
+using System.Reflection.PortableExecutable;
 using System.Text;
 using System.Threading.Tasks;
 using SHADE;
+using SHADE_Scripting.Gameplay.AIBehaviour.AIRework.States;
 
 namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework
 {
     public class HomeOwnerAI:Script
     {
+        public float idleDuration = 1.0f;
+        public float timeoutDuration = 2.0f;
+        public GameObject patrolPointParent;
+
+        public float patrolSpeed = 2.0f;
+        public float chaseSpeed = 3.0f;
+
+        public float alertCooldown = 0.0f;
+
+        public GameObject player;
 
 
 
+        [NonSerialized]
+        public IEnumerable<Transform> patrolPointPool;
+
+
+        protected override void awake()
+        {
+            StateMachine machine = GetScript<StateMachine>();
+            if(machine)
+            {
+                Dictionary<Type, BaseState> dictionary = new Dictionary<Type, BaseState>();
+                dictionary.Add(typeof(IdleState), new IdleState(machine));
+                dictionary.Add(typeof(PatrolState), new PatrolState(machine));
+                dictionary.Add(typeof(TimeoutState), new TimeoutState(machine));
+                dictionary.Add(typeof(ChaseState), new ChaseState(machine));
+                dictionary.Add(typeof(AlertState), new AlertState(machine));
+
+                machine.InitStateMachine(dictionary);
+            }
+
+            patrolPointPool = patrolPointParent.GetComponentsInChildren<Transform>();
+
+
+        }
+
+
+        protected override void update()
+        {
+            if(alertCooldown > 0.0f)
+            {
+                alertCooldown -= Time.DeltaTimeF;
+            }
+            else
+            {
+                alertCooldown = 0.0f;
+            }
+        }
+
+        public bool ShouldTransitAlert()
+        {
+            AILineOfSight los = GetScript<AILineOfSight>();
+            if (los)
+            {
+                if (los.withinSight && alertCooldown <= 0.0f)
+                {
+                    return true;
+                }
+            }
+            return false;
+        }
+
+        public void RotateToPlayer()
+        {
+
+            Transform playerTransform = player.GetComponent<Transform>();
+
+            //Rotate to face player.
+            Transform aiTransform = GetComponent<Transform>();
+            if(playerTransform && aiTransform)
+            {
+                Vector3 direction = playerTransform.GlobalPosition - aiTransform.GlobalPosition;
+                Quaternion currentRotation = aiTransform.LocalRotation;
+                Quaternion targetRotation = Quaternion.Euler(0.0f, MathF.Atan2(direction.x, direction.z), 0.0f);
+                aiTransform.LocalRotation = Quaternion.Slerp(currentRotation, targetRotation, 5.0f * (float)Time.FixedDeltaTime);
+            }
+            
+        }
+
 
     }
 }

Assets/Scripts/Gameplay/AIBehaviour/AIRework/NavigationTestScript.cs

@@ -44,7 +44,7 @@ namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework
             if (nav && transform)
             {
                 transform.LocalPosition = transform.LocalPosition + ( nav.GetForward() * Time.DeltaTimeF * speed);
-
+                
             }
         }
 

Assets/Scripts/Gameplay/AIBehaviour/AIRework/RotateToVelocity.cs

@@ -0,0 +1,40 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using SHADE;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework
+{
+    public class RotateToVelocity : Script
+    {
+        public float rotationPerSecond = 5.0f;
+
+        protected override void update()
+        {
+            RigidBody rigid = GetComponent<RigidBody>();
+            Transform transform = GetComponent<Transform>();
+
+            if(rigid && transform)
+            {
+                Vector3 vel = rigid.LinearVelocity;
+
+                if(vel.GetSqrMagnitude() > 1.0f)
+                {
+                    Quaternion currentRotation = transform.LocalRotation;
+                    Quaternion targetRotation = Quaternion.Euler(0.0f, MathF.Atan2(vel.x, vel.z), 0.0f);
+                    transform.LocalRotation = Quaternion.Slerp(currentRotation, targetRotation, rotationPerSecond * (float)Time.FixedDeltaTime);
+                }
+
+                
+            }
+
+
+
+        }
+
+
+
+    }
+}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/RotateToVelocity.cs.shmeta

@@ -0,0 +1,3 @@
+Name: RotateToVelocity
+ID: 163814533
+Type: 9

Assets/Scripts/Gameplay/AIBehaviour/AIRework/StateMachine.cs

@@ -1,80 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
-using SHADE;
-
-
-namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework
-{
-    public class StateMachine: Script
-    {
-        private Dictionary<Type, BaseState> stateDictionary;
-        public BaseState currentState = null;
-        public string currentStateName;
-        
-        public void InitStateMachine(Dictionary<Type,BaseState> dictionary)
-        {
-            stateDictionary = dictionary;
-            currentState = stateDictionary.First().Value;
-            currentStateName = currentState.GetStateName();
-            currentState.OnEnter();
-        }
-
-        public bool HasState(Type type)
-        {
-            if(!type.IsSubclassOf(typeof(BaseState)))
-            {
-                return false;
-            }
-            else
-            {
-                return stateDictionary.ContainsKey(type);
-            }
-        }
-        public void SetState(Type type)
-        {
-            if (!type.IsSubclassOf(typeof(BaseState)))
-            {
-                return;
-            }
-
-
-            if (stateDictionary.ContainsKey(type))
-            {
-                currentState.OnExit();
-                currentState = stateDictionary[type];
-                currentState.OnEnter();
-            }
-            else
-            {
-                SetState(stateDictionary.First().Key);
-            }
-        }
-
-        public BaseState GetState(Type type)
-        {
-            if (!stateDictionary.ContainsKey(type))
-                return null;
-
-            return stateDictionary[type];
-        }
-
-        protected override void update()
-        {
-            if(currentState != null)
-            {
-                currentStateName = currentState.GetStateName().ToString();
-                currentState.Update();
-            }
-        }
-
-        public bool IsState(Type type)
-        {
-            return (currentState.GetType() == type);
-        }
-
-
-    }
-}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/AIBaseState.cs

@@ -0,0 +1,22 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework.States
+{
+    public abstract class AIBaseState: BaseState
+    {
+
+        protected HomeOwnerAI ai;
+
+        public AIBaseState(StateMachine stateMachine): base(stateMachine, "")
+        {
+            stateName = "AI Base State";
+            ai = stateMachine.GetScript<HomeOwnerAI>();
+        }
+
+
+    }
+}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/AIBaseState.cs.shmeta

@@ -0,0 +1,3 @@
+Name: AIBaseState
+ID: 160233845
+Type: 9

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/AlertState.cs

@@ -0,0 +1,42 @@
+using SHADE;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework.States
+{
+    public class AlertState: AIBaseState
+    {
+        const float alertDuration = 2.0f;
+        float alertTimer = alertDuration;
+
+
+        public AlertState(StateMachine machine): base(machine)
+        {
+            stateName = "Alert";
+        }
+
+        public override void OnEnter()
+        {
+            alertTimer = alertDuration;
+        }
+
+        public override void update()
+        {
+            alertTimer -= Time.DeltaTimeF;
+            if (alertTimer <= 0.0f)
+            {
+                machine.SetState(typeof(ChaseState));
+            }
+        }
+
+
+        public override void fixedUpdate()
+        {
+            
+        }
+
+    }
+}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/AlertState.cs.shmeta

@@ -0,0 +1,3 @@
+Name: AlertState
+ID: 159828775
+Type: 9

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/ChaseState.cs

@@ -0,0 +1,77 @@
+using SHADE;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework.States
+{
+    public class ChaseState :AIBaseState
+    {
+
+        float giveUpDuration = 10.0f;
+        float giveUpTimer = 0.0f;
+        
+
+        public ChaseState(StateMachine machine): base(machine)
+        {
+            stateName = "Chase";
+        }
+
+        public override void OnEnter()
+        {
+            giveUpTimer = giveUpDuration;
+        }
+
+        public override void update()
+        {
+            Navigation nav = machine.GetComponent<Navigation>();
+            AILineOfSight los = ai.GetScript<AILineOfSight>();
+            if(los && nav)
+            {
+                Transform playerTransform = los.player.GetComponent<Transform>();
+                if (los.withinSight)
+                {
+                    nav.MoveTo(playerTransform.GlobalPosition);
+                }
+                else
+                {
+                    nav.MoveTo(los.lastFoundPos);
+                    giveUpTimer -= Time.DeltaTimeF;
+                }
+                if(los.lastFoundTimer>= 10.0f || giveUpTimer <= 0.0f)
+                {
+                    machine.SetState(typeof(TimeoutState));
+                }
+
+                RigidBody rigid = machine.GetComponent<RigidBody>();
+                if(rigid)
+                {
+                    if (los.withinSight)
+                        rigid.LinearVelocity = nav.GetForward() * ai.chaseSpeed;
+                    else
+                        rigid.LinearVelocity = nav.GetForward() * ai.patrolSpeed;
+                }
+
+                if(nav.ReachedTarget())
+                {
+                    giveUpTimer -= Time.DeltaTimeF;
+
+                    ai.RotateToPlayer();
+
+
+                }
+
+            }
+
+        }
+
+        public override void fixedUpdate()
+        {
+            
+        }
+
+
+    }
+}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/ChaseState.cs.shmeta

@@ -0,0 +1,3 @@
+Name: ChaseState
+ID: 166357249
+Type: 9

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/IdleState.cs

@@ -7,18 +7,38 @@ using SHADE;
 
 namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework.States
 {
-    public class IdleState: BaseState
+    public class IdleState: AIBaseState
     {
 
-        float idleDuration = 1.0f;
+        float timer = 0.0f;
 
-        public IdleState(StateMachine machine, float idleDuration): base(machine)
+
+
+        public IdleState(StateMachine machine): base(machine)
         {
-            stateName = "Idle State";
+            stateName = "Idle";
         }
 
 
-        public override void Update()
+        public override void OnEnter()
+        {
+            timer = 0.0f;
+        }
+
+        public override void update()
+        {
+            timer += Time.DeltaTimeF;
+            if(timer >= ai.idleDuration)
+            {
+                machine.SetState(typeof(PatrolState));
+            }
+            if(ai.ShouldTransitAlert())
+            {
+                machine.SetState(typeof(AlertState));
+            }
+        }
+
+        public override void fixedUpdate()
         {
             
         }

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/PatrolState.cs

@@ -0,0 +1,88 @@
+using SHADE;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework.States
+{
+    public class PatrolState: AIBaseState
+    {
+
+        Random rand;
+        Vector3 lastFramePos;
+
+        float stuckTimer ;
+
+
+        public PatrolState(StateMachine machine) : base(machine)
+        {
+            stateName = "Patrol";
+            rand = new Random(); 
+        }
+
+        public override void OnEnter()
+        {
+            Navigation nav = machine.GetComponent<Navigation>();
+            Transform transform = machine.GetComponent<Transform>();
+            if (nav &&  transform)
+            {
+
+                int index = rand.Next(0, ai.patrolPointPool.Count() - 1);
+                Transform dest = ai.patrolPointPool.ElementAt(index);
+                if (dest)
+                {
+                    nav.MoveTo(dest.GlobalPosition);
+                    Debug.Log("Moving to" + dest.GlobalPosition.ToString());
+                }
+
+                lastFramePos = transform.GlobalPosition;
+
+                stuckTimer = 0.0f;
+                
+            }
+        }
+
+        public override void update()
+        {
+            Navigation nav = machine.GetComponent<Navigation>();
+            Transform transform = machine.GetComponent<Transform>();
+            RigidBody rigid = machine.GetComponent<RigidBody>();
+            if(nav && transform && rigid)
+            {
+                rigid.LinearVelocity = nav.GetForward() * ai.patrolSpeed;
+                Vector3 d = lastFramePos - transform.GlobalPosition;
+                if (d.GetSqrMagnitude() < 0.001f)
+                {
+                    stuckTimer += Time.DeltaTimeF;
+                }
+
+
+                if (nav.ReachedTarget())
+                {
+                    machine.SetState(typeof(IdleState));
+                }
+                lastFramePos = transform.GlobalPosition;
+
+            }
+            if(stuckTimer >= 0.5f)
+            {
+                machine.SetState(typeof(IdleState));
+            }
+
+            if (ai.ShouldTransitAlert())
+            {
+                machine.SetState(typeof(AlertState));
+            }
+
+
+        }
+
+        public override void fixedUpdate()
+        {
+        }
+
+
+    }
+}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/PatrolState.cs.shmeta

@@ -0,0 +1,3 @@
+Name: PatrolState
+ID: 155844701
+Type: 9

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/TimeoutState.cs

@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using SHADE;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework.States
+{
+    public class TimeoutState : AIBaseState
+    {
+        float timer = 0.0f;
+        float alertCooldown = 10.0f;
+
+        public TimeoutState(StateMachine machine) : base(machine)
+        {
+            stateName = "Timeout";
+        }
+
+
+        public override void OnEnter()
+        {
+            timer = 0.0f;
+        }
+
+        public override void update()
+        {
+            timer += Time.DeltaTimeF;
+            if (timer >= ai.timeoutDuration)
+            {
+                machine.SetState(typeof(PatrolState));
+            }
+
+        }
+
+        public override void fixedUpdate()
+        {
+
+        }
+
+        public override void OnExit()
+        {
+            ai.alertCooldown = alertCooldown;
+        }
+
+    }
+}

Assets/Scripts/Gameplay/AIBehaviour/AIRework/States/TimeoutState.cs.shmeta

@@ -0,0 +1,3 @@
+Name: TimeoutState
+ID: 167077719
+Type: 9

SHADE_Engine/src/Navigation/SHNavigationComponent.cpp

@@ -8,7 +8,7 @@ namespace SHADE
 
 
   SHNavigationComponent::SHNavigationComponent()
-    :target{ 0.0f }, path{}, threshold{ 0.1f }, recalculatePath{ false }, unreachableTarget{ false }, forward{0.0f}
+    :target{ 0.0f }, path{}, threshold{ 0.1f }, recalculatePath{ false }, unreachableTarget{ false }, forward{0.0f}, tolerance{0}
   {
 
   }
@@ -34,6 +34,11 @@ namespace SHADE
     return unreachableTarget;
   }
 
+  bool SHNavigationComponent::ReachedTarget() const noexcept
+  {
+    return path.empty();
+  }
+
 
   void SHNavigationComponent::SetTarget(SHVec3 value) noexcept
   {
@@ -66,6 +71,7 @@ RTTR_REGISTRATION
     .property("Forward", &SHNavigationComponent::GetForward, &SHNavigationComponent::EmptySetForward)
     .property("Recalculate Path", &SHNavigationComponent::GetRecalculatePath, &SHNavigationComponent::EmptySetBool)
     .property("Unreachable Target", &SHNavigationComponent::GetUnreachableTarget, &SHNavigationComponent::EmptySetBool)
+    .property("Tolerance", &SHNavigationComponent::tolerance)
     .property("Acceptance threshold", &SHNavigationComponent::threshold)
     ;
 

SHADE_Engine/src/Navigation/SHNavigationComponent.h

@@ -61,6 +61,8 @@ namespace SHADE
 
     //The distance threshold that indicates when the entity has reached the navigation grid.
     float threshold;
+    //Number of grid cells surrounding colliding cells to also mark as colliding.
+    uint16_t tolerance;
 
     SHNavigationComponent();
     virtual ~SHNavigationComponent() = default;
@@ -86,7 +88,7 @@ namespace SHADE
 
     bool GetUnreachableTarget() const noexcept;
 
-
+    bool ReachedTarget() const noexcept;
 
     /********************************************************************
      * \brief
@@ -111,6 +113,8 @@ namespace SHADE
 
     void EmptySetBool(bool value) noexcept;
 
+
+
     RTTR_ENABLE()
   protected:
 

SHADE_Engine/src/Navigation/SHNavigationSystem.cpp

@@ -137,7 +137,7 @@ namespace SHADE
 
           SHAABB aabb{ topleft,halfExtents };
 
-          if (physics->TestAABBOverlap(aabb))
+          if (physics->TestAABBOverlap(aabb, (uint16_t)(SHCollisionTagMatrix::GetTag("Navigation")->GetMask())))
           {
             temp |= bitmask;
           }
@@ -169,13 +169,15 @@ namespace SHADE
   void SHNavigationSystem::NavigationSystemGenerateRoutine::Execute(double dt) noexcept
   {
 #ifdef SHEDITOR
+    SHVec3 navigationAreaSize{ 10.5f, 1.0f, 10.5f };
+
     auto editor = SHSystemManager::GetSystem<SHEditor>();
     if (editor->editorState != SHEditor::State::PLAY)
     {
       SHNavigationSystem* system = static_cast<SHNavigationSystem*>(GetSystem());
       if (SHInputManager::GetKeyDown(SHInputManager::SH_KEYCODE::H))
       {
-        system->GenerateNavigationGridData(SHVec3{ 0.0f }, SHVec3{ 25.6f, 1.0f, 25.6f }, 128, 128);
+        system->GenerateNavigationGridData(SHVec3{ 0.0f }, navigationAreaSize, 80, 80);
       }
 
       auto debugDrawSystem = SHSystemManager::GetSystem<SHDebugDrawSystem>();
@@ -183,9 +185,9 @@ namespace SHADE
       {
         SHTransform trans;
         trans.position = SHVec3{ 0.0f };
-        trans.scale = SHVec3{ 25.6f, 1.0f, 25.6f };
+        trans.scale = navigationAreaSize;
         trans.ComputeTRS();
-        debugDrawSystem->DrawWireCube(trans.trs, SHColour::YELLOW, true);
+        debugDrawSystem->DrawWireCube(trans.trs, SHColour::YELLOW, false);
 
 #ifdef DRAW_NAVIGATION_DATA
         for (uint16_t r = 0; r < system->numRows; ++r)
@@ -410,7 +412,7 @@ namespace SHADE
           topNode.g = currNode.g + 10;
           topNode.h = HCostCalculation(topNode.index, endIndex);
           topNode.f = topNode.g + topNode.h;
-          AddNodeToOpenList(topNode, openList, closedList);
+          AddNodeToOpenList(topNode, openList, closedList, comp.tolerance);
 
           //TopLeft
           if (currNode.index.row > 0)
@@ -423,7 +425,7 @@ namespace SHADE
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
             newNode.f = newNode.g + newNode.h;
-            AddNodeToOpenList(newNode, openList, closedList);
+            AddNodeToOpenList(newNode, openList, closedList, comp.tolerance);
           }
 
         }
@@ -437,7 +439,7 @@ namespace SHADE
           btmNode.g = currNode.g + 10;
           btmNode.h = HCostCalculation(btmNode.index, endIndex);
           btmNode.f = btmNode.g + btmNode.h;
-          AddNodeToOpenList(btmNode, openList, closedList);
+          AddNodeToOpenList(btmNode, openList, closedList, comp.tolerance);
 
           //BottomRight
           if (currNode.index.row < numRows - 1)
@@ -450,7 +452,7 @@ namespace SHADE
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
             newNode.f = newNode.g + newNode.h;
-            AddNodeToOpenList(newNode, openList, closedList);
+            AddNodeToOpenList(newNode, openList, closedList, comp.tolerance);
           }
 
         }
@@ -464,7 +466,7 @@ namespace SHADE
           leftNode.g = currNode.g + 10;
           leftNode.h = HCostCalculation(leftNode.index, endIndex);
           leftNode.f = leftNode.g + leftNode.h;
-          AddNodeToOpenList(leftNode, openList, closedList);
+          AddNodeToOpenList(leftNode, openList, closedList, comp.tolerance);
 
 
           //BottomLeft
@@ -478,7 +480,7 @@ namespace SHADE
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
             newNode.f = newNode.g + newNode.h;
-            AddNodeToOpenList(newNode, openList, closedList);
+            AddNodeToOpenList(newNode, openList, closedList, comp.tolerance);
           }
 
 
@@ -493,7 +495,7 @@ namespace SHADE
           rightNode.g = currNode.g + 10;
           rightNode.h = HCostCalculation(rightNode.index, endIndex);
           rightNode.f = rightNode.g + rightNode.h;
-          AddNodeToOpenList(rightNode, openList, closedList);
+          AddNodeToOpenList(rightNode, openList, closedList, comp.tolerance);
 
           //TopRight
           if (currNode.index.column > 0)
@@ -506,7 +508,7 @@ namespace SHADE
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
             newNode.f = newNode.g + newNode.h;
-            AddNodeToOpenList(newNode, openList, closedList);
+            AddNodeToOpenList(newNode, openList, closedList, comp.tolerance);
           }
 
         }
@@ -566,7 +568,7 @@ namespace SHADE
   }//End GeneratePath
 
 
-  bool SHNavigationSystem::AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::map<NavigationGridIndex, NavigationNode>& closedList) noexcept
+  bool SHNavigationSystem::AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::map<NavigationGridIndex, NavigationNode>& closedList, uint16_t tolerance) noexcept
   {
     if (closedList.find(node.index) != closedList.end())
     {
@@ -580,6 +582,58 @@ namespace SHADE
       return false;
     }
 
+    //Check that node surrounding this is also movable. (Controlled by tolerance).
+    for (uint16_t i = 0; i < tolerance; ++i)
+    {
+      NavigationGridIndex tolCheckIndex = node.index;
+      tolCheckIndex.row += i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+      tolCheckIndex = node.index;
+      tolCheckIndex.row -= i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+      tolCheckIndex = node.index;
+      tolCheckIndex.column += i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+      tolCheckIndex = node.index;
+      tolCheckIndex.column -= i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+      tolCheckIndex = node.index;
+      tolCheckIndex.row += i;
+      tolCheckIndex.column += i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+      tolCheckIndex = node.index;
+      tolCheckIndex.row -= i;
+      tolCheckIndex.column -= i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+      tolCheckIndex = node.index;
+      tolCheckIndex.row -= i;
+      tolCheckIndex.column += i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+      tolCheckIndex = node.index;
+      tolCheckIndex.row += i;
+      tolCheckIndex.column -= i;
+      if (GetNavigationData(tolCheckIndex) == true)
+        return false;
+
+
+    }
+
+
+
     //Check if node exist in open list already
     for (auto& n : openList)
     {

SHADE_Engine/src/Navigation/SHNavigationSystem.h

@@ -50,7 +50,7 @@ namespace SHADE
 
     
     uint32_t HCostCalculation(NavigationGridIndex first, NavigationGridIndex second) noexcept; //TO DO
-    bool AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::map<NavigationGridIndex, NavigationNode>& closedList) noexcept;
+    bool AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::map<NavigationGridIndex, NavigationNode>& closedList, uint16_t tolerance) noexcept;
     void UpdateNavigationComponent(SHNavigationComponent& comp) noexcept;
     SHVec3 GetGridWorldPos(NavigationGridIndex index) noexcept;
 

SHADE_Engine/src/Scene/SHSceneGraph.cpp

@@ -316,7 +316,7 @@ namespace SHADE
     // Error handling
     if (!SHEntityManager::IsValidEID(entityID))
     {
-      SHLOG_ERROR("Entity {} is invalid!", entityID)
+      //SHLOG_ERROR("Entity {} is invalid!", entityID)
       return;
     }
 

SHADE_Managed/src/Components/Navigation.cxx

@@ -54,5 +54,10 @@ namespace SHADE
 
 	}
 
+	bool Navigation::ReachedTarget()
+	{
+		return GetNativeComponent()->ReachedTarget();
+	}
+
 
 }

SHADE_Managed/src/Components/Navigation.hxx

@@ -27,9 +27,10 @@ namespace SHADE
 
 		Vector3 GetForward();
 		bool GetUnreachableTarget();
-		
+		bool ReachedTarget();
-		void MoveTo(Vector3 val);
 
+		void MoveTo(Vector3 val);
+		
 
 	};
 }