diff --git a/common/ruletypes.h b/common/ruletypes.h
index c8b927b4f..623260af4 100644
--- a/common/ruletypes.h
+++ b/common/ruletypes.h
@@ -314,6 +314,7 @@ RULE_INT(Pathing, CullNodesFromStart, 1)		// Checks LOS from Start point to seco
 RULE_INT(Pathing, CullNodesFromEnd, 1)		// Checks LOS from End point to second to last node for this many nodes and removes last node if there is LOS
 RULE_REAL(Pathing, CandidateNodeRangeXY, 400)		// When searching for path start/end nodes, only nodes within this range will be considered.
 RULE_REAL(Pathing, CandidateNodeRangeZ, 10)		// When searching for path start/end nodes, only nodes within this range will be considered.
+RULE_REAL(Pathing, NavmeshStepSize, 30.0f)
 RULE_CATEGORY_END()
 
 RULE_CATEGORY(Watermap)
diff --git a/zone/CMakeLists.txt b/zone/CMakeLists.txt
index 103771f06..7eaafb9d7 100644
--- a/zone/CMakeLists.txt
+++ b/zone/CMakeLists.txt
@@ -94,7 +94,6 @@ SET(zone_sources
 	pathfinder_interface.cpp
 	pathfinder_nav_mesh.cpp
 	pathfinder_null.cpp
-	pathfinder_waypoint.cpp
 	pathing.cpp
 	perl_client.cpp
 	perl_doors.cpp
@@ -216,7 +215,6 @@ SET(zone_headers
 	pathfinder_interface.h
 	pathfinder_nav_mesh.h
 	pathfinder_null.h
-	pathfinder_waypoint.h
 	perlpacket.h
 	petitions.h
 	pets.h
diff --git a/zone/mob.cpp b/zone/mob.cpp
index 7bbf76d64..d38121b06 100644
--- a/zone/mob.cpp
+++ b/zone/mob.cpp
@@ -3441,7 +3441,7 @@ float Mob::FindDestGroundZ(glm::vec3 dest, float z_offset)
 	if (zone->zonemap != nullptr)
 	{
 		dest.z += z_offset;
-		best_z = zone->zonemap->FindClosestZ(dest, nullptr);
+		best_z = zone->zonemap->FindBestZ(dest, nullptr);
 	}
 	return best_z;
 }
diff --git a/zone/mob_movement_manager.cpp b/zone/mob_movement_manager.cpp
index ca1b9b14e..437468285 100644
--- a/zone/mob_movement_manager.cpp
+++ b/zone/mob_movement_manager.cpp
@@ -785,15 +785,21 @@ void MobMovementManager::UpdatePath(Mob *who, float x, float y, float z, MobMove
 
 void MobMovementManager::UpdatePathGround(Mob * who, float x, float y, float z, MobMovementMode mode)
 {
+	PathfinderOptions opts;
+	opts.smooth_path = true;
+	opts.step_size = RuleR(Pathing, NavmeshStepSize);
+	opts.offset = who->GetZOffset();
+	opts.flags = PathingNotDisabled ^ PathingZoneLine;
+
 	//This is probably pointless since the nav mesh tool currently sets zonelines to disabled anyway
 	auto partial = false;
 	auto stuck = false;
-	auto route = zone->pathing->FindRoute(
+	auto route = zone->pathing->FindPath(
 		glm::vec3(who->GetX(), who->GetY(), who->GetZ()),
 		glm::vec3(x, y, z),
 		partial,
 		stuck,
-		PathingNotDisabled ^ PathingZoneLine);
+		opts);
 
 	auto eiter = _impl->Entries.find(who);
 	auto &ent = (*eiter);
@@ -864,14 +870,20 @@ void MobMovementManager::UpdatePathUnderwater(Mob *who, float x, float y, float
 		return;
 	}
 
+	PathfinderOptions opts;
+	opts.smooth_path = true;
+	opts.step_size = RuleR(Pathing, NavmeshStepSize);
+	opts.offset = who->GetZOffset();
+	opts.flags = PathingNotDisabled ^ PathingZoneLine;
+
 	auto partial = false;
 	auto stuck = false;
-	auto route = zone->pathing->FindRoute(
+	auto route = zone->pathing->FindPath(
 		glm::vec3(who->GetX(), who->GetY(), who->GetZ()),
 		glm::vec3(x, y, z),
 		partial,
 		stuck,
-		PathingNotDisabled ^ PathingZoneLine);
+		opts);
 
 	if (route.size() == 0) {
 		HandleStuckBehavior(who, x, y, z, mode);
diff --git a/zone/pathfinder_interface.cpp b/zone/pathfinder_interface.cpp
index 983a85a01..68696b9d6 100644
--- a/zone/pathfinder_interface.cpp
+++ b/zone/pathfinder_interface.cpp
@@ -8,15 +8,10 @@
 
 IPathfinder *IPathfinder::Load(const std::string &zone) {
 	struct stat statbuffer;
-	std::string waypoint_path = fmt::format("maps/path/{0}.path", zone);
 	std::string navmesh_path = fmt::format("maps/nav/{0}.nav", zone);
 	if (stat(navmesh_path.c_str(), &statbuffer) == 0) {
 		return new PathfinderNavmesh(navmesh_path);
 	}
 	
-	if (stat(waypoint_path.c_str(), &statbuffer) == 0) {
-		return new PathfinderWaypoint(waypoint_path);
-	}
-
 	return new PathfinderNull();
 }
diff --git a/zone/pathfinder_interface.h b/zone/pathfinder_interface.h
index 92405691a..4984efe14 100644
--- a/zone/pathfinder_interface.h
+++ b/zone/pathfinder_interface.h
@@ -24,6 +24,32 @@ enum PathingPolyFlags
 	PathingNotDisabled = PathingAll ^ PathingDisabled
 };
 
+struct PathfinderOptions
+{
+	PathfinderOptions() {
+		flags = PathingNotDisabled;
+		smooth_path = true;
+		step_size = 10.0f;
+		flag_cost[0] = 1.0f;
+		flag_cost[1] = 3.0f;
+		flag_cost[2] = 5.0f;
+		flag_cost[3] = 1.0f;
+		flag_cost[4] = 2.0f;
+		flag_cost[5] = 2.0f;
+		flag_cost[6] = 4.0f;
+		flag_cost[7] = 1.0f;
+		flag_cost[8] = 0.1f;
+		flag_cost[9] = 0.1f;
+		offset = 3.25f;
+	}
+
+	int flags;
+	bool smooth_path;
+	float step_size;
+	float flag_cost[10];
+	float offset;
+};
+
 class IPathfinder
 {
 public:
@@ -48,6 +74,7 @@ public:
 	virtual ~IPathfinder() { }
 
 	virtual IPath FindRoute(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, int flags = PathingNotDisabled) = 0;
+	virtual IPath FindPath(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, const PathfinderOptions& opts) = 0;
 	virtual glm::vec3 GetRandomLocation(const glm::vec3 &start) = 0;
 	virtual void DebugCommand(Client *c, const Seperator *sep) = 0;
 
diff --git a/zone/pathfinder_nav_mesh.cpp b/zone/pathfinder_nav_mesh.cpp
index 5e3217191..d4879f6f7 100644
--- a/zone/pathfinder_nav_mesh.cpp
+++ b/zone/pathfinder_nav_mesh.cpp
@@ -1,9 +1,9 @@
 #include <memory>
 #include <stdio.h>
 #include <vector>
-#include <DetourNavMesh.h>
-#include <DetourNavMeshQuery.h>
 #include "pathfinder_nav_mesh.h"
+#include <DetourCommon.h>
+#include <DetourNavMeshQuery.h>
 
 #include "zone.h"
 #include "water_map.h"
@@ -131,6 +131,177 @@ IPathfinder::IPath PathfinderNavmesh::FindRoute(const glm::vec3 &start, const gl
 	return Route;
 }
 
+IPathfinder::IPath PathfinderNavmesh::FindPath(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, const PathfinderOptions &opts)
+{
+	partial = false;
+
+	if (!m_impl->nav_mesh) {
+		return IPath();
+	}
+
+	if (!m_impl->query) {
+		m_impl->query = dtAllocNavMeshQuery();
+	}
+
+	m_impl->query->init(m_impl->nav_mesh, MaxNavmeshNodes);
+	glm::vec3 current_location(start.x, start.z, start.y);
+	glm::vec3 dest_location(end.x, end.z, end.y);
+
+	dtQueryFilter filter;
+	filter.setIncludeFlags(opts.flags);
+	filter.setAreaCost(0, opts.flag_cost[0]); //Normal
+	filter.setAreaCost(1, opts.flag_cost[1]); //Water
+	filter.setAreaCost(2, opts.flag_cost[2]); //Lava
+	filter.setAreaCost(4, opts.flag_cost[3]); //PvP
+	filter.setAreaCost(5, opts.flag_cost[4]); //Slime
+	filter.setAreaCost(6, opts.flag_cost[5]); //Ice
+	filter.setAreaCost(7, opts.flag_cost[6]); //V Water (Frigid Water)
+	filter.setAreaCost(8, opts.flag_cost[7]); //General Area
+	filter.setAreaCost(9, opts.flag_cost[8]); //Portal
+	filter.setAreaCost(10, opts.flag_cost[9]); //Prefer
+
+	static const int max_polys = 256;
+	dtPolyRef start_ref;
+	dtPolyRef end_ref;
+	glm::vec3 ext(5.0f, 100.0f, 5.0f);
+
+	m_impl->query->findNearestPoly(&current_location[0], &ext[0], &filter, &start_ref, 0);
+	m_impl->query->findNearestPoly(&dest_location[0], &ext[0], &filter, &end_ref, 0);
+
+	if (!start_ref || !end_ref) {
+		return IPath();
+	}
+
+	int npoly = 0;
+	dtPolyRef path[max_polys] = { 0 };
+	m_impl->query->findPath(start_ref, end_ref, &current_location[0], &dest_location[0], &filter, path, &npoly, max_polys);
+
+	if (npoly) {
+		glm::vec3 epos = dest_location;
+		if (path[npoly - 1] != end_ref) {
+			m_impl->query->closestPointOnPoly(path[npoly - 1], &dest_location[0], &epos[0], 0);
+			partial = true;
+
+			auto dist = DistanceSquared(epos, current_location);
+			if (dist < 10000.0f) {
+				stuck = true;
+			}
+		}
+
+		int n_straight_polys;
+		glm::vec3 straight_path[max_polys];
+		unsigned char straight_path_flags[max_polys];
+		dtPolyRef straight_path_polys[max_polys];
+
+		auto status = m_impl->query->findStraightPath(&current_location[0], &epos[0], path, npoly,
+			(float*)&straight_path[0], straight_path_flags,
+			straight_path_polys, &n_straight_polys, 2048, DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS);
+
+		if (dtStatusFailed(status)) {
+			return IPath();
+		}
+
+		if (n_straight_polys) {
+			if (opts.smooth_path) {
+				IPath Route;
+
+				//Add the first point
+				{
+					auto &flag = straight_path_flags[0];
+					if (flag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
+						auto &p = straight_path[0];
+
+						Route.push_back(glm::vec3(p.x, p.z, p.y));
+					}
+					else {
+						auto &p = straight_path[0];
+
+						float h = 0.0f;
+						if (dtStatusSucceed(GetPolyHeightOnPath(path, npoly, p, &h))) {
+							p.y = h + opts.offset;
+						}
+
+						Route.push_back(glm::vec3(p.x, p.z, p.y));
+					}
+				}
+
+				for (int i = 0; i < n_straight_polys - 1; ++i)
+				{
+					auto &flag = straight_path_flags[i];
+
+					if (flag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
+						auto &poly = straight_path_polys[i];
+
+						auto &p2 = straight_path[i + 1];
+						glm::vec3 node(p2.x, p2.z, p2.y);
+						Route.push_back(node);
+
+						unsigned short pflag = 0;
+						if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &pflag))) {
+							if (pflag & 512) {
+								Route.push_back(true);
+							}
+						}
+					}
+					else {
+						auto &p1 = straight_path[i];
+						auto &p2 = straight_path[i + 1];
+						auto dist = glm::distance(p1, p2);
+						auto dir = glm::normalize(p2 - p1);
+						float total = 0.0f;
+						glm::vec3 previous_pt = p1;
+
+						while (total < dist) {
+							glm::vec3 current_pt;
+							float dist_to_move = opts.step_size;
+							float ff = opts.step_size / 2.0f;
+
+							if (total + dist_to_move + ff >= dist) {
+								current_pt = p2;
+								total = dist;
+							}
+							else {
+								total += dist_to_move;
+								current_pt = p1 + dir * total;
+							}
+
+							float h = 0.0f;
+							if (dtStatusSucceed(GetPolyHeightOnPath(path, npoly, current_pt, &h))) {
+								current_pt.y = h + opts.offset;
+							}
+
+							Route.push_back(glm::vec3(current_pt.x, current_pt.z, current_pt.y));
+							previous_pt = current_pt;
+						}
+					}
+				}
+
+				return Route;
+			}
+			else {
+				IPath Route;
+				for (int i = 0; i < n_straight_polys; ++i)
+				{
+					auto &current = straight_path[i];
+					glm::vec3 node(current.x, current.z, current.y);
+					Route.push_back(node);
+
+					unsigned short flag = 0;
+					if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &flag))) {
+						if (flag & 512) {
+							Route.push_back(true);
+						}
+					}
+				}
+
+				return Route;
+			}
+		}
+	}
+
+	return IPath();
+}
+
 glm::vec3 PathfinderNavmesh::GetRandomLocation(const glm::vec3 &start)
 {
 	if (start.x == 0.0f && start.y == 0.0)
@@ -310,24 +481,85 @@ void PathfinderNavmesh::Load(const std::string &path)
 
 void PathfinderNavmesh::ShowPath(Client * c, const glm::vec3 &start, const glm::vec3 &end)
 {
+	auto &list = entity_list.GetNPCList();
+
+	for (auto &iter : list) {
+		auto npc = iter.second;
+		auto name = npc->GetName();
+
+		if (strstr(name, "PathNode") != nullptr) {
+			npc->Depop();
+		}
+	}
+
+	PathfinderOptions opts;
+	opts.smooth_path = true;
+	opts.step_size = RuleR(Pathing, NavmeshStepSize);
 	bool partial = false;
 	bool stuck = false;
-	auto path = FindRoute(start, end, partial, stuck);
-	std::vector<FindPerson_Point> points;
+	auto path = FindPath(start, end, partial, stuck, opts);
 
-	if (!partial) {
-		FindPerson_Point p;
-		for (auto &node : path)
+	for (auto &node : path) {
+		if (!node.teleport) {
+			NPC::SpawnNPC("PathNode 2253 1 0 1 2 1", glm::vec4(node.pos, 1.0));
+		}
+	}
+}
+
+dtStatus PathfinderNavmesh::GetPolyHeightNoConnections(dtPolyRef ref, const float *pos, float *height) const
+{
+	auto *m_nav = m_impl->nav_mesh;
+
+	if (!m_nav) {
+		return DT_FAILURE;
+	}
+
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly))) {
+		return DT_FAILURE | DT_INVALID_PARAM;
+	}
+
+	if (poly->getType() != DT_POLYTYPE_OFFMESH_CONNECTION) {
+		const unsigned int ip = (unsigned int)(poly - tile->polys);
+		const dtPolyDetail* pd = &tile->detailMeshes[ip];
+		for (int j = 0; j < pd->triCount; ++j)
 		{
-			if (!node.teleport) {
-				p.x = node.pos.x;
-				p.y = node.pos.y;
-				p.z = node.pos.z;
-
-				points.push_back(p);
+			const unsigned char* t = &tile->detailTris[(pd->triBase + j) * 4];
+			const float* v[3];
+			for (int k = 0; k < 3; ++k)
+			{
+				if (t[k] < poly->vertCount)
+					v[k] = &tile->verts[poly->verts[t[k]] * 3];
+				else
+					v[k] = &tile->detailVerts[(pd->vertBase + (t[k] - poly->vertCount)) * 3];
+			}
+			float h;
+			if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
+			{
+				if (height)
+					*height = h;
+				return DT_SUCCESS;
 			}
 		}
 	}
 
-	c->SendPathPacket(points);
+	return DT_FAILURE | DT_INVALID_PARAM;
+}
+
+dtStatus PathfinderNavmesh::GetPolyHeightOnPath(const dtPolyRef *path, const int path_len, const glm::vec3 &pos, float *h) const
+{
+	if (!path || !path_len) {
+		return DT_FAILURE;
+	}
+
+	for (int i = 0; i < path_len; ++i) {
+		dtPolyRef ref = path[i];
+
+		if (dtStatusSucceed(GetPolyHeightNoConnections(ref, &pos[0], h))) {
+			return DT_SUCCESS;
+		}
+	}
+
+	return DT_FAILURE;
 }
diff --git a/zone/pathfinder_nav_mesh.h b/zone/pathfinder_nav_mesh.h
index 42dd28988..565a4a22a 100644
--- a/zone/pathfinder_nav_mesh.h
+++ b/zone/pathfinder_nav_mesh.h
@@ -2,6 +2,7 @@
 
 #include "pathfinder_interface.h"
 #include <string>
+#include <DetourNavMesh.h>
 
 class PathfinderNavmesh : public IPathfinder
 {
@@ -10,6 +11,7 @@ public:
 	virtual ~PathfinderNavmesh();
 
 	virtual IPath FindRoute(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, int flags = PathingNotDisabled);
+	virtual IPath FindPath(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, const PathfinderOptions& opts);
 	virtual glm::vec3 GetRandomLocation(const glm::vec3 &start);
 	virtual void DebugCommand(Client *c, const Seperator *sep);
 
@@ -17,6 +19,8 @@ private:
 	void Clear();
 	void Load(const std::string &path);
 	void ShowPath(Client *c, const glm::vec3 &start, const glm::vec3 &end);
+	dtStatus GetPolyHeightNoConnections(dtPolyRef ref, const float *pos, float *height) const;
+	dtStatus GetPolyHeightOnPath(const dtPolyRef *path, const int path_len, const glm::vec3 &pos, float *h) const;
 
 	struct Implementation;
 	std::unique_ptr<Implementation> m_impl;
diff --git a/zone/pathfinder_null.cpp b/zone/pathfinder_null.cpp
index 400b98be4..35d0a131e 100644
--- a/zone/pathfinder_null.cpp
+++ b/zone/pathfinder_null.cpp
@@ -10,7 +10,17 @@ IPathfinder::IPath PathfinderNull::FindRoute(const glm::vec3 &start, const glm::
 	return ret;
 }
 
+IPathfinder::IPath PathfinderNull::FindPath(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, const PathfinderOptions &opts)
+{
+	partial = false;
+	stuck = false;
+	IPath ret;
+	ret.push_back(start);
+	ret.push_back(end);
+	return ret;
+}
+
 glm::vec3 PathfinderNull::GetRandomLocation(const glm::vec3 &start)
 {
-	return glm::vec3();
+	return glm::vec3(0.0f);
 }
diff --git a/zone/pathfinder_null.h b/zone/pathfinder_null.h
index e5da24146..8c750e92a 100644
--- a/zone/pathfinder_null.h
+++ b/zone/pathfinder_null.h
@@ -9,6 +9,7 @@ public:
 	virtual ~PathfinderNull() { }
 
 	virtual IPath FindRoute(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, int flags = PathingNotDisabled);
+	virtual IPath FindPath(const glm::vec3 &start, const glm::vec3 &end, bool &partial, bool &stuck, const PathfinderOptions& opts);
 	virtual glm::vec3 GetRandomLocation(const glm::vec3 &start);
 	virtual void DebugCommand(Client *c, const Seperator *sep) { }
 };