Navigation Fix

Assets/Scenes/NavigationTest.shade

@@ -4,21 +4,10 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0, y: 0, z: 0}
+      Translate: {x: 0, y: 10.2736483, z: 0}
-      Rotate: {x: 0, y: 0, z: 0}
+      Rotate: {x: -1.48352981, y: 1.5, z: 0.5}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
-    Camera Component:
-      Position: {x: 0, y: 0, z: 0}
-      Pitch: 0
-      Yaw: 0
-      Roll: 0
-      Width: 1920
-      Near: 0.00999999978
-      Far: 10000
-      Perspective: true
-      FOV: 90
-      IsActive: true
   Scripts: ~
 - EID: 1
   Name: Floor
@@ -56,9 +45,9 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0, y: 0, z: 0}
+      Translate: {x: 11, y: 0, z: 0}
-      Rotate: {x: -0, y: 0, z: -0}
+      Rotate: {x: -0, y: 1.57079637, z: -0}
-      Scale: {x: 1, y: 1, z: 1}
+      Scale: {x: 10.6225176, y: 1, z: 1}
       IsActive: true
     Renderable Component:
       Mesh: 142652392
@@ -77,3 +66,255 @@
           Rotation Offset: {x: 0, y: 0, z: 0}
       IsActive: true
   Scripts: ~
+- EID: 4
+  Name: Wall
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 0, z: -11.2958097}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 10.6225176, y: 1, z: 1}
+      IsActive: true
+    Renderable Component:
+      Mesh: 142652392
+      Material: 126223465
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 2.5, y: 1, z: 1.60000002}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 5
+  Name: Wall
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 0, z: 11}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 10.6225176, y: 1, z: 1}
+      IsActive: true
+    Renderable Component:
+      Mesh: 142652392
+      Material: 126223465
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 2.5, y: 1, z: 1.60000002}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 6
+  Name: Wall
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 6.06437206, y: 0, z: -5.60716629}
+      Rotate: {x: -0, y: 1.57079601, z: -0}
+      Scale: {x: 4.99989605, y: 1, z: 0.999986947}
+      IsActive: true
+    Renderable Component:
+      Mesh: 142652392
+      Material: 126223465
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 2.5, y: 1, z: 1.60000002}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 7
+  Name: Wall
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -11, y: 0, z: 0}
+      Rotate: {x: -0, y: 1.57079637, z: -0}
+      Scale: {x: 10.6225176, y: 1, z: 1}
+      IsActive: true
+    Renderable Component:
+      Mesh: 142652392
+      Material: 126223465
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 2.5, y: 1, z: 1.60000002}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 8
+  Name: Wall
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -4.70212746, y: 0, z: -0.00226712227}
+      Rotate: {x: -0, y: 1.57079601, z: -0}
+      Scale: {x: 4.99982977, y: 1, z: 0.999978602}
+      IsActive: true
+    Renderable Component:
+      Mesh: 142652392
+      Material: 126223465
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 2.5, y: 1, z: 1.60000002}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 9
+  Name: Wall
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 1.46194017, y: 0, z: 0.71579361}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 4.99985838, y: 1, z: 0.999982238}
+      IsActive: true
+    Renderable Component:
+      Mesh: 142652392
+      Material: 126223465
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 2.5, y: 1, z: 1.60000002}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 10
+  Name: Wall
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -2.30867815, y: 0, z: -6.02849674}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 4, y: 1, z: 0.999974966}
+      IsActive: true
+    Renderable Component:
+      Mesh: 142652392
+      Material: 126223465
+      IsActive: true
+    Collider Component:
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 0
+          Type: Box
+          Half Extents: {x: 2.5, y: 1, z: 1.60000002}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts: ~
+- EID: 11
+  Name: TestAI
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 8.5, y: -1, z: -8.20610714}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 5, y: 5, z: 5}
+      IsActive: true
+    Renderable Component:
+      Mesh: 140639624
+      Material: 131956078
+      IsActive: true
+    Navigation Component:
+      Target: {x: 0, y: 0, z: 0}
+      Forward: {x: 0, y: 0, z: 0}
+      Recalculate Path: true
+      Unreachable Target: false
+      Acceptance threshold: 0.100000001
+      IsActive: true
+  Scripts:
+    - Type: SHADE_Scripting.Gameplay.AIBehaviour.AIRework.NavigationTestScript
+      Enabled: true
+      endPoint: 12
+      speed: 3
+- EID: 12
+  Name: EndPoint
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -0.940230131, y: -1, z: -2.05140448}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 5, y: 5, z: 5}
+      IsActive: true
+    Renderable Component:
+      Mesh: 140639624
+      Material: 131956078
+      IsActive: true
+  Scripts: ~
+- EID: 13
+  Name: Camera
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 10.2736483, z: 0}
+      Rotate: {x: -1.48352981, y: 0, z: 0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    Camera Component:
+      Position: {x: 0, y: 0, z: 0}
+      Pitch: -85
+      Yaw: 0
+      Roll: 0
+      Width: 1920
+      Near: 0.00999999978
+      Far: 10000
+      Perspective: true
+      FOV: 90
+      IsActive: true
+  Scripts: ~

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

@@ -0,0 +1,37 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using SHADE;
+using static System.IO.Enumeration.FileSystemEnumerable<TResult>;
+
+namespace SHADE_Scripting.Gameplay.AIBehaviour.AIRework
+{
+    public class NavigationTestScript :Script
+    {
+        public GameObject endPoint;
+
+        public float speed = 1.0f;
+
+        protected override void start()
+        {
+            
+        }
+
+        protected override void update()
+        {
+            Navigation nav = GetComponent<Navigation>();
+            Transform transform = GetComponent<Transform>();
+            if (nav && transform)
+            {
+                Transform endTransform = endPoint.GetComponent<Transform>();
+                if(endTransform)
+                    nav.MoveTo(endTransform.GlobalPosition);
+                transform.LocalPosition = transform.LocalPosition + ( nav.GetForward() * Time.DeltaTimeF * speed);
+
+            }
+        }
+
+    }
+}

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

@@ -0,0 +1,3 @@
+Name: NavigationTestScript
+ID: 162476480
+Type: 9

SHADE_Application/src/Application/SBApplication.cpp

@@ -131,6 +131,7 @@ namespace Sandbox
     SHSystemManager::RegisterRoutine<SHPhysicsDebugDrawSystem, SHPhysicsDebugDrawSystem::PhysicsDebugDraw>();
     SHSystemManager::RegisterRoutine<SHNavigationSystem, SHNavigationSystem::NavigationSystemGenerateRoutine>();
 #endif
+    SHSystemManager::RegisterRoutine<SHNavigationSystem, SHNavigationSystem::UpdateNavigationRoutine>();
 
     SHSystemManager::RegisterRoutine<SHTransformSystem, SHTransformSystem::TransformPostPhysicsUpdate>();
     SHSystemManager::RegisterRoutine<SHDebugDrawSystem, SHDebugDrawSystem::ProcessPointsRoutine>();

SHADE_Engine/src/Common/SHAllComponents.h

@@ -16,3 +16,4 @@
 #include "AudioSystem/SHAudioListenerComponent.h"
 #include "Graphics/MiddleEnd/TrajectoryRendering/SHTrajectoryRenderableComponent.h"
 #include "Graphics/MiddleEnd/Particles/SHParticleEmitterComponent.h"
+#include "Navigation/SHNavigationComponent.h"

SHADE_Engine/src/Editor/EditorWindow/Inspector/SHEditorInspector.cpp

@@ -27,6 +27,7 @@
 #include "AudioSystem/SHAudioListenerComponent.h"
 #include "Graphics/MiddleEnd/TextRendering/SHTextRenderableComponent.h"
 #include "Graphics/MiddleEnd/Particles/SHParticleEmitterComponent.h"
+#include "Navigation/SHNavigationComponent.h"
 #include "Camera/SHCameraSystem.h"
 #include "FRC/SHFramerateController.h"
 
@@ -193,6 +194,10 @@ namespace SHADE
         {
           DrawComponent(particleComponent);
         }
+        if (auto navigationComponent = SHComponentManager::GetComponent_s<SHNavigationComponent>(eid))
+        {
+          DrawComponent(navigationComponent);
+        }
         ImGui::Separator();
         // Render Scripts
         SHScriptEngine* scriptEngine = static_cast<SHScriptEngine*>(SHSystemManager::GetSystem<SHScriptEngine>());
@@ -219,6 +224,7 @@ namespace SHADE
           DrawAddComponentWithEnforcedComponentButton<SHTextRenderableComponent, SHTransformComponent>(eid);
           DrawAddComponentWithEnforcedComponentButton<SHAnimatorComponent, SHTransformComponent, SHRenderable>(eid);
           DrawAddComponentWithEnforcedComponentButton<SHAudioListenerComponent, SHTransformComponent>(eid);
+          DrawAddComponentWithEnforcedComponentButton<SHNavigationComponent, SHTransformComponent>(eid);
           //DrawAddComponentWithEnforcedComponentButton<SHParticleEmitterComponent, SHTransformComponent>(eid);
 
           ImGui::EndMenu();

SHADE_Engine/src/Navigation/SHNavigationComponent.cpp

@@ -4,8 +4,11 @@
 
 namespace SHADE
 {
+  uint16_t NavigationGridIndex::numColumns = 8;
+
+
   SHNavigationComponent::SHNavigationComponent()
-    :target{ 0.0f }, path{}, threshold{0.1f}, recalculatePath{false}, unreachableTarget{false}
+    :target{ 0.0f }, path{}, threshold{ 0.1f }, recalculatePath{ false }, unreachableTarget{ false }, forward{0.0f}
   {
 
   }
@@ -21,6 +24,16 @@ namespace SHADE
     return forward;
   }
 
+  bool SHNavigationComponent::GetRecalculatePath() const noexcept
+  {
+    return recalculatePath;
+  }
+
+  bool SHNavigationComponent::GetUnreachableTarget() const noexcept
+  {
+    return unreachableTarget;
+  }
+
 
   void SHNavigationComponent::SetTarget(SHVec3 value) noexcept
   {
@@ -34,5 +47,26 @@ namespace SHADE
 
   }
 
+  void SHNavigationComponent::EmptySetBool(bool value) noexcept
+  {
+
+  }
+
+
+}//namespace SHADE
+
+
+RTTR_REGISTRATION
+{
+  using namespace SHADE;
+  using namespace rttr;
+
+  registration::class_<SHNavigationComponent>("Navigation Component")
+    .property("Target", &SHNavigationComponent::GetTarget, &SHNavigationComponent::SetTarget)
+    .property("Forward", &SHNavigationComponent::GetForward, &SHNavigationComponent::EmptySetForward)
+    .property("Recalculate Path", &SHNavigationComponent::GetRecalculatePath, &SHNavigationComponent::EmptySetBool)
+    .property("Unreachable Target", &SHNavigationComponent::GetUnreachableTarget, &SHNavigationComponent::EmptySetBool)
+    .property("Acceptance threshold", &SHNavigationComponent::threshold)
+    ;
 
 }

SHADE_Engine/src/Navigation/SHNavigationComponent.h

@@ -14,7 +14,32 @@
 
 namespace SHADE
 {
-  using NavigationGridIndex = std::pair<uint16_t, uint16_t>;
+  struct NavigationGridIndex
+  {
+    uint16_t row;
+    uint16_t column;
+
+    static uint16_t numColumns;
+
+    bool operator==(NavigationGridIndex const& rhs) const noexcept
+    {
+      return row == rhs.row && column == rhs.column;
+    }
+
+    bool operator!=(NavigationGridIndex const& rhs) const noexcept
+    {
+      return row != rhs.row || column != rhs.column;
+    }
+
+    bool operator<(NavigationGridIndex const& rhs) const noexcept
+    {
+      return (row * numColumns + column) < (rhs.row * numColumns + rhs.column);
+    }
+
+  };
+
+
+
   #define NullGridIndex 9999;
 
   class SH_API SHNavigationComponent final: public SHComponent
@@ -26,8 +51,7 @@ namespace SHADE
     SHVec3 forward;
     //The path to follow to reach the target.
     std::queue<NavigationGridIndex> path;
-    //The distance threshold that indicates when the entity has reached the navigation grid.
+
-    float threshold;
     //The flag to check against to indicate whether to recalculate path
     bool recalculatePath;
     //The flag to set if the target is unreachable.
@@ -35,6 +59,9 @@ namespace SHADE
   public:
     friend class SHNavigationSystem;
 
+    //The distance threshold that indicates when the entity has reached the navigation grid.
+    float threshold;
+
     SHNavigationComponent();
     virtual ~SHNavigationComponent() = default;
 
@@ -55,6 +82,11 @@ namespace SHADE
     ********************************************************************/
     SHVec3 GetForward() const noexcept;
 
+    bool GetRecalculatePath() const noexcept;
+
+    bool GetUnreachableTarget() const noexcept;
+
+
 
     /********************************************************************
      * \brief
@@ -77,6 +109,7 @@ namespace SHADE
     ********************************************************************/
     void EmptySetForward(SHVec3 value) noexcept;
 
+    void EmptySetBool(bool value) noexcept;
 
     RTTR_ENABLE()
   protected:

SHADE_Engine/src/Navigation/SHNavigationSystem.cpp

@@ -5,16 +5,25 @@
 #include "Math/Geometry/SHAABB.h"
 #include "Input/SHInputManager.h"
 #include "Editor/SHEditor.h"
+#include "Scene/SHSceneManager.h"
+#include "Graphics/MiddleEnd/Interface/SHDebugDrawSystem.h"
 
 
+#include <vector>
 #include <stack>
-#include <unordered_map>
+#include <map>
 #include <list>
 
+#define DRAW_NAVIGATION_DATA
+#define DRAW_NAVIGATION_PATH
+
+
 namespace SHADE
 {
   void SHNavigationSystem::Init()
   {
+    auto id = ComponentFamily::GetID<SHNavigationComponent>();
+    SHComponentManager::CreateComponentSparseSet<SHNavigationComponent>();
     SystemID i = SystemFamily::GetID<SHNavigationSystem>(); 
   }
 
@@ -26,8 +35,8 @@ namespace SHADE
 
   uint16_t SHNavigationSystem::GetIndex(uint16_t row, uint16_t col) noexcept
   {
-    size_t rowOffset = numRows / sizeof(GridDataType);
+    size_t rowOffset = numRows / NumGridDataTypeBits;
-    return row * rowOffset + (col / sizeof(GridDataType));
+    return row * rowOffset + (col / NumGridDataTypeBits );
   }
 
 
@@ -39,7 +48,7 @@ namespace SHADE
     
     GridDataType bitMask = 1 << (col % NumGridDataTypeBits);
 
-    if (navigationGrid.size() < index)
+    if ( index < navigationGrid.size())
     {
       return navigationGrid[index] & bitMask;
     }
@@ -51,7 +60,7 @@ namespace SHADE
 
   bool SHNavigationSystem::GetNavigationData(NavigationGridIndex index) noexcept
   {
-    return GetNavigationData(index.first, index.second);
+    return GetNavigationData(index.row, index.column);
   }
 
 
@@ -63,14 +72,30 @@ namespace SHADE
   NavigationGridIndex SHNavigationSystem::GetNavigationGridIndex(SHVec3 const& worldPosition) noexcept
   {
     NavigationGridIndex result;
-    SHVec3 pos = worldPosition - origin;
+    SHVec3 topleft{ origin.x - (size.x / 2.0f), origin.y, origin.z - (size.z / 2.0f) };
+    SHVec3 pos = worldPosition - topleft;
     SHVec2 gridSize = GetGridSize();
-    result.first = pos.z / gridSize.x;
+    
-    result.second = pos.x / gridSize.y;
+    result.row = (pos.z )/ gridSize.y;
+    result.column = (pos.x )/ gridSize.x;
 
     return result;
   }
 
+  SHVec3 SHNavigationSystem::GetGridWorldPos(NavigationGridIndex index) noexcept
+  {
+    SHVec3 result{0.0f};
+    SHVec3 topleft{ origin.x - (size.x / 2.0f), origin.y, origin.z - (size.z / 2.0f) };
+    SHVec2 gridSize = GetGridSize();
+    result.x = (index.column * gridSize.x);
+    result.z = (index.row * gridSize.y) ;
+
+    result += topleft;
+
+    return result;
+  }
+
+
 
   void SHNavigationSystem::GenerateNavigationGridData(SHVec3 origin, SHVec3 size, uint16_t nr, uint16_t nc) noexcept
   {
@@ -150,12 +175,150 @@ namespace SHADE
       SHNavigationSystem* system = static_cast<SHNavigationSystem*>(GetSystem());
       if (SHInputManager::GetKeyDown(SHInputManager::SH_KEYCODE::H))
       {
-        system->GenerateNavigationGridData(SHVec3{ 0.0f }, SHVec3{ 25.6f, 10.0f, 25.6f }, 128, 128);
+        system->GenerateNavigationGridData(SHVec3{ 0.0f }, SHVec3{ 25.6f, 1.0f, 25.6f }, 128, 128);
+      }
+
+      auto debugDrawSystem = SHSystemManager::GetSystem<SHDebugDrawSystem>();
+      if (debugDrawSystem)
+      {
+        SHTransform trans;
+        trans.position = SHVec3{ 0.0f };
+        trans.scale = SHVec3{ 25.6f, 1.0f, 25.6f };
+        trans.ComputeTRS();
+        debugDrawSystem->DrawWireCube(trans.trs, SHColour::YELLOW, true);
+
+#ifdef DRAW_NAVIGATION_DATA
+        for (uint16_t r = 0; r < system->numRows; ++r)
+        {
+          for (uint16_t c = 0; c < system->numCols; ++c)
+          {
+            if (system->GetNavigationData(r, c) == true)
+            {
+              SHVec3 topleft{ system->origin.x - (system->size.x / 2.0f), system->origin.y, system->origin.z - (system->size.z / 2.0f) };
+              SHVec2 halfGridSize = system->GetGridSize() * 0.5f;
+
+              //offset it by row and column and center it with half grid size.
+              topleft += SHVec3{ c * system->GetGridSize().x, 0.0f, r * system->GetGridSize().y } + SHVec3{ halfGridSize.x,0.0f,halfGridSize.y };
+              SHTransform t;
+              t.position = system->GetGridWorldPos({r,c});
+              t.scale = SHVec3{ halfGridSize.x * 2.0f, 1.0f, halfGridSize.y * 2.0f};
+              t.ComputeTRS();
+              debugDrawSystem->DrawCube(t.trs, SHColour::RED, true);
+
+            }
+            else
+            {
+              SHVec3 topleft{ system->origin.x - (system->size.x / 2.0f), system->origin.y, system->origin.z - (system->size.z / 2.0f) };
+              SHVec2 halfGridSize = system->GetGridSize() * 0.5f;
+
+              //offset it by row and column and center it with half grid size.
+              topleft += SHVec3{ c * system->GetGridSize().x, 0.0f, r * system->GetGridSize().y } + SHVec3{ halfGridSize.x,0.0f,halfGridSize.y };
+              SHTransform t;
+              t.position = system->GetGridWorldPos({ r,c });
+              t.scale = SHVec3{ halfGridSize.x * 2.0f, 1.0f, halfGridSize.y * 2.0f };
+              t.ComputeTRS();
+              debugDrawSystem->DrawCube(t.trs, SHColour::WHITE, true);
+            }
+
+
           }
         }
 #endif
+
+
       }
 
+    }
+#endif
+  }
+
+  void SHNavigationSystem::UpdateNavigationRoutine::Execute(double dt) noexcept
+  {
+    SHNavigationSystem* system = static_cast<SHNavigationSystem*>(GetSystem());
+    auto& dense = SHComponentManager::GetDense<SHNavigationComponent>();
+
+    for (auto& comp : dense)
+    {
+      if (SHSceneManager::CheckNodeAndComponentsActive<SHNavigationComponent>(comp.GetEID()))
+      {
+        if (comp.recalculatePath)
+          system->GeneratePath(comp);
+
+        system->UpdateNavigationComponent(comp);
+      }
+    }
+
+  }
+
+
+  void SHNavigationSystem::UpdateNavigationComponent(SHNavigationComponent& comp) noexcept
+  {
+
+    if (comp.unreachableTarget == true || comp.path.empty())
+    {
+      comp.forward = SHVec3::Zero;
+      return; // no point continuing because target can't be reached / path is empty.
+    }
+      
+
+    SHTransformComponent* transform = SHComponentManager::GetComponent_s<SHTransformComponent>(comp.GetEID());
+    if (transform)
+    {
+      NavigationGridIndex nxtPoint =  comp.path.front();
+      SHVec3 nxtPointPos = GetGridWorldPos(nxtPoint);
+
+      SHVec3 direction = nxtPointPos - transform->GetWorldPosition();
+      direction.y = 0.0f;
+
+      
+
+      if (direction.LengthSquared() <= comp.threshold * comp.threshold || direction.LengthSquared() <= GetGridSize().LengthSquared() * 0.5f)
+      {
+        comp.path.pop();
+        //Rerun this function
+        UpdateNavigationComponent(comp);
+        //return so we don't run the parts after this twice.
+        return;
+      }
+
+
+      comp.forward = SHVec3::Normalise(direction);
+
+#ifdef DRAW_NAVIGATION_PATH
+
+      auto debugDrawSystem = SHSystemManager::GetSystem<SHDebugDrawSystem>();
+      
+      auto queue = comp.path;
+
+      while (!queue.empty())
+      {
+        uint16_t r = queue.front().row;
+        uint16_t c = queue.front().column;
+        
+        {
+          SHVec3 topleft{ origin.x - (size.x / 2.0f), origin.y, origin.z - (size.z / 2.0f) };
+          SHVec2 halfGridSize = GetGridSize() * 0.5f;
+
+          //offset it by row and column and center it with half grid size.
+          topleft += SHVec3{ c * GetGridSize().x, 0.0f, r * GetGridSize().y } + SHVec3{ halfGridSize.x,0.0f,halfGridSize.y };
+          SHTransform t;
+          t.position = GetGridWorldPos({ r,c });
+          t.scale = SHVec3{ halfGridSize.x * 2.0f, 1.0f, halfGridSize.y * 2.0f };
+          t.ComputeTRS();
+          debugDrawSystem->DrawCube(t.trs, SHColour::YELLOW, true);
+
+        }
+        queue.pop();
+      }
+
+#endif
+
+    }
+
+
+  }
+
+
 
   void SHNavigationSystem::GeneratePath(SHNavigationComponent& comp) noexcept
   {
@@ -167,7 +330,10 @@ namespace SHADE
       return;
     }
     std::list<NavigationNode> openList;
-    std::unordered_map<NavigationGridIndex, NavigationNode> closedList;
+    std::map<NavigationGridIndex, NavigationNode> closedList;
+    closedList.clear();
+
+    NavigationGridIndex::numColumns = numCols;
 
     //Check if ending position is set to true in navigation data.
     NavigationGridIndex endIndex = GetNavigationGridIndex(comp.target);
@@ -184,8 +350,8 @@ namespace SHADE
     NavigationNode startingNode;
     startingNode.index = GetNavigationGridIndex(transform->GetWorldPosition());
 
-    startingNode.parent.first = NullGridIndex;
+    startingNode.parent.row = NullGridIndex;
-    startingNode.parent.second = NullGridIndex;
+    startingNode.parent.column = NullGridIndex;
     startingNode.h = 0;
     startingNode.g = 0;
     startingNode.f = 0;
@@ -194,8 +360,8 @@ namespace SHADE
 
 
     NavigationNode endNode;
-    endNode.index.first = NullGridIndex;
+    endNode.index.row = NullGridIndex;
-    endNode.index.second = NullGridIndex;
+    endNode.index.column = NullGridIndex;
     endNode.g = std::numeric_limits<uint32_t>::max();
     endNode.h = std::numeric_limits<uint32_t>::max();
     endNode.f = std::numeric_limits<uint32_t>::max();
@@ -235,11 +401,11 @@ namespace SHADE
       //Add the surrounding 8 tiles into the open list
       {
         //Top
-        if (currNode.index.second > 0)
+        if (currNode.index.column > 0)
         {
           NavigationNode topNode;
           topNode.index = currNode.index;
-          topNode.index.second -= 1;
+          topNode.index.column -= 1;
           topNode.parent = currNode.index;
           topNode.g = currNode.g + 10;
           topNode.h = HCostCalculation(topNode.index, endIndex);
@@ -247,12 +413,12 @@ namespace SHADE
           AddNodeToOpenList(topNode, openList, closedList);
 
           //TopLeft
-          if (currNode.index.first > 0)
+          if (currNode.index.row > 0)
           {
             NavigationNode newNode;
             newNode.index = currNode.index;
-            newNode.index.second -= 1;
+            newNode.index.column -= 1;
-            newNode.index.first -= 1;
+            newNode.index.row -= 1;
             newNode.parent = currNode.index;
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
@@ -262,11 +428,11 @@ namespace SHADE
 
         }
         //Bottom
-        if (currNode.index.second < numRows - 1)
+        if (currNode.index.column < numCols - 1)
         {
           NavigationNode btmNode;
           btmNode.index = currNode.index;
-          btmNode.index.second += 1;
+          btmNode.index.column += 1;
           btmNode.parent = currNode.index;
           btmNode.g = currNode.g + 10;
           btmNode.h = HCostCalculation(btmNode.index, endIndex);
@@ -274,12 +440,12 @@ namespace SHADE
           AddNodeToOpenList(btmNode, openList, closedList);
 
           //BottomRight
-          if (currNode.index.first < numCols - 1)
+          if (currNode.index.row < numRows - 1)
           {
             NavigationNode newNode;
             newNode.index = currNode.index;
-            newNode.index.second += 1;
+            newNode.index.column += 1;
-            newNode.index.first += 1;
+            newNode.index.row += 1;
             newNode.parent = currNode.index;
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
@@ -289,11 +455,11 @@ namespace SHADE
 
         }
         //Left
-        if (currNode.index.first > 0)
+        if (currNode.index.row > 0)
         {
           NavigationNode leftNode;
           leftNode.index = currNode.index;
-          leftNode.index.first -= 1;
+          leftNode.index.row -= 1;
           leftNode.parent = currNode.index;
           leftNode.g = currNode.g + 10;
           leftNode.h = HCostCalculation(leftNode.index, endIndex);
@@ -302,12 +468,12 @@ namespace SHADE
 
 
           //BottomLeft
-          if (currNode.index.second < numRows - 1)
+          if (currNode.index.column < numCols - 1)
           {
             NavigationNode newNode;
             newNode.index = currNode.index;
-            newNode.index.second += 1;
+            newNode.index.column += 1;
-            newNode.index.first -= 1;
+            newNode.index.row -= 1;
             newNode.parent = currNode.index;
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
@@ -318,11 +484,11 @@ namespace SHADE
 
         }
         //Right
-        if (currNode.index.first < numCols - 1)
+        if (currNode.index.row < numRows - 1)
         {
           NavigationNode rightNode;
           rightNode.index = currNode.index;
-          rightNode.index.first += 1;
+          rightNode.index.row += 1;
           rightNode.parent = currNode.index;
           rightNode.g = currNode.g + 10;
           rightNode.h = HCostCalculation(rightNode.index, endIndex);
@@ -330,12 +496,12 @@ namespace SHADE
           AddNodeToOpenList(rightNode, openList, closedList);
 
           //TopRight
-          if (currNode.index.second > 0)
+          if (currNode.index.column > 0)
           {
             NavigationNode newNode;
             newNode.index = currNode.index;
-            newNode.index.second -= 1;
+            newNode.index.column -= 1;
-            newNode.index.first += 1;
+            newNode.index.row += 1;
             newNode.parent = currNode.index;
             newNode.g = currNode.g + 14;
             newNode.h = HCostCalculation(newNode.index, endIndex);
@@ -345,10 +511,12 @@ namespace SHADE
 
         }
       }
+      
 }
     //Check if there is a path.
     if (endNode.g == std::numeric_limits<uint32_t>::max() || endNode.h == std::numeric_limits<uint32_t>::max() || endNode.f == std::numeric_limits<uint32_t>::max())
     {
+      SHLOG_WARNING("Navigation System: End Node not found after running through algo EID: {}", comp.GetEID())
       comp.unreachableTarget = true;
       return;
     }
@@ -382,16 +550,12 @@ namespace SHADE
       comp.path.push(reversePath.top());
       reversePath.pop();
     }
-    
+    comp.recalculatePath = false;
-
+    comp.unreachableTarget = false;
-    
-
-
-
   }//End GeneratePath
 
 
-  bool SHNavigationSystem::AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::unordered_map<NavigationGridIndex, NavigationNode>& closedList) noexcept
+  bool SHNavigationSystem::AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::map<NavigationGridIndex, NavigationNode>& closedList) noexcept
   {
     if (closedList.find(node.index) != closedList.end())
     {
@@ -423,4 +587,20 @@ namespace SHADE
   }
 
 
+  uint32_t SHNavigationSystem::HCostCalculation(NavigationGridIndex first, NavigationGridIndex second) noexcept
+  {
+    uint16_t rDiff = (first.row > second.row)?first.row - second.row: second.row - first.row;
+    uint16_t cDiff = (first.column > second.column) ? first.column - second.column : second.column - first.column;
+
+    if (rDiff > cDiff)
+    {
+      return (cDiff * 14) + ((rDiff - cDiff) * 10);
+    }
+    else
+    {
+      return (rDiff * 14 )+ ((cDiff - rDiff) * 10);
+    }
+  }
+
+
 }

SHADE_Engine/src/Navigation/SHNavigationSystem.h

@@ -6,14 +6,15 @@
 #include "Math/Vector/SHVec2.h"
 #include "Math/Vector/SHVec3.h"
 
-#include <vector>
+
 #include "SH_API.h"
 
 
 namespace SHADE
 {
   
-  struct NavigationNode
+
+  struct SH_API NavigationNode
   {
     NavigationGridIndex index;
     NavigationGridIndex parent;
@@ -22,6 +23,7 @@ namespace SHADE
     uint32_t f;
   };
 
+
   class SH_API SHNavigationSystem final: public SHSystem
   {
   private:
@@ -46,21 +48,22 @@ namespace SHADE
     SHVec2 GetGridSize() noexcept;
     uint16_t GetIndex(uint16_t row, uint16_t col) noexcept;
 
-    void GeneratePath(SHNavigationComponent& comp) noexcept;
-    uint32_t HCostCalculation(NavigationGridIndex first, NavigationGridIndex second) noexcept; //TO DO
-    bool AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::unordered_map<NavigationGridIndex, NavigationNode>& closedList) noexcept;
     
+    uint32_t HCostCalculation(NavigationGridIndex first, NavigationGridIndex second) noexcept; //TO DO
+    bool AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::map<NavigationGridIndex, NavigationNode>& closedList) noexcept;
+    void UpdateNavigationComponent(SHNavigationComponent& comp) noexcept;
+    SHVec3 GetGridWorldPos(NavigationGridIndex index) noexcept;
 
   public:
     SHNavigationSystem() = default;
     virtual ~SHNavigationSystem() = default;
 
-    void Init();
+    virtual void Init();
-    void Exit();
+    virtual void Exit();
+
     
-    void SaveNavigationData() noexcept;
     void GenerateNavigationGridData(SHVec3 origin, SHVec3 size, uint16_t numRow, uint16_t numCol) noexcept;
-
+    void GeneratePath(SHNavigationComponent& comp) noexcept;
     
     bool GetNavigationData(uint16_t row, uint16_t col) noexcept;
     bool GetNavigationData(NavigationGridIndex index) noexcept;
@@ -78,6 +81,15 @@ namespace SHADE
     };
     friend class NavigationSystemGenerateRoutine;
 
+    class SH_API UpdateNavigationRoutine final: public SHSystemRoutine
+    {
+    public:
+      UpdateNavigationRoutine() : SHSystemRoutine("Update Navigation Routine", false) {};
+      virtual void Execute(double dt)noexcept override final;
+    };
+    friend class UpdateNavigationRoutine;
+
+
   };
 
 

SHADE_Engine/src/Serialization/SHSerialization.cpp

@@ -243,6 +243,8 @@ namespace SHADE
     AddComponentToComponentNode<SHToggleButtonComponent>(components, eid);
     AddComponentToComponentNode<SHSliderComponent>(components, eid);
 
+    AddComponentToComponentNode<SHNavigationComponent>(components, eid);
+
     AddComponentToComponentNode<SHTextRenderableComponent>(components, eid);
     AddComponentToComponentNode<SHAnimatorComponent>(components, eid);
     AddComponentToComponentNode<SHUIComponent>(components, eid);
@@ -306,6 +308,9 @@ namespace SHADE
     AddComponentID<SHButtonComponent>(componentIDList, componentsNode);
     AddComponentID<SHToggleButtonComponent>(componentIDList, componentsNode);
     AddComponentID<SHSliderComponent>(componentIDList, componentsNode);
+
+    AddComponentID<SHNavigationComponent>(componentIDList, componentsNode);
+
     AddComponentID<SHTextRenderableComponent>(componentIDList, componentsNode);
     AddComponentID<SHAnimatorComponent>(componentIDList, componentsNode);
     AddComponentID<SHUIComponent>(componentIDList, componentsNode);
@@ -394,6 +399,9 @@ namespace SHADE
     SHSerializationHelper::InitializeComponentFromNode<SHButtonComponent>(componentsNode, eid);
     SHSerializationHelper::InitializeComponentFromNode<SHToggleButtonComponent>(componentsNode, eid);
     SHSerializationHelper::InitializeComponentFromNode<SHSliderComponent>(componentsNode, eid);
+
+    SHSerializationHelper::InitializeComponentFromNode<SHNavigationComponent>(componentsNode, eid);
+
     SHSerializationHelper::InitializeComponentFromNode<SHTextRenderableComponent>(componentsNode, eid);
     SHSerializationHelper::InitializeComponentFromNode<SHLightComponent>(componentsNode, eid);
     SHSerializationHelper::InitializeComponentFromNode<SHAnimatorComponent>(componentsNode, eid);

SHADE_Managed/src/Components/Navigation.cxx

@@ -0,0 +1,58 @@
+#include "SHpch.h"
+
+#include "Navigation.hxx"
+#include "ECS_Base/Managers/SHSystemManager.h"
+#include "Navigation/SHNavigationSystem.h"
+
+namespace SHADE
+{
+	Navigation::Navigation(Entity entity)
+		:Component(entity)
+	{
+
+	}
+
+	float Navigation::Threshold::get()
+	{
+		return GetNativeComponent()->threshold;
+	}
+
+	void Navigation::Threshold::set(float val)
+	{
+		GetNativeComponent()->threshold = val;
+	}
+
+	Vector3 Navigation::Target::get()
+	{
+		return Convert::ToCLI(GetNativeComponent()->GetTarget());
+	}
+
+	void Navigation::Target::set(Vector3 val)
+	{
+		GetNativeComponent()->SetTarget(Convert::ToNative(val));
+	}
+
+
+	Vector3 Navigation::GetForward()
+	{
+		return Convert::ToCLI(GetNativeComponent()->GetForward());
+	}
+
+	bool Navigation::GetUnreachableTarget()
+	{
+		return GetNativeComponent()->GetUnreachableTarget();
+	}
+
+	void Navigation::MoveTo(Vector3 val)
+	{
+		GetNativeComponent()->SetTarget(Convert::ToNative(val));
+		auto system = SHSystemManager::GetSystem<SHNavigationSystem>();
+		if (system)
+		{
+			system->GeneratePath(*GetNativeComponent());
+		}
+
+	}
+
+
+}

SHADE_Managed/src/Components/Navigation.hxx

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "Components/Component.hxx"
+#include "Math/Vector3.hxx"
+
+//External Dependencies
+#include "Navigation/SHNavigationComponent.h"
+
+namespace SHADE
+{
+	public ref class Navigation : public Component<SHNavigationComponent>
+	{
+	internal:
+		Navigation(Entity entity);
+
+	public:
+		property float Threshold
+		{
+			float get();
+			void set(float val);
+		}
+		property Vector3 Target
+		{
+			Vector3 get();
+			void set(Vector3 val);
+		}
+
+		Vector3 GetForward();
+		bool GetUnreachableTarget();
+		
+		void MoveTo(Vector3 val);
+
+
+	};
+}

SHADE_Managed/src/Engine/ECS.cxx

@@ -52,7 +52,7 @@ of DigiPen Institute of Technology is prohibited.
 #include "Components\TrajectoryRenderable.hxx"
 #include "Components\Animator.hxx"
 #include "Components\ParticleEmitter.hxx"
-
+#include "Components\Navigation.hxx"
 
 
 namespace SHADE
@@ -344,6 +344,7 @@ namespace SHADE
         componentMap.Add(createComponentSet<SHTrajectoryRenderableComponent, TrajectoryRenderable>());
         componentMap.Add(createComponentSet<SHAnimatorComponent, Animator>());
         componentMap.Add(createComponentSet<SHParticleEmitterComponent, ParticleEmitter>());
+        componentMap.Add(createComponentSet<SHNavigationComponent, Navigation>());
     }
     
     /*---------------------------------------------------------------------------------*/