Working on navigation system

src/Game.cpp

@@ -473,6 +473,12 @@ namespace ZL
 						onPointerUp(ZL::UiManager::MOUSE_FINGER_ID, eventX, eventY, mx, my);
 					}
 				}
+				else if (event.button.button == SDL_BUTTON_RIGHT
+				         && event.type == SDL_MOUSEBUTTONUP
+				         && navigationEditorMode
+				         && currentLocation) {
+					currentLocation->navigationEditorHandleRightClick();
+				}
 			}
 			else if (event.type == SDL_MOUSEMOTION) {
 				int eventX = event.motion.x;
@@ -514,6 +520,17 @@ namespace ZL
 					currentLocation->dialogueSystem.startDialogue("test_cutscene_pan_dialogue");
 					break;
 
+				case SDLK_n:
+					navigationEditorMode = !navigationEditorMode;
+					if (currentLocation) {
+						currentLocation->navigationEditorMode = navigationEditorMode;
+						if (navigationEditorMode) {
+							currentLocation->navigationEditorBuildNavMeshes();
+						}
+					}
+					std::cout << "[NAV_EDITOR] Mode: " << (navigationEditorMode ? "ON" : "OFF") << std::endl;
+					break;
+
 				case SDLK_o:
 					//y = y + 0.002;
 					//currentLocation->player->hp = 200;
@@ -543,6 +560,12 @@ namespace ZL
 					activateSlowMoEffect();
 					break;
 
+				case SDLK_b:
+					if (navigationEditorMode && currentLocation) {
+						currentLocation->navigationEditorSave();
+					}
+					break;
+
 				case SDLK_j:
 					menuManager.toggleQuestJournal();
 					break;

src/Game.h

@@ -45,6 +45,8 @@ namespace ZL {
 		std::unordered_map<std::string, std::shared_ptr<Location>> locations;
 		std::shared_ptr<Location> currentLocation;
 
+		bool navigationEditorMode = false;
+
 		Inventory inventory;
 		InteractiveObject* pickedUpObject = nullptr;
 

src/Location.cpp

@@ -7,12 +7,14 @@
 #include <random>
 #include <cmath>
 #include <algorithm>
+#include <filesystem>
 #include <functional>
 #include <memory>
 #include <cfloat>
 #include "GameConstants.h"
 #include "Character.h"
 #include "external/nlohmann/json.hpp"
+#include <SDL.h>
 
 
 namespace ZL
@@ -163,6 +165,8 @@ namespace ZL
 			false
 		});*/
 
+		navigationEditorConfigPath = params.navigationJsonPath;
+
 		scriptEngine.init(this, &inventory, params.scriptPath);
 
 
@@ -233,9 +237,9 @@ namespace ZL
 		// NPC + player are handled as dynamic obstacles, so they are intentionally NOT in JSON.
 		navigation.build({}, navigationJsonPath, CONST_ZIP_FILE);
 
-#ifdef SHOW_PATH
-		buildDebugNavMeshes();
-#endif
+		if (navigationEditorMode) {
+			navigationEditorBuildNavMeshes();
+		}
 
 		static constexpr float kDynamicObstacleInfluenceDist = 6.0f;
 
@@ -278,34 +282,145 @@ namespace ZL
 		}
 	}
 
-#ifdef SHOW_PATH
-	void Location::buildDebugNavMeshes()
+	void Location::navigationEditorBuildNavMeshes()
 	{
-		debugNavMeshes.clear();
-		const auto& areas = navigation.getAreas();
-		float y = navigation.getFloorY() + 0.02f;
-		Eigen::Vector3f red(1.0f, 0.0f, 0.0f);
-
-		for (const auto& area : areas) {
-			if (area.polygon.size() < 3) continue;
+		navigationEditorNavMeshes.clear();
+		const float y = navigation.getFloorY() + 0.02f;
+		const Eigen::Vector3f red(1.0f, 0.0f, 0.0f);
+		for (const auto& obs : navigation.getObstaclePolygons()) {
+			if (obs.polygon.size() < 3) continue;
 			VertexRenderStruct mesh;
-			mesh.data = CreatePolygonFloor(area.polygon, y, red);
+			mesh.data = CreatePolygonFloor(obs.polygon, y, red);
 			mesh.RefreshVBO();
-			debugNavMeshes.push_back(std::move(mesh));
+			navigationEditorNavMeshes.push_back(std::move(mesh));
 		}
 	}
 
-	void Location::drawDebugNavigation()
+	void Location::navigationEditorDrawNavigation()
 	{
 		renderer.shaderManager.PushShader("defaultColor");
 		renderer.SetMatrix();
-		for (const auto& mesh : debugNavMeshes) {
+		for (const auto& mesh : navigationEditorNavMeshes) {
 			renderer.DrawVertexRenderStruct(mesh);
 		}
 		renderer.shaderManager.PopShader();
 		renderer.SetMatrix();
 	}
-#endif
+
+	void Location::navigationEditorRebuildPointsMesh()
+	{
+		VertexDataStruct data;
+		const Eigen::Vector3f yellow(1.0f, 1.0f, 0.0f);
+		const float y = navigation.getFloorY() + 0.05f;
+		const float s = 0.2f;
+
+		for (const auto& pt : navigationEditorPoints) {
+			// Small upward-pointing equilateral triangle in the XZ plane
+			Eigen::Vector3f v0(pt.x(),          y, pt.z() - s * 1.15f);
+			Eigen::Vector3f v1(pt.x() - s,      y, pt.z() + s * 0.58f);
+			Eigen::Vector3f v2(pt.x() + s,      y, pt.z() + s * 0.58f);
+
+			data.PositionData.push_back(v0);
+			data.PositionData.push_back(v1);
+			data.PositionData.push_back(v2);
+			data.ColorData.push_back(yellow);
+			data.ColorData.push_back(yellow);
+			data.ColorData.push_back(yellow);
+		}
+
+		navigationEditorPointsMesh.data = std::move(data);
+		navigationEditorPointsMesh.RefreshVBO();
+	}
+
+	void Location::navigationEditorDrawPoints()
+	{
+		if (navigationEditorPoints.empty()) return;
+		renderer.shaderManager.PushShader("defaultColor");
+		renderer.SetMatrix();
+		renderer.DrawVertexRenderStruct(navigationEditorPointsMesh);
+		renderer.shaderManager.PopShader();
+		renderer.SetMatrix();
+	}
+
+	void Location::navigationEditorHandleLeftClick(const Eigen::Vector3f& hit, bool ctrlHeld)
+	{
+		if (ctrlHeld) {
+			const float removeRadius = 1.0f;
+			for (auto it = navigationEditorPoints.begin(); it != navigationEditorPoints.end(); ++it) {
+				const float dx = it->x() - hit.x();
+				const float dz = it->z() - hit.z();
+				if (dx * dx + dz * dz <= removeRadius * removeRadius) {
+					navigationEditorPoints.erase(it);
+					navigationEditorRebuildPointsMesh();
+					std::cout << "[NAV_EDITOR] Removed point, " << navigationEditorPoints.size() << " remaining\n";
+					return;
+				}
+			}
+			std::cout << "[NAV_EDITOR] No point found within " << removeRadius << " units of click\n";
+		} else {
+			navigationEditorPoints.push_back(hit);
+			navigationEditorRebuildPointsMesh();
+			std::cout << "[NAV_EDITOR] Added point (" << hit.x() << ", " << hit.z()
+			          << "), total: " << navigationEditorPoints.size() << "\n";
+		}
+	}
+
+	void Location::navigationEditorHandleRightClick()
+	{
+		if (navigationEditorPoints.size() < 3) {
+			std::cout << "[NAV_EDITOR] Need at least 3 points to form a polygon (have "
+			          << navigationEditorPoints.size() << "); clearing\n";
+			navigationEditorPoints.clear();
+			navigationEditorRebuildPointsMesh();
+			return;
+		}
+
+		PathFinder::ObstaclePolygon poly;
+		poly.name = "editor_obstacle_" + std::to_string(++navigationEditorObstacleCounter);
+		for (const auto& pt : navigationEditorPoints) {
+			poly.polygon.emplace_back(pt.x(), pt.z());
+		}
+
+		navigation.addObstaclePolygon(poly);
+		std::cout << "[NAV_EDITOR] Added obstacle '" << poly.name << "' with "
+		          << poly.polygon.size() << " vertices\n";
+
+		// Add a red mesh for the new obstacle polygon so it's visible immediately.
+		{
+			const float y = navigation.getFloorY() + 0.02f;
+			const Eigen::Vector3f red(1.0f, 0.0f, 0.0f);
+			VertexRenderStruct mesh;
+			mesh.data = CreatePolygonFloor(poly.polygon, y, red);
+			mesh.RefreshVBO();
+			navigationEditorNavMeshes.push_back(std::move(mesh));
+		}
+
+		navigationEditorPoints.clear();
+		navigationEditorRebuildPointsMesh();
+	}
+
+	void Location::navigationEditorSave()
+	{
+		std::string filename;
+		for (int i = 1; ; ++i) {
+			char buf[32];
+			snprintf(buf, sizeof(buf), "saved_mesh%03d.json", i);
+			filename = buf;
+			if (!std::filesystem::exists(filename)) break;
+		}
+
+		if (navigation.saveConfig(filename)) {
+			std::cout << "[NAV_EDITOR] Saved navigation config to: " << filename << "\n";
+		} else {
+			std::cerr << "[NAV_EDITOR] Failed to save to: " << filename << "\n";
+		}
+	}
+
+	void Location::navigationEditorReload()
+	{
+		setupNavigation(navigationEditorConfigPath);
+		std::cout << "[NAV_EDITOR] Reloaded navigation from: " << navigationEditorConfigPath << "\n";
+	}
 
 
 	InteractiveObject* Location::raycastInteractiveObjects(const Eigen::Vector3f& rayOrigin, const Eigen::Vector3f& rayDir) {
@@ -483,9 +598,10 @@ namespace ZL
 
 		for (auto& tz : teleportZones) tz.draw(renderer, Environment::zoom, Environment::width, Environment::height);
 
-#ifdef SHOW_PATH
-		drawDebugNavigation();
-#endif
+		if (navigationEditorMode) {
+			navigationEditorDrawNavigation();
+			navigationEditorDrawPoints();
+		}
 
 		renderer.PopMatrix();
 
@@ -677,6 +793,12 @@ namespace ZL
 
 		for (auto& tz : teleportZones) tz.draw(renderer, Environment::zoom, Environment::width, Environment::height);
 #endif
+
+		if (navigationEditorMode) {
+			navigationEditorDrawNavigation();
+			navigationEditorDrawPoints();
+		}
+
 		CheckGlError(__FILE__, __LINE__);
 
 		renderer.PopMatrix();
@@ -954,6 +1076,16 @@ namespace ZL
 
 		Eigen::Vector3f rayDir = (camForward + camRight * (ndcX * aspect * tanHalfFov) + camUp * (ndcY * tanHalfFov)).normalized();
 
+		if (navigationEditorMode) {
+			if (rayDir.y() < -0.001f) {
+				const float t = -camPos.y() / rayDir.y();
+				const Eigen::Vector3f hit = camPos + rayDir * t;
+				const bool ctrlHeld = (SDL_GetModState() & KMOD_CTRL) != 0;
+				navigationEditorHandleLeftClick(hit, ctrlHeld);
+			}
+			return;
+		}
+
 		std::cout << "[CLICK] Camera position: (" << camPos.x() << ", " << camPos.y() << ", " << camPos.z() << ")" << std::endl;
 		std::cout << "[CLICK] Ray direction: (" << rayDir.x() << ", " << rayDir.y() << ", " << rayDir.z() << ")" << std::endl;
 

src/Location.h

@@ -73,11 +73,24 @@ namespace ZL
 		TeleportZone* targetTeleportZone = nullptr;
 		std::function<void(const std::string&, const Eigen::Vector3f&, float)> onTeleport;
 
-#ifdef SHOW_PATH
-		std::vector<VertexRenderStruct> debugNavMeshes;
-		void buildDebugNavMeshes();
-		void drawDebugNavigation();
-#endif
+		// Navigation editor — toggle with 'N', save with 'B', right-click to finalize polygon
+		bool navigationEditorMode = false;
+
+		std::vector<Eigen::Vector3f> navigationEditorPoints;
+		VertexRenderStruct            navigationEditorPointsMesh;
+		std::vector<VertexRenderStruct> navigationEditorNavMeshes;
+		std::string navigationEditorConfigPath;
+		int         navigationEditorObstacleCounter = 0;
+
+		void navigationEditorBuildNavMeshes();
+		void navigationEditorDrawNavigation();
+		void navigationEditorRebuildPointsMesh();
+		void navigationEditorDrawPoints();
+		void navigationEditorHandleLeftClick(const Eigen::Vector3f& hit, bool ctrlHeld);
+		void navigationEditorHandleRightClick();
+		void navigationEditorSave();
+		void navigationEditorReload();
+
 		// Set by Game when the user's primary pointer (left mouse / single touch)
 		// has crossed the tap-vs-drag threshold and is now rotating the camera.
 		bool cameraDragging = false;

src/navigation/PathFinder.cpp

@@ -3,6 +3,7 @@
 #include "utils/Utils.h"
 #include <algorithm>
 #include <cmath>
+#include <fstream>
 #include <iostream>
 #include <limits>
 #include <queue>
@@ -409,6 +410,58 @@ bool PathFinder::isWalkable(const Eigen::Vector3f& point) const
     return worldToCell(point, cell) && isCellWalkable(cell, walkable);
 }
 
+void PathFinder::addObstaclePolygon(const ObstaclePolygon& polygon)
+{
+    obstaclePolygons.push_back(polygon);
+    if (ready) {
+        rebuildWalkableGrid();
+    }
+}
+
+bool PathFinder::saveConfig(const std::string& path) const
+{
+    json root;
+    root["cellSize"]        = cellSize;
+    root["agentRadius"]     = agentRadius;
+    root["floorY"]          = floorY;
+    root["objectPadding"]   = objectPadding;
+    root["boundaryPadding"] = boundaryPadding;
+
+    json areasArr = json::array();
+    for (const auto& area : areas) {
+        json areaObj;
+        areaObj["name"]      = area.name;
+        areaObj["available"] = area.available;
+        json poly = json::array();
+        for (const auto& pt : area.polygon) {
+            poly.push_back(json::array({ pt.x(), pt.y() }));
+        }
+        areaObj["polygon"] = poly;
+        areasArr.push_back(areaObj);
+    }
+    root["areas"] = areasArr;
+
+    json obstArr = json::array();
+    for (const auto& obs : obstaclePolygons) {
+        json obsObj;
+        obsObj["name"] = obs.name;
+        json poly = json::array();
+        for (const auto& pt : obs.polygon) {
+            poly.push_back(json::array({ pt.x(), pt.y() }));
+        }
+        obsObj["polygon"] = poly;
+        obstArr.push_back(obsObj);
+    }
+    root["obstacles"] = obstArr;
+
+    std::ofstream f(path);
+    if (!f.is_open()) {
+        return false;
+    }
+    f << root.dump(2);
+    return f.good();
+}
+
 void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath)
 {
     cellSize = 0.4f;

src/navigation/PathFinder.h

@@ -50,8 +50,12 @@ public:
     bool isReady() const { return ready; }
     bool isWalkable(const Eigen::Vector3f& point) const;
     const std::vector<NavigationArea>& getAreas() const { return areas; }
+    const std::vector<ObstaclePolygon>& getObstaclePolygons() const { return obstaclePolygons; }
     float getFloorY() const { return floorY; }
 
+    void addObstaclePolygon(const ObstaclePolygon& polygon);
+    bool saveConfig(const std::string& path) const;
+
 private:
 
     float cellSize = 0.4f;