diff --git a/zone/pathfinder_nav_mesh.cpp b/zone/pathfinder_nav_mesh.cpp
index d20b595bc..3e5c8f431 100644
--- a/zone/pathfinder_nav_mesh.cpp
+++ b/zone/pathfinder_nav_mesh.cpp
@@ -20,100 +20,107 @@ PathfinderNavmesh::PathfinderNavmesh(const std::string &path)
 	m_impl.reset(new Implementation());
 	m_impl->nav_mesh = nullptr;
 	m_impl->query = nullptr;
+	Load(path);
 }
 
 PathfinderNavmesh::~PathfinderNavmesh()
 {
-	if (m_impl->nav_mesh) {
-		dtFreeNavMesh(m_impl->nav_mesh);
-	}
-
-	if (m_impl->query) {
-		dtFreeNavMeshQuery(m_impl->query);
-	}
+	Clear();
 }
 
 IPathfinder::IPath PathfinderNavmesh::FindRoute(const glm::vec3 &start, const glm::vec3 &end)
 {
-	if (!m_impl->nav_mesh || !m_impl->query) {
+	if (!m_impl->nav_mesh) {
 		IPath Route;
-		Route.push_back(start);
 		Route.push_back(end);
 		return Route;
 	}
 
-	glm::vec3 ext(15.0f, 100.0f, 15.0f);
+	if (!m_impl->query) {
+		m_impl->query = dtAllocNavMeshQuery();
+		m_impl->query->init(m_impl->nav_mesh, 32768);
+	}
+
+	glm::vec3 current_location(start.x, start.z, start.y);
+	glm::vec3 dest_location(end.x, end.z, end.y);
+
 	dtQueryFilter filter;
 	filter.setIncludeFlags(65535U);
 	filter.setAreaCost(0, 1.0f); //Normal
-	filter.setAreaCost(0, 1.0f); //Water
-	filter.setAreaCost(0, 1.0f); //Lava
-	filter.setAreaCost(0, 1.0f); //PvP
-	filter.setAreaCost(0, 1.0f); //Slime
-	filter.setAreaCost(0, 1.0f); //Ice
-	filter.setAreaCost(0, 1.0f); //V Water (Frigid Water)
-	filter.setAreaCost(0, 1.0f); //General Area
-	filter.setAreaCost(0, 1.0f); //Portal
+	filter.setAreaCost(1, 1.0f); //Water
+	filter.setAreaCost(2, 1.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(8, 1.0f); //General Area
+	filter.setAreaCost(9, 1.0f); //Portal
 
 	dtPolyRef start_ref;
 	dtPolyRef end_ref;
+	glm::vec3 ext(15.0f, 15.0f, 15.0f);
 
-	m_impl->query->findNearestPoly(&start[0], &ext[0], &filter, &start_ref, 0);
-	m_impl->query->findNearestPoly(&end[0], &ext[0], &filter, &end_ref, 0);
+	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) {
 		IPath Route;
-		Route.push_back(start);
 		Route.push_back(end);
 		return Route;
 	}
 
 	int npoly = 0;
-	dtPolyRef path[1024] = { 0 };
-	m_impl->query->findPath(start_ref, end_ref, &start[0], &end[0], &filter, path, &npoly, 1024);
+	dtPolyRef path[512] = { 0 };
+	auto status = m_impl->query->findPath(start_ref, end_ref, &current_location[0], &dest_location[0], &filter, path, &npoly, 512);
 
 	if (npoly) {
-		glm::vec3 epos = end;
-		if (path[npoly - 1] != end_ref)
-			m_impl->query->closestPointOnPoly(path[npoly - 1], &end[0], &epos[0], 0);
+		glm::vec3 epos = dest_location;
+		if (path[npoly - 1] != end_ref) {
+			m_impl->query->closestPointOnPoly(path[npoly - 1], &dest_location[0], &epos[0], 0);
+		}
+
+		float straight_path[512 * 3];
+		unsigned char straight_path_flags[512];
 
-		float straight_path[2048 * 3];
-		unsigned char straight_path_flags[2048];
 		int n_straight_polys;
-		dtPolyRef straight_path_polys[2048];
-		m_impl->query->findStraightPath(&start[0], &epos[0], path, npoly,
+		dtPolyRef straight_path_polys[512];
+
+		status = m_impl->query->findStraightPath(&current_location[0], &epos[0], path, npoly,
 			straight_path, straight_path_flags,
-			straight_path_polys, &n_straight_polys, 2048, DT_STRAIGHTPATH_ALL_CROSSINGS);
+			straight_path_polys, &n_straight_polys, 512, DT_STRAIGHTPATH_ALL_CROSSINGS);
+
+		if (status & DT_OUT_OF_NODES) {
+			IPath Route;
+			Route.push_back(end);
+			return Route;
+		}
 
 		if (n_straight_polys) {
 			IPath Route;
+			for (int i = 0; i < n_straight_polys; ++i)
+			{
+				glm::vec3 node;
+				node.x = straight_path[i * 3];
+				node.z = straight_path[i * 3 + 1];
+				node.y = straight_path[i * 3 + 2];
 
-			for (int i = 0; i < n_straight_polys - 1; ++i) {
-				glm::vec3 color(1.0f, 1.0f, 0.0f);
 				unsigned short flag = 0;
 				if (dtStatusSucceed(m_impl->nav_mesh->getPolyFlags(straight_path_polys[i], &flag))) {
-					if (flag & 512) { //Portal
+					if (flag & 512) {
 						Route.push_back(true);
 					}
 				}
 
-				Route.push_back(glm::vec3(straight_path[i * 3], straight_path[i * 3 + 1] + 0.4f, straight_path[i * 3 + 2]));
+				Route.push_back(node);
 			}
 
 			return Route;
 		}
+	}
 
-		IPath Route;
-		Route.push_back(start);
-		Route.push_back(end);
-		return Route;
-	}
-	else {
-		IPath Route;
-		Route.push_back(start);
-		Route.push_back(end);
-		return Route;
-	}
+	IPath Route;
+	Route.push_back(end);
+	return Route;
 }
 
 glm::vec3 PathfinderNavmesh::GetRandomLocation()
@@ -143,17 +150,20 @@ void PathfinderNavmesh::DebugCommand(Client *c, const Seperator *sep)
 	}
 }
 
-void PathfinderNavmesh::Load(const std::string &path)
+void PathfinderNavmesh::Clear()
 {
 	if (m_impl->nav_mesh) {
 		dtFreeNavMesh(m_impl->nav_mesh);
-		m_impl->nav_mesh = nullptr;
 	}
 
 	if (m_impl->query) {
 		dtFreeNavMeshQuery(m_impl->query);
-		m_impl->query = nullptr;
 	}
+}
+
+void PathfinderNavmesh::Load(const std::string &path)
+{
+	Clear();
 
 	FILE *f = fopen(path.c_str(), "rb");
 	if (f) {
@@ -175,7 +185,7 @@ void PathfinderNavmesh::Load(const std::string &path)
 			return;
 		}
 
-		if (version != 2U) {
+		if (version != 2) {
 			fclose(f);
 			return;
 		}
@@ -241,9 +251,6 @@ void PathfinderNavmesh::Load(const std::string &path)
 
 			m_impl->nav_mesh->addTile(data, data_size, DT_TILE_FREE_DATA, tile_ref, 0);
 		}
-
-		m_impl->query = dtAllocNavMeshQuery();
-		m_impl->query->init(m_impl->nav_mesh, 32768);
 	}
 }
 
diff --git a/zone/pathfinder_nav_mesh.h b/zone/pathfinder_nav_mesh.h
index 6229cd791..a15f02d6b 100644
--- a/zone/pathfinder_nav_mesh.h
+++ b/zone/pathfinder_nav_mesh.h
@@ -14,6 +14,7 @@ public:
 	virtual void DebugCommand(Client *c, const Seperator *sep);
 
 private:
+	void Clear();
 	void Load(const std::string &path);
 	void ShowPath(Client *c, const glm::vec3 &start, const glm::vec3 &end);