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

@@ -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;
 }

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; }

dZoneManager/CMakeLists.txt

@@ -4,3 +4,4 @@ set(DZONEMANAGER_SOURCES "dZoneManager.cpp"
 	"Zone.cpp")
 
 add_library(dZoneManager STATIC ${DZONEMANAGER_SOURCES})
+target_link_libraries(dZoneManager dPhysics) 

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);