Added Navigation System. Added basic AI FSM #438
|
|
@ -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
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -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:
|
||||
|
||||
|
||||
};
|
||||
}
|
||||
|
|
@ -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());
|
||||
if (SHInputManager::GetKeyDown(SHInputManager::SH_KEYCODE::H))
|
||||
#ifdef SHEDITOR
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
SHVec2 size{0.0f};
|
||||
//Size of the navigation area
|
||||
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;
|
||||
bool GetCollisionData(uint16_t row, uint16_t col) noexcept;
|
||||
void GenerateNavigationGridData(SHVec3 origin, SHVec3 size, uint16_t numRow, uint16_t numCol) 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue