From c26086aff5ef186ea38e485363d6c32112225c0b Mon Sep 17 00:00:00 2001
From: David Markowitz <39972741+EmosewaMC@users.noreply.github.com>
Date: Sat, 12 Aug 2023 07:20:00 -0700
Subject: [PATCH] perf: Add path height correction on world load (#1178)

* Add path height correction on world load

* Increase height because Nexus Tower is tall

* Update height checker

- Only go up and down, do not deviate from the point you are on
- As a backup, use the nearestPoint on the nearestPoly, should detour be able to find one.
- Add a debug assert to fail the program should toReturn differ from nearestPoint[1].

Update dNavMesh.cpp

Update dNavMesh.cpp

* Fix if condition to actually return the value...

---------

Co-authored-by: Aaron Kimbrell <aronwk.aaron@gmail.com>
---
 dNavigation/dNavMesh.cpp    | 25 +++++++++++++++++++------
 dNavigation/dNavMesh.h      | 10 +++++++++-
 dZoneManager/CMakeLists.txt |  3 ++-
 dZoneManager/Zone.cpp       |  9 +++++++++
 4 files changed, 39 insertions(+), 8 deletions(-)

diff --git a/dNavigation/dNavMesh.cpp b/dNavigation/dNavMesh.cpp
index fdba4a2d..14b59055 100644
--- a/dNavigation/dNavMesh.cpp
+++ b/dNavigation/dNavMesh.cpp
@@ -10,6 +10,7 @@
 #include "BinaryPathFinder.h"
 
 #include "dZoneManager.h"
+#include "DluAssert.h"
 
 dNavMesh::dNavMesh(uint32_t zoneId) {
 	m_ZoneId = zoneId;
@@ -118,7 +119,7 @@ void dNavMesh::LoadNavmesh() {
 	m_NavMesh = mesh;
 }
 
-float dNavMesh::GetHeightAtPoint(const NiPoint3& location) {
+float dNavMesh::GetHeightAtPoint(const NiPoint3& location, const float halfExtentsHeight) const {
 	if (m_NavMesh == nullptr) {
 		return location.y;
 	}
@@ -130,15 +131,27 @@ float dNavMesh::GetHeightAtPoint(const NiPoint3& location) {
 	pos[2] = location.z;
 
 	dtPolyRef nearestRef = 0;
-	float polyPickExt[3] = { 32.0f, 32.0f, 32.0f };
+	float polyPickExt[3] = { 0.0f, halfExtentsHeight, 0.0f };
+	float nearestPoint[3] = { 0.0f, 0.0f, 0.0f };
 	dtQueryFilter filter{};
 
-	m_NavQuery->findNearestPoly(pos, polyPickExt, &filter, &nearestRef, 0);
+	auto hasPoly = m_NavQuery->findNearestPoly(pos, polyPickExt, &filter, &nearestRef, nearestPoint);
 	m_NavQuery->getPolyHeight(nearestRef, pos, &toReturn);
-
-	if (toReturn == 0.0f) {
-		toReturn = location.y;
+#ifdef _DEBUG
+	if (toReturn != 0.0f) {
+		DluAssert(toReturn == nearestPoint[1]);
 	}
+#endif
+	if (toReturn == 0.0f) {
+		// If we were unable to get the poly height, but the query returned a success, just use the height of the nearest point.
+		// This is what seems to happen anyways and it is better than returning zero.
+		if (hasPoly == DT_SUCCESS) {
+			toReturn = nearestPoint[1];
+		} else {
+			toReturn = location.y;
+		}
+	}
+	// If we failed to even find a poly, do not change the height since we have no idea what it should be.
 
 	return toReturn;
 }
diff --git a/dNavigation/dNavMesh.h b/dNavigation/dNavMesh.h
index dc3db854..600b8b86 100644
--- a/dNavigation/dNavMesh.h
+++ b/dNavigation/dNavMesh.h
@@ -15,7 +15,15 @@ public:
 	dNavMesh(uint32_t zoneId);
 	~dNavMesh();
 
-	float GetHeightAtPoint(const NiPoint3& location);
+	/**
+	 * Get the height at a point
+	 * 
+	 * @param location The location to check for height at. This is the center of the search area.
+	 * @param halfExtentsHeight The half extents height of the search area. This is the distance from the center to the top and bottom of the search area.
+	 * The larger the value of halfExtentsHeight is, the larger the performance cost of the query.
+	 * @return float The height at the point. If the point is not on the navmesh, the height of the point is returned.
+	 */
+	float GetHeightAtPoint(const NiPoint3& location, const float halfExtentsHeight = 32.0f) const;
 	std::vector<NiPoint3> GetPath(const NiPoint3& startPos, const NiPoint3& endPos, float speed = 10.0f);
 
 	class dtNavMesh* GetdtNavMesh() { return m_NavMesh; }
diff --git a/dZoneManager/CMakeLists.txt b/dZoneManager/CMakeLists.txt
index 1dd3841b..bbb22ba7 100644
--- a/dZoneManager/CMakeLists.txt
+++ b/dZoneManager/CMakeLists.txt
@@ -3,4 +3,5 @@ set(DZONEMANAGER_SOURCES "dZoneManager.cpp"
 	"Spawner.cpp"
 	"Zone.cpp")
 
-add_library(dZoneManager STATIC ${DZONEMANAGER_SOURCES}) 
+add_library(dZoneManager STATIC ${DZONEMANAGER_SOURCES})
+target_link_libraries(dZoneManager dPhysics) 
diff --git a/dZoneManager/Zone.cpp b/dZoneManager/Zone.cpp
index 99fbd6ff..07b3d72c 100644
--- a/dZoneManager/Zone.cpp
+++ b/dZoneManager/Zone.cpp
@@ -13,6 +13,7 @@
 #include "CDZoneTableTable.h"
 #include "Spawner.h"
 #include "dZoneManager.h"
+#include "dpWorld.h"
 
 #include "eTriggerCommandType.h"
 #include "eTriggerEventType.h"
@@ -554,6 +555,14 @@ void Zone::LoadPath(std::istream& file) {
 				if (ldfConfig) waypoint.config.push_back(ldfConfig);
 			}
 		}
+		// We verify the waypoint heights against the navmesh because in many movement paths,
+		// the waypoint is located near 0 height, 
+		if (path.pathType == PathType::Movement) {
+			if (dpWorld::Instance().IsLoaded()) {
+				// 2000 should be large enough for every world.
+				waypoint.position.y = dpWorld::Instance().GetNavMesh()->GetHeightAtPoint(waypoint.position, 2000.0f);
+			}
+		}
 		path.pathWaypoints.push_back(waypoint);
 	}
 	m_Paths.push_back(path);