WIP

SHADE_Engine/src/Navigation/SHNavigationComponent.cpp

@@ -0,0 +1,38 @@
+#include "SHpch.h"
+#include "SHNavigationComponent.h"
+
+
+namespace SHADE
+{
+  SHNavigationComponent::SHNavigationComponent()
+    :target{ 0.0f }, path{}, threshold{0.1f}, recalculatePath{false}, unreachableTarget{false}
+  {
+
+  }
+
+
+  SHVec3 SHNavigationComponent::GetTarget() const noexcept
+  {
+    return target;
+  }
+
+  SHVec3 SHNavigationComponent::GetForward() const noexcept
+  {
+    return forward;
+  }
+
+
+  void SHNavigationComponent::SetTarget(SHVec3 value) noexcept
+  {
+    recalculatePath = true;
+    target = value;
+  }
+
+
+  void SHNavigationComponent::EmptySetForward(SHVec3 value) noexcept
+  {
+
+  }
+
+
+}

SHADE_Engine/src/Navigation/SHNavigationComponent.h

@@ -0,0 +1,86 @@
+#pragma once
+
+#include <rttr/registration>
+
+#include "ECS_Base/Components/SHComponent.h"
+#include "Math/Vector/SHVec2.h"
+#include "Math/Vector/SHVec3.h"
+
+#include <queue>
+
+
+#include "SH_API.h"
+
+
+namespace SHADE
+{
+  using NavigationGridIndex = std::pair<uint16_t, uint16_t>;
+  #define NullGridIndex 9999;
+
+  class SH_API SHNavigationComponent final: public SHComponent
+  {
+  private:
+    //Target to move to
+    SHVec3 target;
+    //Direction to mvoe towards to get to the target position.
+    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.
+    bool unreachableTarget;
+  public:
+    friend class SHNavigationSystem;
+
+    SHNavigationComponent();
+    virtual ~SHNavigationComponent() = default;
+
+
+    /********************************************************************
+     * \brief
+     *  Getter for the target position. This should be in world position.
+     * \return 
+     * SHVec3
+    ********************************************************************/
+    SHVec3 GetTarget() const noexcept;
+    /********************************************************************
+     * \brief
+     *  Getter for the forward vector. This shows the direction to move
+     * in order to reach the target. 
+     * \return 
+     * SHVec3
+    ********************************************************************/
+    SHVec3 GetForward() const noexcept;
+
+
+    /********************************************************************
+     * \brief
+     *  Setter for the target variable. Also sets recalculatePath boolean
+     * to true.
+     * \param value
+     * The target world location. The y-axis will be ignored.
+     * \return 
+     * void
+    ********************************************************************/
+    void SetTarget(SHVec3 value) noexcept;
+    /********************************************************************
+     * \brief
+     *  An empty setter so we can see this variable in inspector but 
+     * can't set it. 
+     * \param value
+     * value to set to.
+     * \return 
+     * void
+    ********************************************************************/
+    void EmptySetForward(SHVec3 value) noexcept;
+
+
+    RTTR_ENABLE()
+  protected:
+
+
+  };
+}

SHADE_Engine/src/Navigation/SHNavigationSystem.cpp

@@ -4,6 +4,12 @@
 #include "Physics/System/SHPhysicsSystem.h"
 #include "Math/Geometry/SHAABB.h"
 #include "Input/SHInputManager.h"
+#include "Editor/SHEditor.h"
+
+
+#include <stack>
+#include <unordered_map>
+#include <list>
 
 namespace SHADE
 {
@@ -26,33 +32,49 @@ namespace SHADE
 
 
 
-  bool SHNavigationSystem::GetCollisionData(uint16_t row, uint16_t col) noexcept
+  bool SHNavigationSystem::GetNavigationData(uint16_t row, uint16_t col) noexcept
   {
     
     size_t index = GetIndex(row, col);
     
     GridDataType bitMask = 1 << (col % NumGridDataTypeBits);
 
-    if (collisionGrid.size() < index)
+    if (navigationGrid.size() < index)
     {
-      return collisionGrid[index] & bitMask;
+      return navigationGrid[index] & bitMask;
     }
     else
     {
-      return 0;
+      return true;
     }
-
-
   }
 
+  bool SHNavigationSystem::GetNavigationData(NavigationGridIndex index) noexcept
+  {
+    return GetNavigationData(index.first, index.second);
+  }
+
+
   SHVec2 SHNavigationSystem::GetGridSize() noexcept
   {
-    return SHVec2{size.x / numRows, size.y / numCols};
+    return SHVec2{size.x / numCols, size.z / numRows};
   }
 
-  void SHNavigationSystem::GenerateCollisionGridData(SHVec3 origin, SHVec2 size, uint16_t nr, uint16_t nc, float gridHeight) noexcept
+  NavigationGridIndex SHNavigationSystem::GetNavigationGridIndex(SHVec3 const& worldPosition) noexcept
   {
-    if (size.x <= 0.0f || size.y <= 0.0f || nr == 0 || nc == 0 || gridHeight <= 0.0f)
+    NavigationGridIndex result;
+    SHVec3 pos = worldPosition - origin;
+    SHVec2 gridSize = GetGridSize();
+    result.first = pos.z / gridSize.x;
+    result.second = pos.x / gridSize.y;
+
+    return result;
+  }
+
+
+  void SHNavigationSystem::GenerateNavigationGridData(SHVec3 origin, SHVec3 size, uint16_t nr, uint16_t nc) noexcept
+  {
+    if (size.x <= 0.0f || size.y <= 0.0f || size.z <= 0.0f || nr == 0 || nc == 0 )
     {
       return;
     }
@@ -62,7 +84,7 @@ namespace SHADE
     this->size = size;
     numRows = nr;
     numCols = nc;
-    collisionGrid.clear();
+    navigationGrid.clear();
 
 
     GridDataType temp = 0;
@@ -78,7 +100,7 @@ namespace SHADE
         if (physics != nullptr)
         {
           //Get the top left position
-          SHVec3 topleft{ origin.x - (size.x / 2.0f), origin.y, origin.z - (size.y / 2.0f) };
+          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.
@@ -86,7 +108,7 @@ namespace SHADE
 
           //Get half extents.
           
-          SHVec3 halfExtents{ halfGridSize.x, gridHeight, halfGridSize.y };
+          SHVec3 halfExtents{ halfGridSize.x, size.y, halfGridSize.y };
 
           SHAABB aabb{ topleft,halfExtents };
 
@@ -99,7 +121,7 @@ namespace SHADE
         
         if ((c % NumGridDataTypeBits) == NumGridDataTypeBits - 1)
         {
-          collisionGrid.push_back(temp);
+          navigationGrid.push_back(temp);
           temp = 0;
         }
 
@@ -107,7 +129,7 @@ namespace SHADE
       //if the number of column dont fit perfectly to the NumGridDataTypeBits. We push in what we have with the rest as 0s.
       if (numCols % NumGridDataTypeBits != 0)
       {
-        collisionGrid.push_back(temp);
+        navigationGrid.push_back(temp);
         temp = 0;
       }
 
@@ -121,11 +143,283 @@ namespace SHADE
 
   void SHNavigationSystem::NavigationSystemGenerateRoutine::Execute(double dt) noexcept
   {
-    SHNavigationSystem* system = static_cast<SHNavigationSystem*>(GetSystem());
+#ifdef SHEDITOR
-    if (SHInputManager::GetKeyDown(SHInputManager::SH_KEYCODE::H))
+    auto editor = SHSystemManager::GetSystem<SHEditor>();
+    if (editor->editorState != SHEditor::State::PLAY)
     {
-      system->GenerateCollisionGridData(SHVec3{ 0.0f }, SHVec2{ 25.6f }, 128, 128, 10.0f);
+      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);
+      }
     }
+#endif
+  }
+
+
+  void SHNavigationSystem::GeneratePath(SHNavigationComponent& comp) noexcept
+  {
+
+    //Check that the entity has a transform.
+    if (SHComponentManager::HasComponent<SHTransformComponent>(comp.GetEID()) == false)
+    {
+      comp.unreachableTarget = true;
+      return;
+    }
+    std::list<NavigationNode> openList;
+    std::unordered_map<NavigationGridIndex, NavigationNode> closedList;
+
+    //Check if ending position is set to true in navigation data.
+    NavigationGridIndex endIndex = GetNavigationGridIndex(comp.target);
+    if (GetNavigationData(endIndex) == true)
+    {
+      //Target position is unreachable.
+      SHLOG_WARNING("Navigation System: GeneratePath() target position is unreachable EID: {}", comp.GetEID())
+      comp.unreachableTarget = true;
+      return;
+    }
+
+    auto transform = SHComponentManager::GetComponent<SHTransformComponent>(comp.GetEID());
+    
+    NavigationNode startingNode;
+    startingNode.index = GetNavigationGridIndex(transform->GetWorldPosition());
+
+    startingNode.parent.first = NullGridIndex;
+    startingNode.parent.second = NullGridIndex;
+    startingNode.h = 0;
+    startingNode.g = 0;
+    startingNode.f = 0;
+
+    openList.push_back(startingNode);
+
+
+    NavigationNode endNode;
+    endNode.index.first = NullGridIndex;
+    endNode.index.second = 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();
+
+    while (!openList.empty())
+    {
+      NavigationNode currNode;
+      std::list<NavigationNode>::iterator lowestNode = openList.end();
+      uint32_t lowestFValue = std::numeric_limits<uint32_t>::max();
+      uint32_t lowestHValue = std::numeric_limits<uint32_t>::max();
+      for (std::list<NavigationNode>::iterator node = openList.begin(); node != openList.end(); ++node)
+      {
+        if (node->f < lowestFValue)
+        {
+          lowestNode = node;
+          lowestHValue = node->h;
+          lowestFValue = node->f;
+        }
+        if (node->f == lowestFValue && node->h < lowestHValue)
+        {
+          lowestNode = node;
+          lowestHValue = node->h;
+        }
+
+      }
+      
+      currNode = *lowestNode;
+      openList.erase(lowestNode);
+      closedList.emplace(currNode.index, currNode);
+
+      if (currNode.index == endIndex)
+      {
+        endNode = currNode;
+        break;
+      }
+
+      //Add the surrounding 8 tiles into the open list
+      {
+        //Top
+        if (currNode.index.second > 0)
+        {
+          NavigationNode topNode;
+          topNode.index = currNode.index;
+          topNode.index.second -= 1;
+          topNode.parent = currNode.index;
+          topNode.g = currNode.g + 10;
+          topNode.h = HCostCalculation(topNode.index, endIndex);
+          topNode.f = topNode.g + topNode.h;
+          AddNodeToOpenList(topNode, openList, closedList);
+
+          //TopLeft
+          if (currNode.index.first > 0)
+          {
+            NavigationNode newNode;
+            newNode.index = currNode.index;
+            newNode.index.second -= 1;
+            newNode.index.first -= 1;
+            newNode.parent = currNode.index;
+            newNode.g = currNode.g + 14;
+            newNode.h = HCostCalculation(newNode.index, endIndex);
+            newNode.f = newNode.g + newNode.h;
+            AddNodeToOpenList(newNode, openList, closedList);
+          }
+
+        }
+        //Bottom
+        if (currNode.index.second < numRows - 1)
+        {
+          NavigationNode btmNode;
+          btmNode.index = currNode.index;
+          btmNode.index.second += 1;
+          btmNode.parent = currNode.index;
+          btmNode.g = currNode.g + 10;
+          btmNode.h = HCostCalculation(btmNode.index, endIndex);
+          btmNode.f = btmNode.g + btmNode.h;
+          AddNodeToOpenList(btmNode, openList, closedList);
+
+          //BottomRight
+          if (currNode.index.first < numCols - 1)
+          {
+            NavigationNode newNode;
+            newNode.index = currNode.index;
+            newNode.index.second += 1;
+            newNode.index.first += 1;
+            newNode.parent = currNode.index;
+            newNode.g = currNode.g + 14;
+            newNode.h = HCostCalculation(newNode.index, endIndex);
+            newNode.f = newNode.g + newNode.h;
+            AddNodeToOpenList(newNode, openList, closedList);
+          }
+
+        }
+        //Left
+        if (currNode.index.first > 0)
+        {
+          NavigationNode leftNode;
+          leftNode.index = currNode.index;
+          leftNode.index.first -= 1;
+          leftNode.parent = currNode.index;
+          leftNode.g = currNode.g + 10;
+          leftNode.h = HCostCalculation(leftNode.index, endIndex);
+          leftNode.f = leftNode.g + leftNode.h;
+          AddNodeToOpenList(leftNode, openList, closedList);
+
+
+          //BottomLeft
+          if (currNode.index.second < numRows - 1)
+          {
+            NavigationNode newNode;
+            newNode.index = currNode.index;
+            newNode.index.second += 1;
+            newNode.index.first -= 1;
+            newNode.parent = currNode.index;
+            newNode.g = currNode.g + 14;
+            newNode.h = HCostCalculation(newNode.index, endIndex);
+            newNode.f = newNode.g + newNode.h;
+            AddNodeToOpenList(newNode, openList, closedList);
+          }
+
+
+        }
+        //Right
+        if (currNode.index.first < numCols - 1)
+        {
+          NavigationNode rightNode;
+          rightNode.index = currNode.index;
+          rightNode.index.first += 1;
+          rightNode.parent = currNode.index;
+          rightNode.g = currNode.g + 10;
+          rightNode.h = HCostCalculation(rightNode.index, endIndex);
+          rightNode.f = rightNode.g + rightNode.h;
+          AddNodeToOpenList(rightNode, openList, closedList);
+
+          //TopRight
+          if (currNode.index.second > 0)
+          {
+            NavigationNode newNode;
+            newNode.index = currNode.index;
+            newNode.index.second -= 1;
+            newNode.index.first += 1;
+            newNode.parent = currNode.index;
+            newNode.g = currNode.g + 14;
+            newNode.h = HCostCalculation(newNode.index, endIndex);
+            newNode.f = newNode.g + newNode.h;
+            AddNodeToOpenList(newNode, openList, closedList);
+          }
+
+        }
+      }
+    }
+    //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())
+    {
+      comp.unreachableTarget = true;
+      return;
+    }
+
+
+
+    //Generate the path using end node.
+    std::stack<NavigationGridIndex> reversePath;
+    NavigationNode pathNode = endNode;
+
+    while (pathNode.index != startingNode.index)
+    {
+      reversePath.push(pathNode.index);
+      if (closedList.find(pathNode.parent) == closedList.end())
+      {
+        SHLOG_WARNING("Navigation System: GeneratePath() reverse path parent not in closed list EID: {}", comp.GetEID())
+          comp.unreachableTarget = true;
+          return;
+      }
+      pathNode = closedList.find(pathNode.parent)->second;
+
+    }
+    //Clear the queue.
+    while (!comp.path.empty())
+    {
+      comp.path.pop();
+    }
+
+    while (!reversePath.empty())
+    {
+      comp.path.push(reversePath.top());
+      reversePath.pop();
+    }
+    
+
+    
+
+
+
+  }//End GeneratePath
+
+
+  bool SHNavigationSystem::AddNodeToOpenList(NavigationNode node, std::list<NavigationNode>& openList, std::unordered_map<NavigationGridIndex, NavigationNode>& closedList) noexcept
+  {
+    if (closedList.find(node.index) != closedList.end())
+    {
+      //Node is already in closed list.
+      return false;
+    }
+
+    //Check if the node is pointing to a non movable tile.
+    if (GetNavigationData(node.index) == true)
+    {
+      return false;
+    }
+
+    //Check if node exist in open list already
+    for (auto& n : openList)
+    {
+      if (n.index == node.index )
+      {
+        if (n.f > node.f)
+        {
+          n = node;
+        }        
+        return false;
+      }
+    }
+
+    openList.push_back(node);
+    return true;
   }
 
 

SHADE_Engine/src/Navigation/SHNavigationSystem.h

@@ -2,6 +2,7 @@
 
 #include "ECS_Base/System/SHSystem.h"
 #include "ECS_Base/System/SHSystemRoutine.h"
+#include "SHNavigationComponent.h"
 #include "Math/Vector/SHVec2.h"
 #include "Math/Vector/SHVec3.h"
 
@@ -12,7 +13,14 @@
 namespace SHADE
 {
   
-
+  struct NavigationNode
+  {
+    NavigationGridIndex index;
+    NavigationGridIndex parent;
+    uint32_t g;
+    uint32_t h;
+    uint32_t f;
+  };
 
   class SH_API SHNavigationSystem final: public SHSystem
   {
@@ -22,22 +30,26 @@ namespace SHADE
 
     const size_t NumGridDataTypeBits = sizeof(GridDataType) * CHAR_BIT;
 
-    std::vector<uint8_t> collisionGrid;
+    std::vector<uint8_t> navigationGrid;
 
     //Number of subdivision on the x axis
     uint16_t numRows{0};
     //Number of subdivision on the z axis
     uint16_t numCols{0};
 
-    //The center of the collision area.
+    //The center of the navigation area.
     SHVec3 origin{0.0f};
 
-    //Size of the collision area
+    //Size of the navigation area
-    SHVec2 size{0.0f};
+    SHVec3 size{0.0f};
 
     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;
+
 
   public:
     SHNavigationSystem() = default;
@@ -47,8 +59,15 @@ namespace SHADE
     void Exit();
 
     void SaveNavigationData() noexcept;
-    void GenerateCollisionGridData(SHVec3 origin, SHVec2 size, uint16_t numRow, uint16_t numCol, float gridHeight = 10.0f) noexcept;
+    void GenerateNavigationGridData(SHVec3 origin, SHVec3 size, uint16_t numRow, uint16_t numCol) noexcept;
-    bool GetCollisionData(uint16_t row, uint16_t col) noexcept;
+
+    
+    bool GetNavigationData(uint16_t row, uint16_t col) noexcept;
+    bool GetNavigationData(NavigationGridIndex index) noexcept;
+
+    
+    NavigationGridIndex GetNavigationGridIndex(SHVec3 const& worldPosition) noexcept;
+
 
 
     class SH_API NavigationSystemGenerateRoutine final: public SHSystemRoutine