nav mesh path finding WIP

Using dijkstra instead of AStar because some paths are extremely more expensive in order to prevent routes that require flying (not possible for players without gauss, stacking, etc.). So far it's working pretty well. It prefers to walk around the entire map and up some stairs instead of getting a bunch of players to stack in order to reach a vent in the ceiling or something.

Next major problem is having entities connect leaves together which otherwise wouldn't be, or would be too expensive due to vertical movement (teleports, ladders, water, elevators, trigger_push). Paths also sometimes take odd turns because of going to the center of a leaf before the face of the next leaf.

Author: wootguy

src/editor/BspRenderer.cpp

@@ -978,8 +978,8 @@ void BspRenderer::generateLeafNavMeshBuffer() {
 	vector<cVert> wireframeVerts;
 	vector<FaceMath> faceMaths;
 
-	for (int lf = 0; lf < navMesh->numLeaves; lf++) {
-		LeafMesh& mesh = navMesh->leaves[lf];
+	for (int lf = 0; lf < navMesh->nodes.size(); lf++) {
+		LeafNode& mesh = navMesh->nodes[lf];
 
 		color = hullColors[hull];
 		static int r = 0;

src/editor/Gui.cpp

@@ -15,6 +15,7 @@
 #include "tinyfiledialogs.h"
 #include <algorithm>
 #include "BspMerger.h"
+#include "LeafNavMesh.h"
 
 // embedded binary data
 #include "fonts/robotomono.h"
@@ -1951,8 +1952,15 @@ void Gui::drawDebugWidget() {
 							if (i == 0) {
 								ImGui::Text("Leaf: %d", leafIdx);
 							}
-							else {
-								ImGui::Text("Pseudo ID: %d", map->get_leaf(localCamera, i));
+							else if (i == 3 && g_app->debugLeafNavMesh) {
+								int leafIdx = map->get_leaf(localCamera, 3);
+								int leafNavIdx = -1;
+
+								if (leafIdx >= 0 && leafIdx < MAX_MAP_CLIPNODE_LEAVES) {
+									leafNavIdx = g_app->debugLeafNavMesh->leafMap[leafIdx];
+								}
+
+								ImGui::Text("Nav ID: %d", leafNavIdx);
 							}
 							ImGui::Text("Parent Node: %d (child %d)",
 								nodeBranch.size() ? nodeBranch[nodeBranch.size() - 1] : headNode,

src/editor/Renderer.cpp

@@ -456,95 +456,127 @@ void Renderer::renderLoop() {
 
 			if (debugLeafNavMesh) {
 				glLineWidth(1);
+				glDisable(GL_DEPTH_TEST);
 
 				Bsp* map = mapRenderers[0]->map;
 				int leafIdx = map->get_leaf(cameraOrigin, 3);
-				int leafNavIdx = MAX_NAV_LEAVES;
+				int leafNavIdx = -1;
 
 				if (leafIdx >= 0 && leafIdx < MAX_MAP_CLIPNODE_LEAVES) {
 					leafNavIdx = debugLeafNavMesh->leafMap[leafIdx];
 				}
 
-				if (leafNavIdx < MAX_NAV_LEAVES) {
+				if (leafNavIdx >= 0 && leafNavIdx < debugLeafNavMesh->nodes.size()) {
 
 					if (pickInfo.valid && pickInfo.ent && pickInfo.entIdx != 0) {
 						glDisable(GL_DEPTH_TEST);
 
 						int endNode = debugLeafNavMesh->getNodeIdx(map, pickInfo.ent);
-						vector<int> route = debugLeafNavMesh->AStarRoute(map, leafNavIdx, endNode);
+						//vector<int> route = debugLeafNavMesh->AStarRoute(leafNavIdx, endNode);
+						vector<int> route = debugLeafNavMesh->dijkstraRoute(leafNavIdx, endNode);
 
 						if (route.size()) {
-							LeafMesh& firstNode = debugLeafNavMesh->leaves[route[route.size() - 1]];
-
-							vec3 lastPos = firstNode.center;
-							drawBox(firstNode.center, 2, COLOR4(0, 255, 255, 255));
-
-							for (int i = route.size() - 2; i >= 0; i--) {
-								LeafNavNode& node = debugLeafNavMesh->nodes[route[i]];
-								LeafMesh& mesh = debugLeafNavMesh->leaves[route[i]];
-
-								vec3 nodeCenter = mesh.center;
-
-								for (int k = 0; k < MAX_NAV_LEAF_LINKS; k++) {
-									LeafNavLink& link = node.links[k];
-
-									if (link.node == route[i + 1]) {
-										vec3 linkPoint = link.linkArea.center;
-
-										drawLine(lastPos, linkPoint, COLOR4(0, 255, 255, 255));
-										drawLine(linkPoint, mesh.center, COLOR4(0, 255, 255, 255));
+							LeafNode* lastNode = &debugLeafNavMesh->nodes[route[0]];
+
+							vec3 lastPos = lastNode->bottom;
+							drawBox(lastNode->bottom, 2, COLOR4(0, 255, 255, 255));
+
+							for (int i = 1; i < route.size(); i++) {
+								LeafNode& node = debugLeafNavMesh->nodes[route[i]];
+
+								vec3 nodeCenter = node.bottom;
+
+								for (int k = 0; k < lastNode->links.size(); k++) {
+									LeafLink& link = lastNode->links[k];
+
+									if (link.node == route[i]) {
+										vec3 linkPoint = link.bottom;
+
+										if (link.baseCost > 16000) {
+											drawLine(lastPos, linkPoint, COLOR4(255, 0, 0, 255));
+											drawLine(linkPoint, node.bottom, COLOR4(255, 0, 0, 255));
+										}
+										else if (link.baseCost > 0) {
+											drawLine(lastPos, linkPoint, COLOR4(255, 255, 0, 255));
+											drawLine(linkPoint, node.bottom, COLOR4(255, 255, 0, 255));
+										}
+										else {
+											drawLine(lastPos, linkPoint, COLOR4(0, 255, 255, 255));
+											drawLine(linkPoint, node.bottom, COLOR4(0, 255, 255, 255));
+										}
 										drawBox(nodeCenter, 2, COLOR4(0, 255, 255, 255));
 										lastPos = nodeCenter;
 										break;
 									}
 								}
+
+								lastNode = &node;
 							}
 
 							drawLine(lastPos, pickInfo.ent->getHullOrigin(map), COLOR4(0, 255, 255, 255));
 						}
 					}
 					else {
-						LeafNavNode& node = debugLeafNavMesh->nodes[leafNavIdx];
-						LeafMesh& leaf = debugLeafNavMesh->leaves[leafNavIdx];
+						LeafNode& node = debugLeafNavMesh->nodes[leafNavIdx];
 
-						drawBox(leaf.center, 2, COLOR4(0, 255, 0, 255));
+						drawBox(node.bottom, 2, COLOR4(0, 255, 0, 255));
 
 						std::string linkStr;
 
-						for (int i = 0; i < MAX_NAV_LEAF_LINKS; i++) {
-							LeafNavLink& link = node.links[i];
+						for (int i = 0; i < node.links.size(); i++) {
+							LeafLink& link = node.links[i];
 							if (link.node == -1) {
 								break;
 							}
-							LeafMesh& linkLeaf = debugLeafNavMesh->leaves[link.node];
+							LeafNode& linkLeaf = debugLeafNavMesh->nodes[link.node];
 							Polygon3D& linkArea = link.linkArea;
 
-							drawLine(leaf.center, linkArea.center, COLOR4(0, 255, 255, 255));
-							drawLine(linkArea.center, linkLeaf.center, COLOR4(0, 255, 255, 255));
+							if (link.baseCost > 16000) {
+								drawLine(node.bottom, link.bottom, COLOR4(255, 0, 0, 255));
+								drawLine(link.bottom, linkLeaf.bottom, COLOR4(255, 0, 0, 255));
+							}
+							else if (link.baseCost > 0) {
+								drawLine(node.bottom, link.bottom, COLOR4(255, 255, 0, 255));
+								drawLine(link.bottom, linkLeaf.bottom, COLOR4(255, 255, 0, 255));
+							}
+							else {
+								drawLine(node.bottom, link.bottom, COLOR4(0, 255, 255, 255));
+								drawLine(link.bottom, linkLeaf.bottom, COLOR4(0, 255, 255, 255));
+							}
 
 							for (int k = 0; k < linkArea.verts.size(); k++) {
-								drawBox(linkArea.verts[k], 1, COLOR4(255, 255, 0, 255));
+								//drawBox(linkArea.verts[k], 1, COLOR4(255, 255, 0, 255));
 							}
-							drawBox(linkArea.center, 1, COLOR4(0, 255, 0, 255));
-							drawBox(linkLeaf.center, 2, COLOR4(0, 255, 255, 255));
+							drawBox(link.bottom, 1, COLOR4(0, 255, 0, 255));
+							drawBox(linkLeaf.bottom, 2, COLOR4(0, 255, 255, 255));
 							linkStr += to_string(link.node) + " (" + to_string(linkArea.verts.size()) + "v), ";
 						}
 
 						//logf("Leaf node idx: %d, links: %s\n", leafNavIdx, linkStr.c_str());
 					}
 
 				}
-
-				glDisable(GL_DEPTH_TEST);
 				/*
+				Polygon3D linkPoly;
+				LeafNode& node = debugLeafNavMesh->nodes[1441];
+				for (int i = 0; i < MAX_NAV_LEAF_LINKS; i++) {
+					LeafLink& link = node.links[i];
+
+					if (link.node == 1442) {
+						linkPoly = link.linkArea;
+						break;
+					}
+				}
+
+				drawPolygon3D(linkPoly, COLOR4(255, 255, 255, 255));
+
 				colorShader->pushMatrix(MAT_PROJECTION);
 				colorShader->pushMatrix(MAT_VIEW);
 				projection.ortho(0, windowWidth, windowHeight, 0, -1.0f, 1.0f);
 				view.loadIdentity();
 				colorShader->updateMatrixes();
 
-				Line2D edge(vec2(1000, 400), vec2(1400, 630));
-				drawLine2D(edge.start, edge.end, COLOR4(255, 0, 0, 255));
+				drawPolygon2D(linkPoly, vec2(800, 100), vec2(500, 500), COLOR4(255, 0, 0, 255));
 
 				colorShader->popMatrix(MAT_PROJECTION);
 				colorShader->popMatrix(MAT_VIEW);
@@ -1799,20 +1831,38 @@ void Renderer::drawBox(vec3 mins, vec3 maxs, COLOR4 color) {
 	buffer.draw(GL_TRIANGLES);
 }
 
+void Renderer::drawPolygon3D(Polygon3D& poly, COLOR4 color) {
+	static cVert verts[64];
+
+	for (int i = 0; i < poly.verts.size() && i < 64; i++) {
+		vec3 pos = poly.verts[i];
+		verts[i].x = pos.x;
+		verts[i].y = pos.z;
+		verts[i].z = -pos.y;
+		verts[i].c = color;
+	}
+
+	VertexBuffer buffer(colorShader, COLOR_4B | POS_3F, verts, poly.verts.size());
+	buffer.draw(GL_TRIANGLE_FAN);
+}
+
 float Renderer::drawPolygon2D(Polygon3D poly, vec2 pos, vec2 maxSz, COLOR4 color) {
 	vec2 sz = poly.localMaxs - poly.localMins;
 	float scale = min(maxSz.y / sz.y, maxSz.x / sz.x);
 
-	vec2 offset = poly.localMins * -scale;
+	vec2 offset = poly.localMins * -scale + pos;
 
 	for (int i = 0; i < poly.verts.size(); i++) {
 		vec2 v1 = poly.localVerts[i];
-		vec2 v2 = poly.localVerts[(i + 1) % debugPoly.verts.size()];
+		vec2 v2 = poly.localVerts[(i + 1) % poly.verts.size()];
 		drawLine2D(offset + v1*scale, offset + v2 * scale, color);
+		if (i == 0) {
+			drawLine2D(offset + v1 * scale, offset + (v1 + (v2-v1)*0.5f) * scale, COLOR4(0,255,0,255));
+		}
 	}
 
 	{
-		vec2 cam = debugPoly.project(cameraOrigin);
+		vec2 cam = poly.project(cameraOrigin);
 		drawBox2D(offset + cam*scale, 16, poly.isInside(cam) ? COLOR4(0, 255, 0, 255) : COLOR4(255, 32, 0, 255));
 	}
 

src/editor/Renderer.h

@@ -250,6 +250,7 @@ class Renderer {
 	void drawLine2D(vec2 start, vec2 end, COLOR4 color);
 	void drawBox(vec3 center, float width, COLOR4 color);
 	void drawBox(vec3 mins, vec3 maxs, COLOR4 color);
+	void drawPolygon3D(Polygon3D& poly, COLOR4 color);
 	float drawPolygon2D(Polygon3D poly, vec2 pos, vec2 maxSz, COLOR4 color); // returns render scale
 	void drawBox2D(vec2 center, float width, COLOR4 color);
 	void drawPlane(BSPPLANE& plane, COLOR4 color);