Pathfinding algorithm start. Currently freezes in pathfinding loop

Co-authored-by: sigonasr2 <sigonasr2@gmail.com>

Crawler/Class.cpp

@@ -309,7 +309,7 @@ bool Wizard::RightClickAbility(){
 		dist--;
 		teleportPoint=p.GetPos()+pointTowardsMouse*dist;
 	}
-	if(dist>0){
+	if(dist>0&&p.CanPathfindTo(p.GetPos(),teleportPoint)){
 		p.SetState(State::TELEPORT);
 		p.teleportAnimationTimer=0.35;
 		p.teleportTarget=teleportPoint;

Crawler/Crawler.cpp

@@ -30,6 +30,9 @@ Crawler::Crawler()
 }
 
 bool Crawler::OnUserCreate(){
+
+	ConsoleCaptureStdOut(true);
+
 	InitializeLevel("assets/maps/Level1.tmx",LEVEL1);
 	InitializeLevel("assets/maps/Level2.tmx",LEVEL2);
 	InitializeLevel("assets/Campaigns/1_1_test.tmx",CAMPAIGN_1_1);
@@ -390,6 +393,9 @@ bool Crawler::DownReleased(){
 
 void Crawler::HandleUserInput(float fElapsedTime){
 	bool setIdleAnimation=true;
+	if(GetKey(F1).bPressed){
+		ConsoleShow(F1);
+	}
 	if(GetMouseWheel()>0){
 		switch(player.cl){
 			case WARRIOR:{

Crawler/Pathfinding.cpp

@@ -0,0 +1,109 @@
+#include "Pathfinding.h"
+#include "DEFINES.h"
+#include "Crawler.h"
+
+INCLUDE_game
+
+void Pathfinding::Initialize(){
+    nodes = new sNode[game->WORLD_SIZE.x * game->WORLD_SIZE.y];
+		for (int x = 0; x < game->WORLD_SIZE.x; x++)
+			for (int y = 0; y < game->WORLD_SIZE.y; y++)
+			{
+				nodes[y * game->WORLD_SIZE.x + x].x = x; // ...because we give each node its own coordinates
+				nodes[y * game->WORLD_SIZE.x + x].y = y;
+                geom2d::rect<int>tile=game->GetTileCollision(game->GetCurrentLevel(),{float(x),float(y)});
+				nodes[y * game->WORLD_SIZE.x + x].bObstacle = tile.pos!=game->NO_COLLISION.pos||tile.size!=game->NO_COLLISION.size;
+				nodes[y * game->WORLD_SIZE.x + x].parent = nullptr;
+				nodes[y * game->WORLD_SIZE.x + x].bVisited = false;
+			}
+
+		for (int x = 0; x < game->WORLD_SIZE.x; x++)
+			for (int y = 0; y < game->WORLD_SIZE.y; y++)
+			{
+				if(y>0)
+					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y - 1) * game->WORLD_SIZE.x + (x + 0)]);
+				if(y<game->WORLD_SIZE.y-1)
+					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 1) * game->WORLD_SIZE.x + (x + 0)]);
+				if (x>0)
+					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 0) * game->WORLD_SIZE.x + (x - 1)]);
+				if(x<game->WORLD_SIZE.x-1)
+					nodes[y*game->WORLD_SIZE.x + x].vecNeighbours.push_back(&nodes[(y + 0) * game->WORLD_SIZE.x + (x + 1)]);
+			}
+
+		// Manually positio the start and end markers so they are not nullptr
+		nodeStart = &nodes[(game->WORLD_SIZE.y / 2) * game->WORLD_SIZE.x + 1];
+		nodeEnd = &nodes[(game->WORLD_SIZE.y / 2) * game->WORLD_SIZE.x + game->WORLD_SIZE.x-2];
+}
+
+int Pathfinding::Solve_AStar(vf2d startPos,vf2d endPos){
+    nodeStart=&nodes[int(startPos.y/24)*game->WORLD_SIZE.x+int(startPos.x/24)];
+    nodeEnd=&nodes[int(endPos.y/24)*game->WORLD_SIZE.x+int(endPos.x/24)];
+    
+    std::cout<<"Path Start:"<<nodeStart<<std::endl;
+    std::cout<<"Path End:"<<nodeEnd<<std::endl;
+    for (int x = 0; x < game->WORLD_SIZE.x; x++){
+        for (int y = 0; y < game->WORLD_SIZE.y; y++){
+            nodes[y*game->WORLD_SIZE.x + x].bVisited = false;
+            nodes[y*game->WORLD_SIZE.x + x].fGlobalGoal = INFINITY;
+            nodes[y*game->WORLD_SIZE.x + x].fLocalGoal = INFINITY;
+            nodes[y*game->WORLD_SIZE.x + x].parent = nullptr;	// No parents
+        }
+    }
+
+    auto distance = [](sNode* a, sNode* b) // For convenience
+    {
+        return sqrtf((a->x - b->x)*(a->x - b->x) + (a->y - b->y)*(a->y - b->y));
+    };
+
+    auto heuristic = [distance](sNode* a, sNode* b)
+    {
+        return distance(a, b);
+    };
+
+    sNode *nodeCurrent = nodeStart;
+    nodeStart->fLocalGoal = 0.0f;
+    nodeStart->fGlobalGoal = heuristic(nodeStart, nodeEnd);
+
+    std::list<sNode*> listNotTestedNodes;
+    listNotTestedNodes.push_back(nodeStart);
+
+    while (!listNotTestedNodes.empty() && nodeCurrent != nodeEnd)
+    {
+        listNotTestedNodes.sort([](const sNode* lhs, const sNode* rhs){ return lhs->fGlobalGoal < rhs->fGlobalGoal; } );
+        
+        while(!listNotTestedNodes.empty() && listNotTestedNodes.front()->bVisited)
+            listNotTestedNodes.pop_front();
+        if (listNotTestedNodes.empty())
+            break;
+
+        nodeCurrent = listNotTestedNodes.front();
+        nodeCurrent->bVisited = true;
+        for (auto nodeNeighbour : nodeCurrent->vecNeighbours)
+        {
+            if (!nodeNeighbour->bVisited && nodeNeighbour->bObstacle == 0)
+                listNotTestedNodes.push_back(nodeNeighbour);
+
+            float fPossiblyLowerGoal = nodeCurrent->fLocalGoal + distance(nodeCurrent, nodeNeighbour);
+
+            if (fPossiblyLowerGoal < nodeNeighbour->fLocalGoal)
+            {
+                nodeNeighbour->parent = nodeCurrent;
+                nodeNeighbour->fLocalGoal = fPossiblyLowerGoal;
+                nodeNeighbour->fGlobalGoal = nodeNeighbour->fLocalGoal + heuristic(nodeNeighbour, nodeEnd);
+            }
+        }	
+    }
+
+    int pathLength=0;
+    if (nodeEnd != nullptr)
+    {
+        sNode *p = nodeEnd;
+        while (p->parent != nullptr)
+        {
+            pathLength++;
+            p = p->parent;
+        }
+    }
+    std::cout<<"Path Length:"<<pathLength<<std::endl;
+    return pathLength;
+}

Crawler/Pathfinding.h

@@ -0,0 +1,24 @@
+#pragma once
+#include "olcPixelGameEngine.h"
+
+struct Pathfinding{
+    struct sNode
+	{
+		bool bObstacle = false;			// Is the node an obstruction?
+		bool bVisited = false;			// Have we searched this node before?
+		float fGlobalGoal;				// Distance to goal so far
+		float fLocalGoal;				// Distance to goal if we took the alternative route
+		int x;							// Nodes position in 2D space
+		int y;
+		std::vector<sNode*> vecNeighbours;	// Connections to neighbours
+		sNode* parent;					// Node connecting to this node that offers shortest parent
+	};
+
+	sNode *nodes = nullptr;
+
+	sNode *nodeStart = nullptr;
+	sNode *nodeEnd = nullptr;
+
+    void Initialize();
+    int Solve_AStar(vf2d startPos,vf2d endPos);
+};

Crawler/Player.cpp

@@ -433,3 +433,7 @@ std::vector<Buff>Player::GetBuffs(BuffType buff){
 	std::copy_if(buffList.begin(),buffList.end(),std::back_inserter(filteredBuffs),[buff](Buff&b){return b.type==buff;});
 	return filteredBuffs;
 }
+
+bool Player::CanPathfindTo(vf2d pos,vf2d targetPos){
+	return path.Solve_AStar(pos,targetPos)<8;//We'll say 7 tiles or less is close enough to 650 range. Have a little bit of wiggle room.
+}

Crawler/Player.h

@@ -6,6 +6,7 @@
 #include "Ability.h"
 #include "Class.h"
 #include "Buff.h"
+#include "Pathfinding.h"
 
 struct Player{
 	friend class Crawler;
@@ -63,6 +64,7 @@ struct Player{
 	bool SetPos(vf2d pos);
 	void SetClass(Class cl);
 	std::vector<Buff>buffList;
+	Pathfinding path;
 	protected:
 	public:
 	Player();
@@ -86,6 +88,7 @@ struct Player{
 	bool HasIframes();
 	void UpdateWalkingAnimation(Key direction);
 	void UpdateIdleAnimation(Key direction);
+	bool CanPathfindTo(vf2d pos,vf2d targetPos);
 
 	void AddBuff(BuffType type,float duration,float intensity);
 	std::vector<Buff>GetBuffs(BuffType buff);