vastly optimized pathfinding

Assets/Models/Maps/222/222.lua

@@ -16,18 +16,19 @@ resourceDeposits = {
 {id = ResourceId.RESOURCE, pos = {x = 50, y = 0, z = 0}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}, initAmmount = 30000},
 },
 units = {
-	{id = UnitId.TANK, pos = {x = -50.000000, y = 0.000000, z = -50.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
+	{id = UnitId.ENGINEER, pos = {x = -10.000000, y = 0.000000, z = -10.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
-	--{id = UnitId.ENGINEER, pos = {x = -10.000000, y = 0.000000, z = -10.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
+	--{id = UnitId.TANK, pos = {x = -50.000000, y = 0.000000, z = -50.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
 	--{id = UnitId.POINT_DEFENSE, pos = {x = 0.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
 }
 },
 {
 	resourceDeposits = {},
 	units = {
-		{id = UnitId.WAR_MECH, pos = {x = 3.000000, y = 0.000000, z = 12.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
+		--{id = UnitId.WAR_MECH, pos = {x = 3.000000, y = 0.000000, z = 12.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
-		{id = UnitId.TANK, pos = {x = 0.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
+		--{id = UnitId.TANK, pos = {x = 0.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
-		{id = UnitId.POINT_DEFENSE, pos = {x = 0.000000, y = 0.000000, z = 20.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}, buildStatus = 100},
+		--{id = UnitId.POINT_DEFENSE, pos = {x = 0.000000, y = 0.000000, z = 20.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}, buildStatus = 100},
-		{id = UnitId.SCOUT_TRANSPORT, pos = {x = 40.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
+		--{id = UnitId.SCOUT_TRANSPORT, pos = {x = 40.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
+		{id = UnitId.WAR_MECH, pos = {x = 45.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
 		{id = UnitId.ENGINEER, pos = {x = 45.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}},
 		--{id = UnitId.EXTRACTOR, pos = {x = 30.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}, buildStatus = 100},
 		--{id = UnitId.LAB, pos = {x = 50.000000, y = 0.000000, z = 0.000000}, rot = {w = 1.000000, x = 0.000000, y = 0.000000, z = 0.000000}, buildStatus = 100},

CMakeLists.txt

@@ -11,7 +11,7 @@ endif()
 
 set(CMAKE_CXX_STANDART 17)
 set(CMAKE_BUILD_TYPE Debug)
-set(BUILD_TESTS OFF)
+set(BUILD_TESTS ON)
 set(BUILD_SHARED_LIBS OFF CACHE BOOL FORCED)
 cmake_policy(SET CMP0015 NEW)
 

map.cpp

@@ -520,7 +520,7 @@ namespace battleship{
     void Map::load(string mapName){
 		this->mapName = mapName;
 
-		Pathfinder::getSingleton()->setImpassibleNodeVal(u16(0 - 1));
+		Pathfinder::getSingleton()->setImpassibleNodeVal(u32(0 - 1));
 		string path = GameManager::getSingleton()->getPath();
 		AssetManager::getSingleton()->load(path + "Textures/", true);
 

pathfinder.cpp

@@ -16,55 +16,64 @@ namespace battleship{
 			return pathfinder;
 		}
 
-		int Pathfinder::findMinDistVert(vector<pair<int, bool>> &cells, u32 distances[]){
+		vector<int> Pathfinder::findPath(vector<Map::Cell> &cells, vector<float> &heuristics, int source, int dest, int unitType){
-			int numCells = cells.size(), minDistVert;
-
-			for(int i = 0; i < numCells; i++)
-				if(!cells[i].second){
-					minDistVert = i;
-					break;
-				}
-
-			for(int i = 0; i < numCells; i++){
-				bool checked = cells[i].second;
-
-				if(!checked && distances[i] < distances[minDistVert])
-					minDistVert = i;
-			}
-
-			return minDistVert;
-		}
-
-		vector<int> Pathfinder::findPath(vector<Map::Cell> &cells, int source, int dest, int unitType){
 			const int size = cells.size();
 			u32 *distances = new u32[size];
 			vector<int> *paths = new vector<int>[size];
 			paths[source].push_back(source);
+
 			vector<pair<int, bool>> cellsByCheck;
 
+			vector<bool> cellChecked;
+
 			for(int i = 0; i < size; i++){
 				cellsByCheck.push_back(pair(i, false));
 				distances[i] = (i == source ? 0 : impassibleNodeVal);
+				cellChecked.push_back(false);
 			}
 
+			bool useHeur = !heuristics.empty();
+			vector<int> possibleMinCells = vector<int>{source};
+			cellChecked[source] = true;
+
 			while(!cellsByCheck[dest].second){
-				int vertStrich = findMinDistVert(cellsByCheck, distances);
+				int vertStrich = possibleMinCells[0], vsId = 0;
+
+				for(int i = 0; i < possibleMinCells.size(); i++){
+					float sum1 = distances[possibleMinCells[i]] + (useHeur ? heuristics[possibleMinCells[i]] : 0); 
+					float sum2 = distances[possibleMinCells[vsId]] + (useHeur ? heuristics[possibleMinCells[vsId]] : 0); 
+
+					if(sum1 < sum2 || (useHeur && sum1 == sum2 && heuristics[i] < heuristics[vertStrich])){
+						vertStrich = possibleMinCells[i];
+						vsId = i;
+					}
+				}
+
+				cellChecked[possibleMinCells[vsId]] = false;
+				possibleMinCells.erase(possibleMinCells.begin() + vsId);
+
 				cellsByCheck[vertStrich].second = true;
 
 				int numEdges = cells[vertStrich].edges.size();
 
 				for(int i = 0; i < numEdges; i++){
+					int edgeNode = cells[vertStrich].edges[i].destCellId;
+
+					if(cellsByCheck[edgeNode].second) continue;
+
+					if(!cellChecked[edgeNode]){
+						cellChecked[edgeNode] = true;
+						possibleMinCells.push_back(edgeNode);
+					}
+
 					bool canMoveToStrichCell = true;
 					bool ship = ((UnitType)unitType == UnitType::UNDERWATER || (UnitType)unitType == UnitType::SEA_LEVEL);
 
 					if((ship && cells[vertStrich].type != Map::Cell::WATER) || ((UnitType)unitType == UnitType::LAND && cells[vertStrich].type != Map::Cell::LAND))
-						canMoveToStrichCell = false;
+						continue;
 
-					int edgeNode = cells[vertStrich].edges[i].destCellId, cellTypeFactor = (canMoveToStrichCell ? 1 : 100);
+					if(distances[vertStrich] + cells[vertStrich].edges[i].weight < distances[edgeNode]){
-					bool isChecked = cellsByCheck[edgeNode].second;
+						distances[edgeNode] = distances[vertStrich] + cells[vertStrich].edges[i].weight;
-
-					if(!isChecked && (distances[vertStrich] + cells[vertStrich].edges[i].weight * cellTypeFactor < distances[edgeNode])){
-						distances[edgeNode] = distances[vertStrich] + cells[vertStrich].edges[i].weight * cellTypeFactor;
 						paths[edgeNode] = paths[vertStrich];
 						paths[edgeNode].push_back(edgeNode);
 					}

pathfinder.h

@@ -11,12 +11,11 @@ namespace battleship{
 	class Pathfinder{
 		public:
 			static Pathfinder* getSingleton();
-			std::vector<int> findPath(std::vector<Map::Cell>&, int, int, int = -1);
+			std::vector<int> findPath(std::vector<Map::Cell>&, std::vector<float>&, int, int, int = -1);
 			inline vb01::u32 getImpassibleNodeVal(){return impassibleNodeVal;}
 			inline void setImpassibleNodeVal(vb01::u32 val){this->impassibleNodeVal = val;}
 		private:
 			Pathfinder(){}
-			int findMinDistVert(std::vector<std::pair<int, bool>>&, vb01::u32[]);
 
 			vb01::u32 impassibleNodeVal;
 	};

pathfinderTest.cpp

@@ -1,6 +1,7 @@
 #include "pathfinderTest.h"
 #include "pathfinder.h"
 
+#include <vector.h>
 #include <util.h>
 
 #include <vector>
@@ -11,11 +12,12 @@ namespace battleship{
 
 		vector<Map::Cell> PathfinderTest::generateCellGraph(int numCellsOnSide){
 			vector<Map::Cell> cells;
+			float cellSize = 2;
 
 			for(int i = 0; i < numCellsOnSide; i++)
 				for(int j = 0; j < numCellsOnSide; j++){
-					vector<Map::Edge> edges = Map::generateAdjacentNodeEdges(numCellsOnSide, i, numCellsOnSide, j, 1);
+					vector<Map::Edge> edges = Map::generateAdjacentNodeEdges(numCellsOnSide, i, numCellsOnSide, j, 1000);
-					cells.push_back(Map::Cell(Vector3::VEC_ZERO, Map::Cell::Type::LAND, edges));
+					cells.push_back(Map::Cell(Vector3(j * cellSize, 0, i * cellSize), Map::Cell::Type::LAND, edges));
 				}
 
 			return cells;
@@ -32,6 +34,22 @@ namespace battleship{
 			return sumPathWeights;
 		}
 
+		vector<float> PathfinderTest::generateHeuristics(vector<Map::Cell> &cells, int dest, int type){
+			vector<float> heur;
+			int numSideCells = sqrt(cells.size());
+
+			if(type == 0)
+				for(Map::Cell &cell : cells)
+					heur.push_back(145 * cells[dest].pos.getDistanceFrom(cell.pos));
+			else if(type == 1)
+				for(int i = 0; i < cells.size(); i++){
+					int x = i % numSideCells, y = i / numSideCells;
+					heur.push_back(2 * (numSideCells - 1) - (x + y));
+				}
+
+			return heur;
+		}
+
 		void PathfinderTest::testFindPath(){
 			cells = vector<Map::Cell>{
 				Map::Cell(Vector3::VEC_ZERO, Map::Cell::Type::LAND, vector<Map::Edge>{Map::Edge(0, 0, 0), Map::Edge(2, 0, 1), Map::Edge(4, 0, 2)}),
@@ -46,7 +64,9 @@ namespace battleship{
 			const u16 INF = u16(0 - 1);
 			pathfinder->setImpassibleNodeVal(INF);
 
-			vector<int> path = pathfinder->findPath(cells, 0, cells.size() - 1);
+			int src = 0, dest = cells.size() - 1;
+			vector<float> heur;
+			vector<int> path = pathfinder->findPath(cells, heur, src, dest, 3);
 			CPPUNIT_ASSERT(path == vector<int>({0, 1, 2, 4, 3, 6}));
 
 			int sumPathWeights = calcPathLength(path);
@@ -54,13 +74,15 @@ namespace battleship{
 		}
 
 		void PathfinderTest::testFindBigPath(){
-			int numIterations = 1;
+			cells = generateCellGraph(250);
-			cells = generateCellGraph(60);
+			int numIterations = 1, src = 0, dest = cells.size() - 1;
+			vector<float> heur;
+			heur = generateHeuristics(cells, dest, 0);
 			s64 sumTime = 0;
 
 			for(int i = 0; i < numIterations; i++){
 				s64 t0 = getTime();
-				pathfinder->findPath(cells, 0, cells.size() - 1);
+				pathfinder->findPath(cells, heur, src, dest, 3);
 				s64 t1 = getTime();
 				sumTime += t1 - t0;
 			}

pathfinderTest.h

@@ -26,6 +26,7 @@ namespace battleship{
 			std::vector<Map::Cell> cells;
 
 			std::vector<Map::Cell> generateCellGraph(int);
+			std::vector<float> generateHeuristics(std::vector<Map::Cell>&, int, int = 0);
 			int calcPathLength(std::vector<int>&);
 	};
 }

vehicle.cpp

@@ -259,9 +259,14 @@ namespace battleship{
 
 		if(type != UnitType::HOVER && !(waterVehCanMove || landVehCanMove)) return;
 
-		Pathfinder *pathfinder = Pathfinder::getSingleton();
 		int dest = map->getCellId(destPos);
-		vector<int> path = pathfinder->findPath(cells, source, dest, (int)type);
+
+		vector<float> heuristics;
+
+		for(Map::Cell &cell : cells)
+			heuristics.push_back(145 * (cells[dest].pos.getDistanceFrom(cell.pos)));
+
+		vector<int> path = Pathfinder::getSingleton()->findPath(cells, heuristics, source, dest, (int)type);
 		bool pathTruncated = false;
 
 		for(int i = 0; i < path.size(); i++){