From 3afee1f84158d1d6ac9245493098d6083b817a9e Mon Sep 17 00:00:00 2001
From: KimLS <kls@peqtgc.com>
Date: Sat, 5 Aug 2017 20:54:43 -0700
Subject: [PATCH] Work on making the paths work well when being z corrected esp
 for nav meshes

---
 zone/mob.cpp                 |  1 +
 zone/mob.h                   |  6 ++----
 zone/pathfinder_nav_mesh.cpp | 19 +++++++++++-----
 zone/pathing.cpp             | 42 ++++++++++++++++++++++++++++++++++--
 4 files changed, 57 insertions(+), 11 deletions(-)

diff --git a/zone/mob.cpp b/zone/mob.cpp
index 6b40615a3..c5f8c090e 100644
--- a/zone/mob.cpp
+++ b/zone/mob.cpp
@@ -446,6 +446,7 @@ Mob::Mob(const char* in_name,
 	npc_assist_cap = 0;
 
 	PathRecalcTimer.reset(new Timer(2000));
+	PathingLoopCount = 0;
 }
 
 Mob::~Mob()
diff --git a/zone/mob.h b/zone/mob.h
index ce09f2b88..f1607f6f7 100644
--- a/zone/mob.h
+++ b/zone/mob.h
@@ -1227,7 +1227,6 @@ protected:
 	void CalculateNewFearpoint();
 	float FindGroundZ(float new_x, float new_y, float z_offset=0.0);
 	glm::vec3 UpdatePath(float ToX, float ToY, float ToZ, float Speed, bool &WaypointChange, bool &NodeReached);
-	void PrintRoute();
 
 	virtual float GetSympatheticProcChances(uint16 spell_id, int16 ProcRateMod, int32 ItemProcRate = 0);
 	int16 GetSympatheticSpellProcRate(uint16 spell_id);
@@ -1373,8 +1372,6 @@ protected:
 	int32 GetItemFactionBonus(uint32 pFactionID);
 	void ClearItemFactionBonuses();
 
-	void CalculateFearPosition();
-
 	bool flee_mode;
 	Timer flee_timer;
 	Timer fix_z_timer;
@@ -1401,7 +1398,6 @@ protected:
 	int npc_assist_cap;
 	Timer assist_cap_timer; // clear assist cap so more nearby mobs can be called for help
 
-
 	int patrol;
 	glm::vec3 m_FearWalkTarget;
 	bool currently_fleeing;
@@ -1412,6 +1408,8 @@ protected:
 	IPathfinder::IPath Route;
 	std::unique_ptr<Timer> PathRecalcTimer;
 	bool DistractedFromGrid;
+	glm::vec3 PathingLastPosition;
+	int PathingLoopCount;
 
 	uint32 pDontHealMeBefore;
 	uint32 pDontBuffMeBefore;
diff --git a/zone/pathfinder_nav_mesh.cpp b/zone/pathfinder_nav_mesh.cpp
index 3e5c8f431..9ac5402d8 100644
--- a/zone/pathfinder_nav_mesh.cpp
+++ b/zone/pathfinder_nav_mesh.cpp
@@ -9,6 +9,8 @@
 #include "client.h"
 #include "../common/compression.h"
 
+extern Zone *zone;
+
 struct PathfinderNavmesh::Implementation
 {
 	dtNavMesh *nav_mesh;
@@ -47,12 +49,12 @@ IPathfinder::IPath PathfinderNavmesh::FindRoute(const glm::vec3 &start, const gl
 	dtQueryFilter filter;
 	filter.setIncludeFlags(65535U);
 	filter.setAreaCost(0, 1.0f); //Normal
-	filter.setAreaCost(1, 1.0f); //Water
-	filter.setAreaCost(2, 1.0f); //Lava
+	filter.setAreaCost(1, 2.0f); //Water
+	filter.setAreaCost(2, 2.0f); //Lava
 	filter.setAreaCost(4, 1.0f); //PvP
-	filter.setAreaCost(5, 1.0f); //Slime
-	filter.setAreaCost(6, 1.0f); //Ice
-	filter.setAreaCost(7, 1.0f); //V Water (Frigid Water)
+	filter.setAreaCost(5, 1.5f); //Slime
+	filter.setAreaCost(6, 1.5f); //Ice
+	filter.setAreaCost(7, 2.0f); //V Water (Frigid Water)
 	filter.setAreaCost(8, 1.0f); //General Area
 	filter.setAreaCost(9, 1.0f); //Portal
 
@@ -104,6 +106,13 @@ IPathfinder::IPath PathfinderNavmesh::FindRoute(const glm::vec3 &start, const gl
 				node.z = straight_path[i * 3 + 1];
 				node.y = straight_path[i * 3 + 2];
 
+				if (zone->HasMap()) {
+					auto best_z = zone->zonemap->FindBestZ(node, nullptr);
+					if (best_z != BEST_Z_INVALID) {
+						node.z = best_z;
+					}
+				}
+
 				unsigned short flag = 0;
 				if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &flag))) {
 					if (flag & 512) {
diff --git a/zone/pathing.cpp b/zone/pathing.cpp
index a5d439d94..1fc47b549 100644
--- a/zone/pathing.cpp
+++ b/zone/pathing.cpp
@@ -35,7 +35,7 @@ glm::vec3 Mob::UpdatePath(float ToX, float ToY, float ToZ, float Speed, bool &Wa
 	}
 	else {
 		if (PathRecalcTimer->Check()) {
-			bool SameDestination = DistanceSquared(To, PathingDestination) < 4.0f;
+			bool SameDestination = DistanceSquared(To, PathingDestination) < 100.0f;
 			if (!SameDestination) {
 				//We had a route but our target position moved too much
 				Route = zone->pathing->FindRoute(From, To);
@@ -46,7 +46,45 @@ glm::vec3 Mob::UpdatePath(float ToX, float ToY, float ToZ, float Speed, bool &Wa
 			}
 		}
 
-		bool AtNextNode = DistanceSquared(From, (*Route.begin()).pos) < 4.0f;
+		if (!IsRooted()) {
+			bool AtPrevNode = DistanceSquared(From, PathingLastPosition) < 1.0f;
+			if (AtPrevNode) {
+				PathingLoopCount++;
+
+				if (PathingLoopCount > 5) {
+					SendPosition();
+				}
+
+				auto front = (*Route.begin()).pos;
+				Teleport(front);
+				Route.pop_front();
+
+				WaypointChanged = true;
+				NodeReached = true;
+				PathingLoopCount = 0;
+
+				return front;
+			}
+			else {
+				PathingLastPosition = From;
+				PathingLoopCount = 0;
+			}
+		}
+		else {
+			PathingLastPosition = From;
+			PathingLoopCount = 0;
+		}
+
+		bool AtNextNode = false;
+		if (flymode == 1) {
+			AtNextNode = DistanceSquared(From, (*Route.begin()).pos) < 4.0f;
+		}
+		else {
+			float z_dist = From.z - (*Route.begin()).pos.z;
+			z_dist *= z_dist;
+			AtNextNode = DistanceSquaredNoZ(From, (*Route.begin()).pos) < 4.0f && z_dist < 25.0f;
+		}
+		
 		if (AtNextNode) {
 			WaypointChanged = false;
 			NodeReached = true;