diff options
Diffstat (limited to 'engine-ocean/Game/Systems/Pathfinding')
4 files changed, 473 insertions, 0 deletions
diff --git a/engine-ocean/Game/Systems/Pathfinding/aimovementsystem.cpp b/engine-ocean/Game/Systems/Pathfinding/aimovementsystem.cpp new file mode 100644 index 0000000..8abbec8 --- /dev/null +++ b/engine-ocean/Game/Systems/Pathfinding/aimovementsystem.cpp @@ -0,0 +1,35 @@ +#include "aimovementsystem.h" +#include "Game/Components/TransformComponent.h" +#include "Game/Components/PathfindComponent.h" +#include "Game/Systems/AI/aibehaviorcomponent.h" + +AIMovementSystem::AIMovementSystem(std::map<std::string, std::shared_ptr<GameObject>>& dynamic_gameobjects, + std::map<std::string, std::shared_ptr<GameObject>>& rigid_gameobjects) : + m_dynamic_gameobjects(dynamic_gameobjects), + m_rigid_gameobjects(rigid_gameobjects) + +{ + // m_path = m_rigid_gameobjects.at("navmesh")->getComponent<PathfindComponent>()->getPath(glm::vec3(-0.58249, 0, -0.0210782), glm::vec3(19.5371, 0, 1.39167)); + +} + +TransformComponent* AIMovementSystem::getTransform(std::shared_ptr<GameObject> &go){ + return go->getComponent<TransformComponent>(); +} + + +void AIMovementSystem::update(double deltaTime){ + for (auto &go : m_dynamic_gameobjects){ + if (go.second->hasComponent<AIBehaviorComponent>()){ + go.second->getComponent<AIBehaviorComponent>()->update(deltaTime); + } + } +} + + + + +void AIMovementSystem::draw(){} +void AIMovementSystem::scrollEvent(double distance){} +void AIMovementSystem::mousePosEvent(double xpos, double ypos){} + diff --git a/engine-ocean/Game/Systems/Pathfinding/aimovementsystem.h b/engine-ocean/Game/Systems/Pathfinding/aimovementsystem.h new file mode 100644 index 0000000..e880e44 --- /dev/null +++ b/engine-ocean/Game/Systems/Pathfinding/aimovementsystem.h @@ -0,0 +1,34 @@ +#ifndef AIMOVEMENTSYSTEM_H +#define AIMOVEMENTSYSTEM_H +#include "Game/Components/TransformComponent.h" +#include "Game/Systems/Pathfinding/pathfinder.h" +#include "Game/Systems/system.h" + + +class AIMovementSystem : public System +{ +public: + AIMovementSystem(std::map<std::string, std::shared_ptr<GameObject>>& dynamic_gameobjects, + std::map<std::string, std::shared_ptr<GameObject>>& rigid_gameobjects); + void draw() override; + void update(double deltaTime) override; + void scrollEvent(double distance) override; + void mousePosEvent(double xpos, double ypos) override; + +private: + float gravitySimulation(float &initial_v, double deltaTime, float snapshot_time, float gravity); + TransformComponent* getTransform(std::shared_ptr<GameObject> &go); + + std::map<std::string, std::shared_ptr<GameObject>>& m_dynamic_gameobjects; + std::map<std::string, std::shared_ptr<GameObject>>& m_rigid_gameobjects; + + + float horiz_velocity = .005f; + float snapshot_time = 0.f; + + std::vector<glm::vec3> m_path; + + //std::shared_ptr<Pathfinder> m_pathfinder; +}; + +#endif // AIMOVEMENTSYSTEM_H diff --git a/engine-ocean/Game/Systems/Pathfinding/pathfinder.cpp b/engine-ocean/Game/Systems/Pathfinding/pathfinder.cpp new file mode 100644 index 0000000..35d67d2 --- /dev/null +++ b/engine-ocean/Game/Systems/Pathfinding/pathfinder.cpp @@ -0,0 +1,321 @@ +#include "pathfinder.h" +#include "glm/glm.hpp" +#include "glm/gtx/hash.hpp" + +#include <iostream> +#include <set> +#include <vector> + +// there can be multiple pathfinders for multiple environemnt meshes +// pathfinding/environment component for an environment mesh? +Pathfinder::Pathfinder(std::vector<glm::vec3> vertices, std::vector<glm::ivec3> triangles): + m_vertices(vertices), + m_triangles(triangles) +{ + initializeEdges(); +} + +// for entire scene, generate data structure that holds nav mesh +void Pathfinder::initializeEdges(){ + + std::map<std::pair<int,int>, TriangleEdge> temp_data; + + // indices of vertex pairs + std::vector<std::pair<int,int>> range = {std::make_pair(0, 1), + std::make_pair(1, 2), + std::make_pair(2, 0)}; + + // make edges for all triangles and populate temp_data map + for (glm::vec3 triangle : m_triangles){ + std::vector<std::pair<int,int>> keys; + for (auto index_pair : range ){ + // make ordered pair of the index of vertex held by triangle , for all edges + std::pair<int, int> key = makeOrderedPair(triangle[index_pair.first], triangle[index_pair.second]); + keys.push_back(key); + + if (temp_data.count(key) != 0){ + temp_data.at(key).count ++; + } else { + // make a new entry that populates the midpoint + TriangleEdge edge = {m_vertices[key.first], m_vertices[key.second]}; + temp_data.insert(std::make_pair(key, edge)); + } + } + + for (auto key : keys){ + // add the two neighbors for each key + for (auto neighbor_id : keys){ + if (key != neighbor_id){ + temp_data.at(key).adjacentEdges.insert(neighbor_id); + } + } + } + } + + std::set<std::pair<int,int>> interiorEdges; + + // remove exterior edges from adjacent lists + for (auto &entry : temp_data){ + // make new adjacency list + std::set<std::pair<int,int>> edited_adj_list; + + for (const auto adjNode : entry.second.adjacentEdges){ + // if adjacent node is an interior node, add to new adj list + if (temp_data.at(adjNode).count > 1){ + edited_adj_list.insert(adjNode); + } + } + + // reassign adj list + entry.second.adjacentEdges.swap(edited_adj_list); + } + + // add interior edges + for (const auto &entry : temp_data){ + if (entry.second.count > 1){ + ANode node; + node.adjacentNodes = entry.second.adjacentEdges; + node.pos = entry.second.midpoint; + m_navdata.insert(std::make_pair(entry.first, node)); + } + } + + +} + + +std::pair<int, int> Pathfinder::makeOrderedPair(int i, int j){ + std::pair<int, int> pair = std::pair(std::min(i,j), std::max(i,j)); + return pair; +} + +// traverse navmeshgraph +// A --> start ; B --> destination +std::vector<glm::vec3> Pathfinder::findPath(const glm::vec3 A, const glm::vec3 B){ + // start and end indices are double pair because no existing edge has two of the same vertices + m_startNodePos = A; + m_endNodePos = B; + + // initialize empty path + std::vector<glm::vec3> empty; + empty.push_back(glm::vec3(0.f)); + + // initialize start and ending node + ANode startNode, endNode; + startNode.pos = m_startNodePos; + endNode.pos = m_endNodePos; + startNode.Gcost = 0; + m_navdata.insert(std::pair(startNodeID, startNode)); + m_navdata.insert(std::pair(endNodeID, endNode)); + + + // calculate distance to end point for each item in map + for (auto &node : m_navdata){ + node.second.Hcost = getDistance(node.second.pos, B); + updateFCost(node.first); + } + + // determine if one or both start/end points are in navmesh or not + std::pair<bool, std::set<std::pair<int, int>>> validStartPoint = findEnclosingTriangle(A); + std::pair<bool, std::set<std::pair<int, int>>> validEndPoint = findEnclosingTriangle(B); + + // if neither or both points can be found in navmesh, end early + if (!validStartPoint.first || !validEndPoint.first){ + std::cout << "-- a point is not in navmesh" << std::endl; + return empty; + } + + // if both points are on same triangle, then return only the destination + if (validStartPoint.second == validEndPoint.second){ + std::vector<glm::vec3> destination; + destination.push_back(B); + std::cout << "-- SAME TRIANGLE" << std::endl; + return destination; + } + + std::set<std::pair<int,int>> open; + std::set<std::pair<int,int>> closed; + + // initialize open list + // check if id is a valid interior node + for (const auto &id : validStartPoint.second){ + if (m_navdata.contains(id)){ + m_navdata[id].previousV = startNodeID; + m_navdata[id].Gcost = getDistance(m_navdata[id].pos, m_startNodePos); + updateFCost(id); + open.insert(id); + } + } + + // calculate paths with Astar + bool reachable = traverseAStar(startNodeID, validEndPoint.second, open, closed); + + if (reachable){ + std::cout << "reachable!" << std::endl; + return getNavigablePath(startNodeID, endNodeID); + } + + // otherwise, return glm::vec3(0) so that entity doesnt move + std::cout << "not reachable" << std::endl; + return empty; +} + +void Pathfinder::updateFCost(const std::pair<int,int> &node){ + m_navdata[node].Fcost = m_navdata[node].Gcost + m_navdata[node].Hcost; + +} + +bool Pathfinder::traverseAStar(const std::pair<int,int> &currNodeID, const std::set<std::pair<int,int>> &endpointNodeIDs, + std::set<std::pair<int,int>> &open, std::set<std::pair<int,int>> &closed){ + + glm::vec3 currNodePos = m_navdata.at(currNodeID).pos; + + // base case: if looking at nodes that are way too far from entity start pos, then just end + if (getDistance(currNodePos, m_startNodePos) > m_maxDistance){ + std::cout << "-- DISTANCE TOO FAR" << std::endl; + return false; + } + + float lowestF = INFINITY; + float lowestH = INFINITY; + std::pair<int,int> C; // where C is the next node to visit + + for (const std::pair<int,int> &nodeID : open){ + // calculate G, which also populates F + // --------------- do i also need to check if the previous f cost was less than the updated f cost? + float new_Gcost = getDistance(m_navdata[nodeID].pos, currNodePos) + m_navdata[currNodeID].Gcost; + m_navdata[nodeID].Gcost = new_Gcost; + updateFCost(nodeID); + + if (m_navdata[nodeID].Fcost == lowestF){ + // go by lowest H if F costs are equal + if (m_navdata[nodeID].Hcost < lowestH){ + C = nodeID; + lowestF = m_navdata[nodeID].Fcost; + lowestH = m_navdata[nodeID].Hcost; + } + } + + if (m_navdata[nodeID].Fcost < lowestF){ + C = nodeID; + lowestF = m_navdata[nodeID].Fcost; + lowestH = m_navdata[nodeID].Hcost; + } + } + + // move C from open to closed list + open.erase(C); + closed.insert(C); + + //if C is equal to any of the endpoint indices, then path is found + for (const std::pair<int,int> &id : endpointNodeIDs){ + if (C == id){ + m_navdata[endNodeID].previousV = C; + return true; + } + } + + // for neighbor N of C + for (const std::pair<int,int> &N: m_navdata[C].adjacentNodes){ + if (closed.contains(N)) continue; + + // calculate potential F cost of C-->N + float new_Gcost = getDistance(m_navdata[N].pos, m_navdata[C].pos) + m_navdata[C].Gcost; + float new_Fcost = new_Gcost + m_navdata[N].Hcost; + + if (new_Fcost < m_navdata[N].Fcost || !open.contains(N)){ + // update N's G and F cost + m_navdata[N].Gcost = new_Gcost; + updateFCost(N); + // set previous node of N to be C + m_navdata[N].previousV = C; + + // add N to open if its not already in it + open.insert(N); + } + } + + // if there are still open nodes, visit the next one (recurse) + if (!open.empty()){ + traverseAStar(C, endpointNodeIDs, open, closed); + } + + // otherwise no more open nodes, thus a* has finished + return true; +} + + +// distance is the un-squarerooted distance between two points +float Pathfinder::getDistance(glm::vec3 a, glm::vec3 b){ + return pow(a.x-b.x, 2) + pow(a.y-b.y, 2) + pow(a.z-b.z, 2); +} + + +// referenced from https://blackpawn.com/texts/pointinpoly/default.html +bool Pathfinder::sameSide(glm::vec3 p1, glm::vec3 p2, glm::vec3 a, glm::vec3 b){ + glm::vec3 cp1 = glm::cross(b-a, p1-a); + glm::vec3 cp2 = glm::cross(b-a, p2-a); + + glm::vec3 a1 = glm::vec3(cp1.x, cp1.y + .001, cp1.z); + glm::vec3 a2 = glm::vec3(cp1.x, cp1.y - .001, cp1.z); + + glm::vec3 b1 = glm::vec3(cp2.x, cp2.y + .001, cp2.z); + glm::vec3 b2 = glm::vec3(cp2.x, cp2.y - .001, cp2.z); + + + if (glm::dot(a1, b1) >= 0 || glm::dot(a2, b2) >= 0 + || glm::dot(a1, b2) >= 0 || glm::dot(a2, b1) >= 0) return true; + return false; +} + +bool Pathfinder::pointInTriangle(glm::vec3 p, glm::vec3 v1, glm::vec3 v2, glm::vec3 v3){ + if (sameSide(p,v1, v2,v3) && sameSide(p,v2, v1,v3) && sameSide(p,v3, v1,v2)) return true; + return false; +} + +std::pair<bool, std::set<std::pair<int, int>>> Pathfinder::findEnclosingTriangle(glm::vec3 point){ + + // for each triangle in navmesh, get its vertices + for (glm::vec3 vertex_triple : m_triangles){ + // if point is in any navmesh triangle, then return the 3 nodes associated with that tri + if (pointInTriangle(point, m_vertices[vertex_triple[0]], m_vertices[vertex_triple[1]], m_vertices[vertex_triple[2]])){ + return std::make_pair(true, getEnclosingTriangleEdges(vertex_triple)); + } + } + + // if no triangles contained the point, return false + return std::make_pair(false, getEnclosingTriangleEdges(glm::vec3(0.f))); +} + +// gets node indices +std::set<std::pair<int, int>> Pathfinder::getEnclosingTriangleEdges(glm::vec3 indices){ + std::set<std::pair<int, int>> ids; + ids.insert(makeOrderedPair(indices[0], indices[1])); + ids.insert(makeOrderedPair(indices[1], indices[2])); + ids.insert(makeOrderedPair(indices[2], indices[0])); + + return ids; +} + +std::vector<glm::vec3> Pathfinder::getNavigablePath(const std::pair<int,int> &start, const std::pair<int,int> &end){ + // start at B, and use previousV pointers to get back to A + std::vector<glm::vec3> path; + path.push_back(m_navdata[end].pos); + + std::pair<int,int> currV = end; + + while (currV != start){ + // add current point to path + glm::vec3 pos = m_navdata[m_navdata[currV].previousV].pos; + path.push_back(pos); + + // reassign variable to where currV points, and enter loop again + currV = m_navdata[currV].previousV; + } + + // path where the very last entry is the start point, and the very first entry is the end point + return path; + +} + + diff --git a/engine-ocean/Game/Systems/Pathfinding/pathfinder.h b/engine-ocean/Game/Systems/Pathfinding/pathfinder.h new file mode 100644 index 0000000..d9abbd6 --- /dev/null +++ b/engine-ocean/Game/Systems/Pathfinding/pathfinder.h @@ -0,0 +1,83 @@ +#ifndef PATHFINDER_H +#define PATHFINDER_H + +#include "glm/fwd.hpp" +#include "glm/gtx/hash.hpp" + + +#include <map> +#include <set> +#include <vector> + +struct ANode{ + glm::vec3 pos; + // A* related: + float Hcost = 0; // distance to destination + float Gcost = 0; // distance from start to current node (accumulative G) + float Fcost = Gcost+Hcost; + std::pair<int,int> previousV; + std::set<std::pair<int,int>> adjacentNodes; +}; + +struct TriangleEdge{ + glm::vec3 midpoint; + std::set<std::pair<int,int>> adjacentEdges; + + TriangleEdge(glm::vec3 v1, glm::vec3 v2): + midpoint(.5f*(v2+v1)) + {} + // num of times it appears in the navgraph + int count = 1; +}; + +class Pathfinder +{ +public: + Pathfinder(std::vector<glm::vec3> vertices, std::vector<glm::ivec3> triangles); + std::vector<glm::vec3> findPath(const glm::vec3 A, const glm::vec3 B); + + + +private: + + void updateFCost(const std::pair<int,int> &node); + + + + + bool traverseAStar(const std::pair<int,int> &currNodeID, const std::set<std::pair<int,int>> &endpointNodeIDs, + std::set<std::pair<int,int>> &open, std::set<std::pair<int,int>> &closed); + + std::set<std::pair<int, int>> getEnclosingTriangleEdges(glm::vec3 indices); + + void initializeEdges(); + float getDistance(glm::vec3 a, glm::vec3 b); + std::vector<glm::vec3> getNavigablePath(const std::pair<int,int> &start, const std::pair<int,int> &end); + std::pair<bool, std::set<std::pair<int, int>>> findEnclosingTriangle(glm::vec3 point); + bool pointInTriangle(glm::vec3 p, glm::vec3 v1, glm::vec3 v2, glm::vec3 v3); + bool sameSide(glm::vec3 p1, glm::vec3 p2, glm::vec3 a, glm::vec3 b); + + std::set<std::vector<float>> getAdjacentNodes(const std::set<std::pair<int,int>> &id_pairs, std::map<std::pair<int,int>, TriangleEdge> &temp_data); + + + std::pair<int, int> makeOrderedPair(int i, int j); + + // maps pos (midpoint of a edge) to node + std::map<std::pair<int,int>, ANode> m_navdata; + std::vector<glm::vec3> m_vertices; + std::vector<glm::ivec3> m_triangles; + glm::vec3 m_startNodePos; + glm::vec3 m_endNodePos; + + std::pair<int,int> startNodeID = std::make_pair(0,0); + std::pair<int,int> endNodeID = std::make_pair(1,1); + + + float m_maxDistance = 500; // 20 meters max distance that an ai can pathfind + + + + +}; + +#endif // PATHFINDER_H |